modules/contrib/src/rgbdodometry.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44
  45 #define SHOW_DEBUG_IMAGES 0
  46
  47 #include "opencv2/core.hpp"
  48 #include "opencv2/calib3d.hpp"
  49
  50 #if SHOW_DEBUG_IMAGES
  51 #  include "opencv2/highgui.hpp"
  52 #endif
  53
  54 #include <iostream>
  55 #include <limits>
  56
  57 #if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
  58 #  ifdef ANDROID
  59      template <typename Scalar> Scalar log2(Scalar v) { return std::log(v)/std::log(Scalar(2)); }
  60 #  endif
  61 #  if defined __GNUC__ && defined __APPLE__
  62 #    pragma GCC diagnostic ignored "-Wshadow"
  63 #  endif
  64 #  include <unsupported/Eigen/MatrixFunctions>
  65 #  include <Eigen/Dense>
  66 #endif
  67
  68 using namespace cv;
  69
  70 inline static
  71 void computeC_RigidBodyMotion( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
  72 {
  73     double invz  = 1. / p3d.z,
  74            v0 = dIdx * fx * invz,
  75            v1 = dIdy * fy * invz,
  76            v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
  77
  78     C[0] = -p3d.z * v1 + p3d.y * v2;
  79     C[1] =  p3d.z * v0 - p3d.x * v2;
  80     C[2] = -p3d.y * v0 + p3d.x * v1;
  81     C[3] = v0;
  82     C[4] = v1;
  83     C[5] = v2;
  84 }
  85
  86 inline static
  87 void computeC_Rotation( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
  88 {
  89     double invz  = 1. / p3d.z,
  90            v0 = dIdx * fx * invz,
  91            v1 = dIdy * fy * invz,
  92            v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
  93
  94     C[0] = -p3d.z * v1 + p3d.y * v2;
  95     C[1] =  p3d.z * v0 - p3d.x * v2;
  96     C[2] = -p3d.y * v0 + p3d.x * v1;
  97 }
  98
  99 inline static
 100 void computeC_Translation( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy )
 101 {
 102     double invz  = 1. / p3d.z,
 103            v0 = dIdx * fx * invz,
 104            v1 = dIdy * fy * invz,
 105            v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
 106
 107     C[0] = v0;
 108     C[1] = v1;
 109     C[2] = v2;
 110 }
 111
 112 inline static
 113 void computeProjectiveMatrix( const Mat& ksi, Mat& Rt )
 114 {
 115     CV_Assert( ksi.size() == Size(1,6) && ksi.type() == CV_64FC1 );
 116
 117 #if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3 && (!defined _MSC_VER || !defined _M_X64 || _MSC_VER > 1500)
 118     const double* ksi_ptr = reinterpret_cast<const double*>(ksi.ptr(0));
 119     Eigen::Matrix<double,4,4> twist, g;
 120     twist << 0.,          -ksi_ptr[2], ksi_ptr[1],  ksi_ptr[3],
 121              ksi_ptr[2],  0.,          -ksi_ptr[0], ksi_ptr[4],
 122              -ksi_ptr[1], ksi_ptr[0],  0,           ksi_ptr[5],
 123              0.,          0.,          0.,          0.;
 124     g = twist.exp();
 125
 126
 127     eigen2cv(g, Rt);
 128 #else
 129     // for infinitesimal transformation
 130     Rt = Mat::eye(4, 4, CV_64FC1);
 131
 132     Mat R = Rt(Rect(0,0,3,3));
 133     Mat rvec = ksi.rowRange(0,3);
 134
 135     Rodrigues( rvec, R );
 136
 137     Rt.at<double>(0,3) = ksi.at<double>(3);
 138     Rt.at<double>(1,3) = ksi.at<double>(4);
 139     Rt.at<double>(2,3) = ksi.at<double>(5);
 140 #endif
 141 }
 142
 143 static
 144 void cvtDepth2Cloud( const Mat& depth, Mat& cloud, const Mat& cameraMatrix )
 145 {
 146     CV_Assert( cameraMatrix.type() == CV_64FC1 );
 147     const double inv_fx = 1.f/cameraMatrix.at<double>(0,0);
 148     const double inv_fy = 1.f/cameraMatrix.at<double>(1,1);
 149     const double ox = cameraMatrix.at<double>(0,2);
 150     const double oy = cameraMatrix.at<double>(1,2);
 151     cloud.create( depth.size(), CV_32FC3 );
 152     for( int y = 0; y < cloud.rows; y++ )
 153     {
 154         Point3f* cloud_ptr = reinterpret_cast<Point3f*>(cloud.ptr(y));
 155         const float* depth_prt = reinterpret_cast<const float*>(depth.ptr(y));
 156         for( int x = 0; x < cloud.cols; x++ )
 157         {
 158             float z = depth_prt[x];
 159             cloud_ptr[x].x = (float)((x - ox) * z * inv_fx);
 160             cloud_ptr[x].y = (float)((y - oy) * z * inv_fy);
 161             cloud_ptr[x].z = z;
 162         }
 163     }
 164 }
 165
 166 #if SHOW_DEBUG_IMAGES
 167 template<class ImageElemType>
 168 static void warpImage( const Mat& image, const Mat& depth,
 169                        const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
 170                        Mat& warpedImage )
 171 {
 172     const Rect rect = Rect(0, 0, image.cols, image.rows);
 173
 174     std::vector<Point2f> points2d;
 175     Mat cloud, transformedCloud;
 176
 177     cvtDepth2Cloud( depth, cloud, cameraMatrix );
 178     perspectiveTransform( cloud, transformedCloud, Rt );
 179     projectPoints( transformedCloud.reshape(3,1), Mat::eye(3,3,CV_64FC1), Mat::zeros(3,1,CV_64FC1), cameraMatrix, distCoeff, points2d );
 180
 181     Mat pointsPositions( points2d );
 182     pointsPositions = pointsPositions.reshape( 2, image.rows );
 183
 184     warpedImage.create( image.size(), image.type() );
 185     warpedImage = Scalar::all(0);
 186
 187     Mat zBuffer( image.size(), CV_32FC1, FLT_MAX );
 188     for( int y = 0; y < image.rows; y++ )
 189     {
 190         for( int x = 0; x < image.cols; x++ )
 191         {
 192             const Point3f p3d = transformedCloud.at<Point3f>(y,x);
 193             const Point p2d = pointsPositions.at<Point2f>(y,x);
 194             if( !cvIsNaN(cloud.at<Point3f>(y,x).z) && cloud.at<Point3f>(y,x).z > 0 &&
 195                 rect.contains(p2d) && zBuffer.at<float>(p2d) > p3d.z )
 196             {
 197                 warpedImage.at<ImageElemType>(p2d) = image.at<ImageElemType>(y,x);
 198                 zBuffer.at<float>(p2d) = p3d.z;
 199             }
 200         }
 201     }
 202 }
 203 #endif
 204
 205 static inline
 206 void set2shorts( int& dst, int short_v1, int short_v2 )
 207 {
 208     unsigned short* ptr = reinterpret_cast<unsigned short*>(&dst);
 209     ptr[0] = static_cast<unsigned short>(short_v1);
 210     ptr[1] = static_cast<unsigned short>(short_v2);
 211 }
 212
 213 static inline
 214 void get2shorts( int src, int& short_v1, int& short_v2 )
 215 {
 216     typedef union { int vint32; unsigned short vuint16[2]; } s32tou16;
 217     const unsigned short* ptr = (reinterpret_cast<s32tou16*>(&src))->vuint16;
 218     short_v1 = ptr[0];
 219     short_v2 = ptr[1];
 220 }
 221
 222 static
 223 int computeCorresp( const Mat& K, const Mat& K_inv, const Mat& Rt,
 224                     const Mat& depth0, const Mat& depth1, const Mat& texturedMask1, float maxDepthDiff,
 225                     Mat& corresps )
 226 {
 227     CV_Assert( K.type() == CV_64FC1 );
 228     CV_Assert( K_inv.type() == CV_64FC1 );
 229     CV_Assert( Rt.type() == CV_64FC1 );
 230
 231     corresps.create( depth1.size(), CV_32SC1 );
 232
 233     Mat R = Rt(Rect(0,0,3,3)).clone();
 234
 235     Mat KRK_inv = K * R * K_inv;
 236     const double * KRK_inv_ptr = reinterpret_cast<const double *>(KRK_inv.ptr());
 237
 238     Mat Kt = Rt(Rect(3,0,1,3)).clone();
 239     Kt = K * Kt;
 240     const double * Kt_ptr = reinterpret_cast<const double *>(Kt.ptr());
 241
 242     Rect r(0, 0, depth1.cols, depth1.rows);
 243
 244     corresps = Scalar(-1);
 245     int correspCount = 0;
 246     for( int v1 = 0; v1 < depth1.rows; v1++ )
 247     {
 248         for( int u1 = 0; u1 < depth1.cols; u1++ )
 249         {
 250             float d1 = depth1.at<float>(v1,u1);
 251             if( !cvIsNaN(d1) && texturedMask1.at<uchar>(v1,u1) )
 252             {
 253                 float transformed_d1 = (float)(d1 * (KRK_inv_ptr[6] * u1 + KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]) + Kt_ptr[2]);
 254                 int u0 = cvRound((d1 * (KRK_inv_ptr[0] * u1 + KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]) + Kt_ptr[0]) / transformed_d1);
 255                 int v0 = cvRound((d1 * (KRK_inv_ptr[3] * u1 + KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]) + Kt_ptr[1]) / transformed_d1);
 256
 257                 if( r.contains(Point(u0,v0)) )
 258                 {
 259                     float d0 = depth0.at<float>(v0,u0);
 260                     if( !cvIsNaN(d0) && std::abs(transformed_d1 - d0) <= maxDepthDiff )
 261                     {
 262                         int c = corresps.at<int>(v0,u0);
 263                         if( c != -1 )
 264                         {
 265                             int exist_u1, exist_v1;
 266                             get2shorts( c, exist_u1, exist_v1);
 267
 268                             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]);
 269
 270                             if( transformed_d1 > exist_d1 )
 271                                 continue;
 272                         }
 273                         else
 274                             correspCount++;
 275
 276                         set2shorts( corresps.at<int>(v0,u0), u1, v1 );
 277                     }
 278                 }
 279             }
 280         }
 281     }
 282
 283     return correspCount;
 284 }
 285
 286 static inline
 287 void preprocessDepth( Mat depth0, Mat depth1,
 288                       const Mat& validMask0, const Mat& validMask1,
 289                       float minDepth, float maxDepth )
 290 {
 291     CV_DbgAssert( depth0.size() == depth1.size() );
 292
 293     for( int y = 0; y < depth0.rows; y++ )
 294     {
 295         for( int x = 0; x < depth0.cols; x++ )
 296         {
 297             float& d0 = depth0.at<float>(y,x);
 298             if( !cvIsNaN(d0) && (d0 > maxDepth || d0 < minDepth || d0 <= 0 || (!validMask0.empty() && !validMask0.at<uchar>(y,x))) )
 299                 d0 = std::numeric_limits<float>::quiet_NaN();
 300
 301             float& d1 = depth1.at<float>(y,x);
 302             if( !cvIsNaN(d1) && (d1 > maxDepth || d1 < minDepth || d1 <= 0 || (!validMask1.empty() && !validMask1.at<uchar>(y,x))) )
 303                 d1 = std::numeric_limits<float>::quiet_NaN();
 304         }
 305     }
 306 }
 307
 308 static
 309 void buildPyramids( const Mat& image0, const Mat& image1,
 310                     const Mat& depth0, const Mat& depth1,
 311                     const Mat& cameraMatrix, int sobelSize, double sobelScale,
 312                     const std::vector<float>& minGradMagnitudes,
 313                     std::vector<Mat>& pyramidImage0, std::vector<Mat>& pyramidDepth0,
 314                     std::vector<Mat>& pyramidImage1, std::vector<Mat>& pyramidDepth1,
 315                     std::vector<Mat>& pyramid_dI_dx1, std::vector<Mat>& pyramid_dI_dy1,
 316                     std::vector<Mat>& pyramidTexturedMask1, std::vector<Mat>& pyramidCameraMatrix )
 317 {
 318     const int pyramidMaxLevel = (int)minGradMagnitudes.size() - 1;
 319
 320     buildPyramid( image0, pyramidImage0, pyramidMaxLevel );
 321     buildPyramid( image1, pyramidImage1, pyramidMaxLevel );
 322
 323     pyramid_dI_dx1.resize( pyramidImage1.size() );
 324     pyramid_dI_dy1.resize( pyramidImage1.size() );
 325     pyramidTexturedMask1.resize( pyramidImage1.size() );
 326
 327     pyramidCameraMatrix.reserve( pyramidImage1.size() );
 328
 329     Mat cameraMatrix_dbl;
 330     cameraMatrix.convertTo( cameraMatrix_dbl, CV_64FC1 );
 331
 332     for( size_t i = 0; i < pyramidImage1.size(); i++ )
 333     {
 334         Sobel( pyramidImage1[i], pyramid_dI_dx1[i], CV_16S, 1, 0, sobelSize );
 335         Sobel( pyramidImage1[i], pyramid_dI_dy1[i], CV_16S, 0, 1, sobelSize );
 336
 337         const Mat& dx = pyramid_dI_dx1[i];
 338         const Mat& dy = pyramid_dI_dy1[i];
 339
 340         Mat texturedMask( dx.size(), CV_8UC1, Scalar(0) );
 341         const float minScalesGradMagnitude2 = (float)((minGradMagnitudes[i] * minGradMagnitudes[i]) / (sobelScale * sobelScale));
 342         for( int y = 0; y < dx.rows; y++ )
 343         {
 344             for( int x = 0; x < dx.cols; x++ )
 345             {
 346                 float m2 = (float)(dx.at<short>(y,x)*dx.at<short>(y,x) + dy.at<short>(y,x)*dy.at<short>(y,x));
 347                 if( m2 >= minScalesGradMagnitude2 )
 348                     texturedMask.at<uchar>(y,x) = 255;
 349             }
 350         }
 351         pyramidTexturedMask1[i] = texturedMask;
 352         Mat levelCameraMatrix = i == 0 ? cameraMatrix_dbl : 0.5f * pyramidCameraMatrix[i-1];
 353         levelCameraMatrix.at<double>(2,2) = 1.;
 354         pyramidCameraMatrix.push_back( levelCameraMatrix );
 355     }
 356
 357     buildPyramid( depth0, pyramidDepth0, pyramidMaxLevel );
 358     buildPyramid( depth1, pyramidDepth1, pyramidMaxLevel );
 359 }
 360
 361 static
 362 bool solveSystem( const Mat& C, const Mat& dI_dt, double detThreshold, Mat& ksi )
 363 {
 364 #if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
 365     Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> eC, eCt, edI_dt;
 366     cv2eigen(C, eC);
 367     cv2eigen(dI_dt, edI_dt);
 368     eCt = eC.transpose();
 369
 370     Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A, B, eksi;
 371
 372     A = eCt * eC;
 373     double det = A.determinant();
 374     if( fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det) )
 375         return false;
 376
 377     B = -eCt * edI_dt;
 378
 379     eksi = A.ldlt().solve(B);
 380     eigen2cv( eksi, ksi );
 381
 382 #else
 383     Mat A = C.t() * C;
 384
 385     double det = cv::determinant(A);
 386
 387     if( fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det) )
 388         return false;
 389
 390     Mat B = -C.t() * dI_dt;
 391     cv::solve( A, B, ksi, DECOMP_CHOLESKY );
 392 #endif
 393
 394     return true;
 395 }
 396
 397 typedef void (*ComputeCFuncPtr)( double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy );
 398
 399 static
 400 bool computeKsi( int transformType,
 401                  const Mat& image0, const Mat&  cloud0,
 402                  const Mat& image1, const Mat& dI_dx1, const Mat& dI_dy1,
 403                  const Mat& corresps, int correspsCount,
 404                  double fx, double fy, double sobelScale, double determinantThreshold,
 405                  Mat& ksi )
 406 {
 407     int Cwidth = -1;
 408     ComputeCFuncPtr computeCFuncPtr = 0;
 409     if( transformType == RIGID_BODY_MOTION )
 410     {
 411         Cwidth = 6;
 412         computeCFuncPtr = computeC_RigidBodyMotion;
 413     }
 414     else if( transformType == ROTATION )
 415     {
 416         Cwidth = 3;
 417         computeCFuncPtr = computeC_Rotation;
 418     }
 419     else if( transformType == TRANSLATION )
 420     {
 421         Cwidth = 3;
 422         computeCFuncPtr = computeC_Translation;
 423     }
 424     else
 425         CV_Error(Error::StsBadFlag, "Unsupported value of transformation type flag.");
 426
 427     Mat C( correspsCount, Cwidth, CV_64FC1 );
 428     Mat dI_dt( correspsCount, 1, CV_64FC1 );
 429
 430     double sigma = 0;
 431     int pointCount = 0;
 432     for( int v0 = 0; v0 < corresps.rows; v0++ )
 433     {
 434         for( int u0 = 0; u0 < corresps.cols; u0++ )
 435         {
 436             if( corresps.at<int>(v0,u0) != -1 )
 437             {
 438                 int u1, v1;
 439                 get2shorts( corresps.at<int>(v0,u0), u1, v1 );
 440                 double diff = static_cast<double>(image1.at<uchar>(v1,u1)) -
 441                               static_cast<double>(image0.at<uchar>(v0,u0));
 442                 sigma += diff * diff;
 443                 pointCount++;
 444             }
 445         }
 446     }
 447     sigma = std::sqrt(sigma/pointCount);
 448
 449     pointCount = 0;
 450     for( int v0 = 0; v0 < corresps.rows; v0++ )
 451     {
 452         for( int u0 = 0; u0 < corresps.cols; u0++ )
 453         {
 454             if( corresps.at<int>(v0,u0) != -1 )
 455             {
 456                 int u1, v1;
 457                 get2shorts( corresps.at<int>(v0,u0), u1, v1 );
 458
 459                 double diff = static_cast<double>(image1.at<uchar>(v1,u1)) -
 460                               static_cast<double>(image0.at<uchar>(v0,u0));
 461                 double w = sigma + std::abs(diff);
 462                 w = w > DBL_EPSILON ? 1./w : 1.;
 463
 464                 (*computeCFuncPtr)( (double*)C.ptr(pointCount),
 465                                      w * sobelScale * dI_dx1.at<short int>(v1,u1),
 466                                      w * sobelScale * dI_dy1.at<short int>(v1,u1),
 467                                      cloud0.at<Point3f>(v0,u0), fx, fy);
 468
 469                 dI_dt.at<double>(pointCount) = w * diff;
 470                 pointCount++;
 471             }
 472         }
 473     }
 474
 475     Mat sln;
 476     bool solutionExist = solveSystem( C, dI_dt, determinantThreshold, sln );
 477
 478     if( solutionExist )
 479     {
 480         ksi.create(6,1,CV_64FC1);
 481         ksi = Scalar(0);
 482
 483         Mat subksi;
 484         if( transformType == RIGID_BODY_MOTION )
 485         {
 486             subksi = ksi;
 487         }
 488         else if( transformType == ROTATION )
 489         {
 490             subksi = ksi.rowRange(0,3);
 491         }
 492         else if( transformType == TRANSLATION )
 493         {
 494             subksi = ksi.rowRange(3,6);
 495         }
 496
 497         sln.copyTo( subksi );
 498     }
 499
 500     return solutionExist;
 501 }
 502
 503 bool cv::RGBDOdometry( cv::Mat& Rt, const Mat& initRt,
 504                        const cv::Mat& image0, const cv::Mat& _depth0, const cv::Mat& validMask0,
 505                        const cv::Mat& image1, const cv::Mat& _depth1, const cv::Mat& validMask1,
 506                        const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff,
 507                        const std::vector<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
 508                        int transformType )
 509 {
 510     const int sobelSize = 3;
 511     const double sobelScale = 1./8;
 512
 513     Mat depth0 = _depth0.clone(),
 514         depth1 = _depth1.clone();
 515
 516     // check RGB-D input data
 517     CV_Assert( !image0.empty() );
 518     CV_Assert( image0.type() == CV_8UC1 );
 519     CV_Assert( depth0.type() == CV_32FC1 && depth0.size() == image0.size() );
 520
 521     CV_Assert( image1.size() == image0.size() );
 522     CV_Assert( image1.type() == CV_8UC1 );
 523     CV_Assert( depth1.type() == CV_32FC1 && depth1.size() == image0.size() );
 524
 525     // check masks
 526     CV_Assert( validMask0.empty() || (validMask0.type() == CV_8UC1 && validMask0.size() == image0.size()) );
 527     CV_Assert( validMask1.empty() || (validMask1.type() == CV_8UC1 && validMask1.size() == image0.size()) );
 528
 529     // check camera params
 530     CV_Assert( cameraMatrix.type() == CV_32FC1 && cameraMatrix.size() == Size(3,3) );
 531
 532     // other checks
 533     CV_Assert( iterCounts.empty() || minGradientMagnitudes.empty() ||
 534                minGradientMagnitudes.size() == iterCounts.size() );
 535     CV_Assert( initRt.empty() || (initRt.type()==CV_64FC1 && initRt.size()==Size(4,4) ) );
 536
 537     std::vector<int> defaultIterCounts;
 538     std::vector<float> defaultMinGradMagnitudes;
 539     std::vector<int> const* iterCountsPtr = &iterCounts;
 540     std::vector<float> const* minGradientMagnitudesPtr = &minGradientMagnitudes;
 541
 542     if( iterCounts.empty() || minGradientMagnitudes.empty() )
 543     {
 544         defaultIterCounts.resize(4);
 545         defaultIterCounts[0] = 7;
 546         defaultIterCounts[1] = 7;
 547         defaultIterCounts[2] = 7;
 548         defaultIterCounts[3] = 10;
 549
 550         defaultMinGradMagnitudes.resize(4);
 551         defaultMinGradMagnitudes[0] = 12;
 552         defaultMinGradMagnitudes[1] = 5;
 553         defaultMinGradMagnitudes[2] = 3;
 554         defaultMinGradMagnitudes[3] = 1;
 555
 556         iterCountsPtr = &defaultIterCounts;
 557         minGradientMagnitudesPtr = &defaultMinGradMagnitudes;
 558     }
 559
 560     preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth );
 561
 562     std::vector<Mat> pyramidImage0, pyramidDepth0,
 563                 pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1,
 564                 pyramidCameraMatrix;
 565     buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelSize, sobelScale, *minGradientMagnitudesPtr,
 566                    pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1,
 567                    pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix );
 568
 569     Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
 570     Mat currRt, ksi;
 571     for( int level = (int)iterCountsPtr->size() - 1; level >= 0; level-- )
 572     {
 573         const Mat& levelCameraMatrix = pyramidCameraMatrix[level];
 574
 575         const Mat& levelImage0 = pyramidImage0[level];
 576         const Mat& levelDepth0 = pyramidDepth0[level];
 577         Mat levelCloud0;
 578         cvtDepth2Cloud( pyramidDepth0[level], levelCloud0, levelCameraMatrix );
 579
 580         const Mat& levelImage1 = pyramidImage1[level];
 581         const Mat& levelDepth1 = pyramidDepth1[level];
 582         const Mat& level_dI_dx1 = pyramid_dI_dx1[level];
 583         const Mat& level_dI_dy1 = pyramid_dI_dy1[level];
 584
 585         CV_Assert( level_dI_dx1.type() == CV_16S );
 586         CV_Assert( level_dI_dy1.type() == CV_16S );
 587
 588         const double fx = levelCameraMatrix.at<double>(0,0);
 589         const double fy = levelCameraMatrix.at<double>(1,1);
 590         const double determinantThreshold = 1e-6;
 591
 592         Mat corresps( levelImage0.size(), levelImage0.type() );
 593
 594         // Run transformation search on current level iteratively.
 595         for( int iter = 0; iter < (*iterCountsPtr)[level]; iter ++ )
 596         {
 597             int correspsCount = computeCorresp( levelCameraMatrix, levelCameraMatrix.inv(), resultRt.inv(DECOMP_SVD),
 598                                                 levelDepth0, levelDepth1, pyramidTexturedMask1[level], maxDepthDiff,
 599                                                 corresps );
 600
 601             if( correspsCount == 0 )
 602                 break;
 603
 604             bool solutionExist = computeKsi( transformType,
 605                                              levelImage0, levelCloud0,
 606                                              levelImage1, level_dI_dx1, level_dI_dy1,
 607                                              corresps, correspsCount,
 608                                              fx, fy, sobelScale, determinantThreshold,
 609                                              ksi );
 610
 611             if( !solutionExist )
 612                 break;
 613
 614             computeProjectiveMatrix( ksi, currRt );
 615
 616             resultRt = currRt * resultRt;
 617
 618 #if SHOW_DEBUG_IMAGES
 619             std::cout << "currRt " << currRt << std::endl;
 620             Mat warpedImage0;
 621             const Mat distCoeff(1,5,CV_32FC1,Scalar(0));
 622             warpImage<uchar>( levelImage0, levelDepth0, resultRt, levelCameraMatrix, distCoeff, warpedImage0 );
 623
 624             imshow( "im0", levelImage0 );
 625             imshow( "wim0", warpedImage0 );
 626             imshow( "im1", levelImage1 );
 627             waitKey();
 628 #endif
 629         }
 630     }
 631
 632     Rt = resultRt;
 633
 634     return !Rt.empty();
 635 }