2a6fb2867e
Made all STL usages explicit to be able automatically find all usages of particular class or function.
637 lines
22 KiB
C++
637 lines
22 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
//
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
// If you do not agree to this license, do not download, install,
|
|
// copy or use the software.
|
|
//
|
|
//
|
|
// License Agreement
|
|
// For Open Source Computer Vision Library
|
|
//
|
|
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
|
|
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
|
|
// Third party copyrights are property of their respective owners.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
// are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistribution's of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
//
|
|
// * Redistribution's in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
//
|
|
// * The name of the copyright holders may not be used to endorse or promote products
|
|
// derived from this software without specific prior written permission.
|
|
//
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
//
|
|
//M*/
|
|
|
|
#include "precomp.hpp"
|
|
|
|
#define SHOW_DEBUG_IMAGES 0
|
|
|
|
#include "opencv2/core/core.hpp"
|
|
#include "opencv2/calib3d/calib3d.hpp"
|
|
|
|
#if SHOW_DEBUG_IMAGES
|
|
# include "opencv2/highgui/highgui.hpp"
|
|
#endif
|
|
|
|
#include <iostream>
|
|
#include <limits>
|
|
|
|
#include "opencv2/core/internal.hpp"
|
|
#if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
|
|
# ifdef ANDROID
|
|
template <typename Scalar> Scalar log2(Scalar v) { return std::log(v)/std::log(Scalar(2)); }
|
|
# endif
|
|
# if defined __GNUC__ && defined __APPLE__
|
|
# pragma GCC diagnostic ignored "-Wshadow"
|
|
# endif
|
|
# include <unsupported/Eigen/MatrixFunctions>
|
|
# include <Eigen/Dense>
|
|
#endif
|
|
|
|
using namespace cv;
|
|
|
|
inline static
|
|
void computeC_RigidBodyMotion( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
|
|
{
|
|
double invz = 1. / p3d.z,
|
|
v0 = dIdx * fx * invz,
|
|
v1 = dIdy * fy * invz,
|
|
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
|
|
|
|
C[0] = -p3d.z * v1 + p3d.y * v2;
|
|
C[1] = p3d.z * v0 - p3d.x * v2;
|
|
C[2] = -p3d.y * v0 + p3d.x * v1;
|
|
C[3] = v0;
|
|
C[4] = v1;
|
|
C[5] = v2;
|
|
}
|
|
|
|
inline static
|
|
void computeC_Rotation( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
|
|
{
|
|
double invz = 1. / p3d.z,
|
|
v0 = dIdx * fx * invz,
|
|
v1 = dIdy * fy * invz,
|
|
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
|
|
|
|
C[0] = -p3d.z * v1 + p3d.y * v2;
|
|
C[1] = p3d.z * v0 - p3d.x * v2;
|
|
C[2] = -p3d.y * v0 + p3d.x * v1;
|
|
}
|
|
|
|
inline static
|
|
void computeC_Translation( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
|
|
{
|
|
double invz = 1. / p3d.z,
|
|
v0 = dIdx * fx * invz,
|
|
v1 = dIdy * fy * invz,
|
|
v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
|
|
|
|
C[0] = v0;
|
|
C[1] = v1;
|
|
C[2] = v2;
|
|
}
|
|
|
|
inline static
|
|
void computeProjectiveMatrix( const Mat& ksi, Mat& Rt )
|
|
{
|
|
CV_Assert( ksi.size() == Size(1,6) && ksi.type() == CV_64FC1 );
|
|
|
|
#if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
|
|
const double* ksi_ptr = reinterpret_cast<const double*>(ksi.ptr(0));
|
|
Eigen::Matrix<double,4,4> twist, g;
|
|
twist << 0., -ksi_ptr[2], ksi_ptr[1], ksi_ptr[3],
|
|
ksi_ptr[2], 0., -ksi_ptr[0], ksi_ptr[4],
|
|
-ksi_ptr[1], ksi_ptr[0], 0, ksi_ptr[5],
|
|
0., 0., 0., 0.;
|
|
g = twist.exp();
|
|
|
|
|
|
eigen2cv(g, Rt);
|
|
#else
|
|
// for infinitesimal transformation
|
|
Rt = Mat::eye(4, 4, CV_64FC1);
|
|
|
|
Mat R = Rt(Rect(0,0,3,3));
|
|
Mat rvec = ksi.rowRange(0,3);
|
|
|
|
Rodrigues( rvec, R );
|
|
|
|
Rt.at<double>(0,3) = ksi.at<double>(3);
|
|
Rt.at<double>(1,3) = ksi.at<double>(4);
|
|
Rt.at<double>(2,3) = ksi.at<double>(5);
|
|
#endif
|
|
}
|
|
|
|
static
|
|
void cvtDepth2Cloud( const Mat& depth, Mat& cloud, const Mat& cameraMatrix )
|
|
{
|
|
CV_Assert( cameraMatrix.type() == CV_64FC1 );
|
|
const double inv_fx = 1.f/cameraMatrix.at<double>(0,0);
|
|
const double inv_fy = 1.f/cameraMatrix.at<double>(1,1);
|
|
const double ox = cameraMatrix.at<double>(0,2);
|
|
const double oy = cameraMatrix.at<double>(1,2);
|
|
cloud.create( depth.size(), CV_32FC3 );
|
|
for( int y = 0; y < cloud.rows; y++ )
|
|
{
|
|
Point3f* cloud_ptr = reinterpret_cast<Point3f*>(cloud.ptr(y));
|
|
const float* depth_prt = reinterpret_cast<const float*>(depth.ptr(y));
|
|
for( int x = 0; x < cloud.cols; x++ )
|
|
{
|
|
float z = depth_prt[x];
|
|
cloud_ptr[x].x = (float)((x - ox) * z * inv_fx);
|
|
cloud_ptr[x].y = (float)((y - oy) * z * inv_fy);
|
|
cloud_ptr[x].z = z;
|
|
}
|
|
}
|
|
}
|
|
|
|
#if SHOW_DEBUG_IMAGES
|
|
template<class ImageElemType>
|
|
static void warpImage( const Mat& image, const Mat& depth,
|
|
const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
|
|
Mat& warpedImage )
|
|
{
|
|
const Rect rect = Rect(0, 0, image.cols, image.rows);
|
|
|
|
std::vector<Point2f> points2d;
|
|
Mat cloud, transformedCloud;
|
|
|
|
cvtDepth2Cloud( depth, cloud, cameraMatrix );
|
|
perspectiveTransform( cloud, transformedCloud, Rt );
|
|
projectPoints( transformedCloud.reshape(3,1), Mat::eye(3,3,CV_64FC1), Mat::zeros(3,1,CV_64FC1), cameraMatrix, distCoeff, points2d );
|
|
|
|
Mat pointsPositions( points2d );
|
|
pointsPositions = pointsPositions.reshape( 2, image.rows );
|
|
|
|
warpedImage.create( image.size(), image.type() );
|
|
warpedImage = Scalar::all(0);
|
|
|
|
Mat zBuffer( image.size(), CV_32FC1, FLT_MAX );
|
|
for( int y = 0; y < image.rows; y++ )
|
|
{
|
|
for( int x = 0; x < image.cols; x++ )
|
|
{
|
|
const Point3f p3d = transformedCloud.at<Point3f>(y,x);
|
|
const Point p2d = pointsPositions.at<Point2f>(y,x);
|
|
if( !cvIsNaN(cloud.at<Point3f>(y,x).z) && cloud.at<Point3f>(y,x).z > 0 &&
|
|
rect.contains(p2d) && zBuffer.at<float>(p2d) > p3d.z )
|
|
{
|
|
warpedImage.at<ImageElemType>(p2d) = image.at<ImageElemType>(y,x);
|
|
zBuffer.at<float>(p2d) = p3d.z;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
static inline
|
|
void set2shorts( int& dst, int short_v1, int short_v2 )
|
|
{
|
|
unsigned short* ptr = reinterpret_cast<unsigned short*>(&dst);
|
|
ptr[0] = static_cast<unsigned short>(short_v1);
|
|
ptr[1] = static_cast<unsigned short>(short_v2);
|
|
}
|
|
|
|
static inline
|
|
void get2shorts( int src, int& short_v1, int& short_v2 )
|
|
{
|
|
typedef union { int vint32; unsigned short vuint16[2]; } s32tou16;
|
|
const unsigned short* ptr = (reinterpret_cast<s32tou16*>(&src))->vuint16;
|
|
short_v1 = ptr[0];
|
|
short_v2 = ptr[1];
|
|
}
|
|
|
|
static
|
|
int computeCorresp( const Mat& K, const Mat& K_inv, const Mat& Rt,
|
|
const Mat& depth0, const Mat& depth1, const Mat& texturedMask1, float maxDepthDiff,
|
|
Mat& corresps )
|
|
{
|
|
CV_Assert( K.type() == CV_64FC1 );
|
|
CV_Assert( K_inv.type() == CV_64FC1 );
|
|
CV_Assert( Rt.type() == CV_64FC1 );
|
|
|
|
corresps.create( depth1.size(), CV_32SC1 );
|
|
|
|
Mat R = Rt(Rect(0,0,3,3)).clone();
|
|
|
|
Mat KRK_inv = K * R * K_inv;
|
|
const double * KRK_inv_ptr = reinterpret_cast<const double *>(KRK_inv.ptr());
|
|
|
|
Mat Kt = Rt(Rect(3,0,1,3)).clone();
|
|
Kt = K * Kt;
|
|
const double * Kt_ptr = reinterpret_cast<const double *>(Kt.ptr());
|
|
|
|
Rect r(0, 0, depth1.cols, depth1.rows);
|
|
|
|
corresps = Scalar(-1);
|
|
int correspCount = 0;
|
|
for( int v1 = 0; v1 < depth1.rows; v1++ )
|
|
{
|
|
for( int u1 = 0; u1 < depth1.cols; u1++ )
|
|
{
|
|
float d1 = depth1.at<float>(v1,u1);
|
|
if( !cvIsNaN(d1) && texturedMask1.at<uchar>(v1,u1) )
|
|
{
|
|
float transformed_d1 = (float)(d1 * (KRK_inv_ptr[6] * u1 + KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]) + Kt_ptr[2]);
|
|
int u0 = cvRound((d1 * (KRK_inv_ptr[0] * u1 + KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]) + Kt_ptr[0]) / transformed_d1);
|
|
int v0 = cvRound((d1 * (KRK_inv_ptr[3] * u1 + KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]) + Kt_ptr[1]) / transformed_d1);
|
|
|
|
if( r.contains(Point(u0,v0)) )
|
|
{
|
|
float d0 = depth0.at<float>(v0,u0);
|
|
if( !cvIsNaN(d0) && std::abs(transformed_d1 - d0) <= maxDepthDiff )
|
|
{
|
|
int c = corresps.at<int>(v0,u0);
|
|
if( c != -1 )
|
|
{
|
|
int exist_u1, exist_v1;
|
|
get2shorts( c, exist_u1, exist_v1);
|
|
|
|
float exist_d1 = (float)(depth1.at<float>(exist_v1,exist_u1) * (KRK_inv_ptr[6] * exist_u1 + KRK_inv_ptr[7] * exist_v1 + KRK_inv_ptr[8]) + Kt_ptr[2]);
|
|
|
|
if( transformed_d1 > exist_d1 )
|
|
continue;
|
|
}
|
|
else
|
|
correspCount++;
|
|
|
|
set2shorts( corresps.at<int>(v0,u0), u1, v1 );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return correspCount;
|
|
}
|
|
|
|
static inline
|
|
void preprocessDepth( Mat depth0, Mat depth1,
|
|
const Mat& validMask0, const Mat& validMask1,
|
|
float minDepth, float maxDepth )
|
|
{
|
|
CV_DbgAssert( depth0.size() == depth1.size() );
|
|
|
|
for( int y = 0; y < depth0.rows; y++ )
|
|
{
|
|
for( int x = 0; x < depth0.cols; x++ )
|
|
{
|
|
float& d0 = depth0.at<float>(y,x);
|
|
if( !cvIsNaN(d0) && (d0 > maxDepth || d0 < minDepth || d0 <= 0 || (!validMask0.empty() && !validMask0.at<uchar>(y,x))) )
|
|
d0 = std::numeric_limits<float>::quiet_NaN();
|
|
|
|
float& d1 = depth1.at<float>(y,x);
|
|
if( !cvIsNaN(d1) && (d1 > maxDepth || d1 < minDepth || d1 <= 0 || (!validMask1.empty() && !validMask1.at<uchar>(y,x))) )
|
|
d1 = std::numeric_limits<float>::quiet_NaN();
|
|
}
|
|
}
|
|
}
|
|
|
|
static
|
|
void buildPyramids( const Mat& image0, const Mat& image1,
|
|
const Mat& depth0, const Mat& depth1,
|
|
const Mat& cameraMatrix, int sobelSize, double sobelScale,
|
|
const std::vector<float>& minGradMagnitudes,
|
|
std::vector<Mat>& pyramidImage0, std::vector<Mat>& pyramidDepth0,
|
|
std::vector<Mat>& pyramidImage1, std::vector<Mat>& pyramidDepth1,
|
|
std::vector<Mat>& pyramid_dI_dx1, std::vector<Mat>& pyramid_dI_dy1,
|
|
std::vector<Mat>& pyramidTexturedMask1, std::vector<Mat>& pyramidCameraMatrix )
|
|
{
|
|
const int pyramidMaxLevel = (int)minGradMagnitudes.size() - 1;
|
|
|
|
buildPyramid( image0, pyramidImage0, pyramidMaxLevel );
|
|
buildPyramid( image1, pyramidImage1, pyramidMaxLevel );
|
|
|
|
pyramid_dI_dx1.resize( pyramidImage1.size() );
|
|
pyramid_dI_dy1.resize( pyramidImage1.size() );
|
|
pyramidTexturedMask1.resize( pyramidImage1.size() );
|
|
|
|
pyramidCameraMatrix.reserve( pyramidImage1.size() );
|
|
|
|
Mat cameraMatrix_dbl;
|
|
cameraMatrix.convertTo( cameraMatrix_dbl, CV_64FC1 );
|
|
|
|
for( size_t i = 0; i < pyramidImage1.size(); i++ )
|
|
{
|
|
Sobel( pyramidImage1[i], pyramid_dI_dx1[i], CV_16S, 1, 0, sobelSize );
|
|
Sobel( pyramidImage1[i], pyramid_dI_dy1[i], CV_16S, 0, 1, sobelSize );
|
|
|
|
const Mat& dx = pyramid_dI_dx1[i];
|
|
const Mat& dy = pyramid_dI_dy1[i];
|
|
|
|
Mat texturedMask( dx.size(), CV_8UC1, Scalar(0) );
|
|
const float minScalesGradMagnitude2 = (float)((minGradMagnitudes[i] * minGradMagnitudes[i]) / (sobelScale * sobelScale));
|
|
for( int y = 0; y < dx.rows; y++ )
|
|
{
|
|
for( int x = 0; x < dx.cols; x++ )
|
|
{
|
|
float m2 = (float)(dx.at<short>(y,x)*dx.at<short>(y,x) + dy.at<short>(y,x)*dy.at<short>(y,x));
|
|
if( m2 >= minScalesGradMagnitude2 )
|
|
texturedMask.at<uchar>(y,x) = 255;
|
|
}
|
|
}
|
|
pyramidTexturedMask1[i] = texturedMask;
|
|
Mat levelCameraMatrix = i == 0 ? cameraMatrix_dbl : 0.5f * pyramidCameraMatrix[i-1];
|
|
levelCameraMatrix.at<double>(2,2) = 1.;
|
|
pyramidCameraMatrix.push_back( levelCameraMatrix );
|
|
}
|
|
|
|
buildPyramid( depth0, pyramidDepth0, pyramidMaxLevel );
|
|
buildPyramid( depth1, pyramidDepth1, pyramidMaxLevel );
|
|
}
|
|
|
|
static
|
|
bool solveSystem( const Mat& C, const Mat& dI_dt, double detThreshold, Mat& ksi )
|
|
{
|
|
#if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
|
|
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> eC, eCt, edI_dt;
|
|
cv2eigen(C, eC);
|
|
cv2eigen(dI_dt, edI_dt);
|
|
eCt = eC.transpose();
|
|
|
|
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A, B, eksi;
|
|
|
|
A = eCt * eC;
|
|
double det = A.determinant();
|
|
if( fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det) )
|
|
return false;
|
|
|
|
B = -eCt * edI_dt;
|
|
|
|
eksi = A.ldlt().solve(B);
|
|
eigen2cv( eksi, ksi );
|
|
|
|
#else
|
|
Mat A = C.t() * C;
|
|
|
|
double det = cv::determinant(A);
|
|
|
|
if( fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det) )
|
|
return false;
|
|
|
|
Mat B = -C.t() * dI_dt;
|
|
cv::solve( A, B, ksi, DECOMP_CHOLESKY );
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
typedef void (*ComputeCFuncPtr)( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy );
|
|
|
|
static
|
|
bool computeKsi( int transformType,
|
|
const Mat& image0, const Mat& cloud0,
|
|
const Mat& image1, const Mat& dI_dx1, const Mat& dI_dy1,
|
|
const Mat& corresps, int correspsCount,
|
|
double fx, double fy, double sobelScale, double determinantThreshold,
|
|
Mat& ksi )
|
|
{
|
|
int Cwidth = -1;
|
|
ComputeCFuncPtr computeCFuncPtr = 0;
|
|
if( transformType == RIGID_BODY_MOTION )
|
|
{
|
|
Cwidth = 6;
|
|
computeCFuncPtr = computeC_RigidBodyMotion;
|
|
}
|
|
else if( transformType == ROTATION )
|
|
{
|
|
Cwidth = 3;
|
|
computeCFuncPtr = computeC_Rotation;
|
|
}
|
|
else if( transformType == TRANSLATION )
|
|
{
|
|
Cwidth = 3;
|
|
computeCFuncPtr = computeC_Translation;
|
|
}
|
|
else
|
|
CV_Error( CV_StsBadFlag, "Unsupported value of transformation type flag.");
|
|
|
|
Mat C( correspsCount, Cwidth, CV_64FC1 );
|
|
Mat dI_dt( correspsCount, 1, CV_64FC1 );
|
|
|
|
double sigma = 0;
|
|
int pointCount = 0;
|
|
for( int v0 = 0; v0 < corresps.rows; v0++ )
|
|
{
|
|
for( int u0 = 0; u0 < corresps.cols; u0++ )
|
|
{
|
|
if( corresps.at<int>(v0,u0) != -1 )
|
|
{
|
|
int u1, v1;
|
|
get2shorts( corresps.at<int>(v0,u0), u1, v1 );
|
|
double diff = static_cast<double>(image1.at<uchar>(v1,u1)) -
|
|
static_cast<double>(image0.at<uchar>(v0,u0));
|
|
sigma += diff * diff;
|
|
pointCount++;
|
|
}
|
|
}
|
|
}
|
|
sigma = std::sqrt(sigma/pointCount);
|
|
|
|
pointCount = 0;
|
|
for( int v0 = 0; v0 < corresps.rows; v0++ )
|
|
{
|
|
for( int u0 = 0; u0 < corresps.cols; u0++ )
|
|
{
|
|
if( corresps.at<int>(v0,u0) != -1 )
|
|
{
|
|
int u1, v1;
|
|
get2shorts( corresps.at<int>(v0,u0), u1, v1 );
|
|
|
|
double diff = static_cast<double>(image1.at<uchar>(v1,u1)) -
|
|
static_cast<double>(image0.at<uchar>(v0,u0));
|
|
double w = sigma + std::abs(diff);
|
|
w = w > DBL_EPSILON ? 1./w : 1.;
|
|
|
|
(*computeCFuncPtr)( (double*)C.ptr(pointCount),
|
|
w * sobelScale * dI_dx1.at<short int>(v1,u1),
|
|
w * sobelScale * dI_dy1.at<short int>(v1,u1),
|
|
cloud0.at<Point3f>(v0,u0), fx, fy);
|
|
|
|
dI_dt.at<double>(pointCount) = w * diff;
|
|
pointCount++;
|
|
}
|
|
}
|
|
}
|
|
|
|
Mat sln;
|
|
bool solutionExist = solveSystem( C, dI_dt, determinantThreshold, sln );
|
|
|
|
if( solutionExist )
|
|
{
|
|
ksi.create(6,1,CV_64FC1);
|
|
ksi = Scalar(0);
|
|
|
|
Mat subksi;
|
|
if( transformType == RIGID_BODY_MOTION )
|
|
{
|
|
subksi = ksi;
|
|
}
|
|
else if( transformType == ROTATION )
|
|
{
|
|
subksi = ksi.rowRange(0,3);
|
|
}
|
|
else if( transformType == TRANSLATION )
|
|
{
|
|
subksi = ksi.rowRange(3,6);
|
|
}
|
|
|
|
sln.copyTo( subksi );
|
|
}
|
|
|
|
return solutionExist;
|
|
}
|
|
|
|
bool cv::RGBDOdometry( cv::Mat& Rt, const Mat& initRt,
|
|
const cv::Mat& image0, const cv::Mat& _depth0, const cv::Mat& validMask0,
|
|
const cv::Mat& image1, const cv::Mat& _depth1, const cv::Mat& validMask1,
|
|
const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff,
|
|
const std::vector<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
|
|
int transformType )
|
|
{
|
|
const int sobelSize = 3;
|
|
const double sobelScale = 1./8;
|
|
|
|
Mat depth0 = _depth0.clone(),
|
|
depth1 = _depth1.clone();
|
|
|
|
// check RGB-D input data
|
|
CV_Assert( !image0.empty() );
|
|
CV_Assert( image0.type() == CV_8UC1 );
|
|
CV_Assert( depth0.type() == CV_32FC1 && depth0.size() == image0.size() );
|
|
|
|
CV_Assert( image1.size() == image0.size() );
|
|
CV_Assert( image1.type() == CV_8UC1 );
|
|
CV_Assert( depth1.type() == CV_32FC1 && depth1.size() == image0.size() );
|
|
|
|
// check masks
|
|
CV_Assert( validMask0.empty() || (validMask0.type() == CV_8UC1 && validMask0.size() == image0.size()) );
|
|
CV_Assert( validMask1.empty() || (validMask1.type() == CV_8UC1 && validMask1.size() == image0.size()) );
|
|
|
|
// check camera params
|
|
CV_Assert( cameraMatrix.type() == CV_32FC1 && cameraMatrix.size() == Size(3,3) );
|
|
|
|
// other checks
|
|
CV_Assert( iterCounts.empty() || minGradientMagnitudes.empty() ||
|
|
minGradientMagnitudes.size() == iterCounts.size() );
|
|
CV_Assert( initRt.empty() || (initRt.type()==CV_64FC1 && initRt.size()==Size(4,4) ) );
|
|
|
|
std::vector<int> defaultIterCounts;
|
|
std::vector<float> defaultMinGradMagnitudes;
|
|
std::vector<int> const* iterCountsPtr = &iterCounts;
|
|
std::vector<float> const* minGradientMagnitudesPtr = &minGradientMagnitudes;
|
|
|
|
if( iterCounts.empty() || minGradientMagnitudes.empty() )
|
|
{
|
|
defaultIterCounts.resize(4);
|
|
defaultIterCounts[0] = 7;
|
|
defaultIterCounts[1] = 7;
|
|
defaultIterCounts[2] = 7;
|
|
defaultIterCounts[3] = 10;
|
|
|
|
defaultMinGradMagnitudes.resize(4);
|
|
defaultMinGradMagnitudes[0] = 12;
|
|
defaultMinGradMagnitudes[1] = 5;
|
|
defaultMinGradMagnitudes[2] = 3;
|
|
defaultMinGradMagnitudes[3] = 1;
|
|
|
|
iterCountsPtr = &defaultIterCounts;
|
|
minGradientMagnitudesPtr = &defaultMinGradMagnitudes;
|
|
}
|
|
|
|
preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth );
|
|
|
|
std::vector<Mat> pyramidImage0, pyramidDepth0,
|
|
pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1,
|
|
pyramidCameraMatrix;
|
|
buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelSize, sobelScale, *minGradientMagnitudesPtr,
|
|
pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1,
|
|
pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix );
|
|
|
|
Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
|
|
Mat currRt, ksi;
|
|
for( int level = (int)iterCountsPtr->size() - 1; level >= 0; level-- )
|
|
{
|
|
const Mat& levelCameraMatrix = pyramidCameraMatrix[level];
|
|
|
|
const Mat& levelImage0 = pyramidImage0[level];
|
|
const Mat& levelDepth0 = pyramidDepth0[level];
|
|
Mat levelCloud0;
|
|
cvtDepth2Cloud( pyramidDepth0[level], levelCloud0, levelCameraMatrix );
|
|
|
|
const Mat& levelImage1 = pyramidImage1[level];
|
|
const Mat& levelDepth1 = pyramidDepth1[level];
|
|
const Mat& level_dI_dx1 = pyramid_dI_dx1[level];
|
|
const Mat& level_dI_dy1 = pyramid_dI_dy1[level];
|
|
|
|
CV_Assert( level_dI_dx1.type() == CV_16S );
|
|
CV_Assert( level_dI_dy1.type() == CV_16S );
|
|
|
|
const double fx = levelCameraMatrix.at<double>(0,0);
|
|
const double fy = levelCameraMatrix.at<double>(1,1);
|
|
const double determinantThreshold = 1e-6;
|
|
|
|
Mat corresps( levelImage0.size(), levelImage0.type() );
|
|
|
|
// Run transformation search on current level iteratively.
|
|
for( int iter = 0; iter < (*iterCountsPtr)[level]; iter ++ )
|
|
{
|
|
int correspsCount = computeCorresp( levelCameraMatrix, levelCameraMatrix.inv(), resultRt.inv(DECOMP_SVD),
|
|
levelDepth0, levelDepth1, pyramidTexturedMask1[level], maxDepthDiff,
|
|
corresps );
|
|
|
|
if( correspsCount == 0 )
|
|
break;
|
|
|
|
bool solutionExist = computeKsi( transformType,
|
|
levelImage0, levelCloud0,
|
|
levelImage1, level_dI_dx1, level_dI_dy1,
|
|
corresps, correspsCount,
|
|
fx, fy, sobelScale, determinantThreshold,
|
|
ksi );
|
|
|
|
if( !solutionExist )
|
|
break;
|
|
|
|
computeProjectiveMatrix( ksi, currRt );
|
|
|
|
resultRt = currRt * resultRt;
|
|
|
|
#if SHOW_DEBUG_IMAGES
|
|
std::cout << "currRt " << currRt << std::endl;
|
|
Mat warpedImage0;
|
|
const Mat distCoeff(1,5,CV_32FC1,Scalar(0));
|
|
warpImage<uchar>( levelImage0, levelDepth0, resultRt, levelCameraMatrix, distCoeff, warpedImage0 );
|
|
|
|
imshow( "im0", levelImage0 );
|
|
imshow( "wim0", warpedImage0 );
|
|
imshow( "im1", levelImage1 );
|
|
waitKey();
|
|
#endif
|
|
}
|
|
}
|
|
|
|
Rt = resultRt;
|
|
|
|
return !Rt.empty();
|
|
}
|