initial version of rgbd odometry with sample

modules/contrib/include/opencv2/contrib/contrib.hpp

@@ -623,6 +623,19 @@ namespace cv
      * 4) convert the colors back to RGB
      */
     CV_EXPORTS void generateColors( std::vector<Scalar>& colors, size_t count, size_t factor=100 );
+
+
+    /*
+     *  Estimate the rigid body motion from frame0 to frame1. The method is based on the paper
+     *  "Real-Time Visual Odometry from Dense RGB-D Images", F. Steinbucker, J. Strum, D. Cremers, ICCV, 2011.
+     */
+    CV_EXPORTS bool RGBDOdometry( cv::Mat& Rt,
+                                  const cv::Mat& image0, const cv::Mat& depth0, const cv::Mat& mask0,
+                                  const cv::Mat& image1, const cv::Mat& depth1, const cv::Mat& mask1,
+                                  const cv::Mat& cameraMatrix, const std::vector<int>& iterCounts,
+                                  const std::vector<float>& minGradientMagnitudes,
+                                  float minDepth, float maxDepth, float maxDepthDiff );
+
 }
 
 #include "opencv2/contrib/retina.hpp"

modules/contrib/src/rgbdodometry.cpp

@@ -0,0 +1,433 @@
+/*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 "opencv2/core/core.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+
+#include "opencv2/highgui/highgui.hpp"
+#include "precomp.hpp"
+
+#include <iostream>
+
+#ifdef HAVE_EIGEN
+#include <Eigen/Core>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include <Eigen/Dense>
+#endif
+
+using namespace cv;
+
+inline static
+void computeC( 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 computeProjectiveMatrix( const Mat& ksi, Mat& Rt )
+{
+    CV_Assert( ksi.size() == Size(1,6) && ksi.type() == CV_64FC1 );
+
+#ifdef HAVE_EIGEN
+    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 = (x - ox) * z * inv_fx;
+            cloud_ptr[x].y = (y - oy) * z * inv_fy;
+            cloud_ptr[x].z = z;
+        }
+    }
+}
+
+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 )
+{
+    unsigned short* ptr = reinterpret_cast<unsigned short*>(&src);
+    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 = 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 = 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 = 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 = NAN;
+        }
+    }
+}
+
+static
+void buildPyramids( const Mat& image0, const Mat& image1,
+                    const Mat& depth0, const Mat& depth1,
+                    const Mat& cameraMatrix, double sobelScale,
+                    const vector<float>& minGradMagnitudes,
+                    vector<Mat>& pyramidImage0, vector<Mat>& pyramidDepth0,
+                    vector<Mat>& pyramidImage1, vector<Mat>& pyramidDepth1,
+                    vector<Mat>& pyramid_dI_dx1, vector<Mat>& pyramid_dI_dy1,
+                    vector<Mat>& pyramidTexturedMask1, vector<Mat>& pyramidCameraMatrix )
+{
+    const int pyramidMaxLevel = 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 );
+        Sobel( pyramidImage1[i], pyramid_dI_dy1[i], CV_16S, 0, 1 );
+
+        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 = (minGradMagnitudes[i] * minGradMagnitudes[i]) / (sobelScale * sobelScale);
+        for( int y = 0; y < dx.rows; y++ )
+        {
+            for( int x = 0; x < dx.cols; x++ )
+            {
+                float m2 = dx.at<short int>(y,x)*dx.at<short int>(y,x) + dy.at<short int>(y,x)*dy.at<short int>(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& Rt )
+{
+    Mat ksi;
+#ifdef HAVE_EIGEN
+    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
+
+	computeProjectiveMatrix( ksi, Rt );
+
+	return true;
+}
+
+bool cv::RGBDOdometry( cv::Mat& Rt,
+                       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, const std::vector<int>& iterCounts,
+                       const std::vector<float>& minGradientMagnitudes,
+                       float minDepth, float maxDepth, float maxDepthDiff )
+{
+    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() );
+    CV_Assert( minGradientMagnitudes.size() == iterCounts.size() );
+
+    preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth );
+    
+    vector<Mat> pyramidImage0, pyramidDepth0,
+                pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1,
+                pyramidCameraMatrix;
+    buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelScale, minGradientMagnitudes,
+                   pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1,
+                   pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix );
+
+    Mat resultRt = Mat::eye(4,4,CV_64FC1);
+    for( int level = iterCounts.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 );
+
+        Mat corresp( levelImage0.size(), levelImage0.type(), CV_32SC1 );
+
+        // Run transformation search on current level iteratively.
+        for( int iter = 0; iter < iterCounts[level]; iter ++ )
+        {
+            int correspCount = computeCorresp( levelCameraMatrix, levelCameraMatrix.inv(), resultRt.inv(DECOMP_SVD),
+                                               levelDepth0, levelDepth1, pyramidTexturedMask1[level], maxDepthDiff,
+                                               corresp );
+
+            if( correspCount == 0 )
+                break;
+
+            Mat C( correspCount, 6, CV_64FC1 );
+            Mat dI_dt( correspCount, 1, CV_64FC1 );
+
+            const double fx = levelCameraMatrix.at<double>(0,0);
+            const double fy = levelCameraMatrix.at<double>(1,1);
+            int pointCount = 0;
+            for( int v0 = 0; v0 < corresp.rows; v0++ )
+            {
+                for( int u0 = 0; u0 < corresp.cols; u0++ )
+                {
+                    if( corresp.at<int>(v0,u0) != -1 )
+                    {
+                        int u1, v1;
+                        get2shorts( corresp.at<int>(v0,u0), u1, v1 );
+
+                        computeC( (double*)C.ptr(pointCount),
+                                  sobelScale * level_dI_dx1.at<short int>(v1,u1), sobelScale * level_dI_dy1.at<short int>(v1,u1),
+                                  levelCloud0.at<Point3f>(v0,u0), fx, fy);
+
+                        dI_dt.at<double>(pointCount) = static_cast<double>(levelImage1.at<uchar>(v1,u1)) -
+                                                       static_cast<double>(levelImage0.at<uchar>(v0,u0));
+                        pointCount++;
+                    }
+                }
+            }
+
+            const double detThreshold = 1.e-6;
+            Mat currRt;
+            bool solutionExist = solveSystem( C, dI_dt, detThreshold, currRt );
+			if( !solutionExist )
+                break;
+			
+            resultRt = currRt * resultRt;
+        }
+    }
+
+    Rt = resultRt;
+
+    return !Rt.empty();
+}

samples/cpp/rgbdodometry.cpp

@@ -0,0 +1,150 @@
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+#include "opencv2/contrib/contrib.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+#include <cstdio>
+#include <iostream>
+#include <ctime>
+
+using namespace cv;
+using namespace std;
+
+static
+void cvtDepth2Cloud( const Mat& depth, Mat& cloud, const Mat& cameraMatrix )
+{
+    const float inv_fx = 1.f/cameraMatrix.at<float>(0,0);
+    const float inv_fy = 1.f/cameraMatrix.at<float>(1,1);
+    const float ox = cameraMatrix.at<float>(0,2);
+    const float oy = cameraMatrix.at<float>(1,2);
+    cloud.create( depth.size(), CV_32FC3 );
+    for( int y = 0; y < cloud.rows; y++ )
+    {
+        Point3f* cloud_ptr = (Point3f*)cloud.ptr(y);
+        const float* depth_prt = (const float*) depth.ptr(y);
+        for( int x = 0; x < cloud.cols; x++ )
+        {
+            float z = depth_prt[x];
+            cloud_ptr[x].x = (x - ox) * z * inv_fx;
+            cloud_ptr[x].y = (y - oy) * z * inv_fy;
+            cloud_ptr[x].z = z;
+        }
+    }
+}
+
+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);
+
+    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;
+            }
+        }
+    }
+}
+
+int main(int argc, char** argv)
+{
+    float vals[] = {525., 0., 3.1950000000000000e+02,
+                    0., 525., 2.3950000000000000e+02,
+                    0., 0., 1.};
+
+    const Mat cameraMatrix = Mat(3,3,CV_32FC1,vals);
+    const Mat distCoeff(1,5,CV_32FC1,Scalar(0));
+
+    if( argc != 5 )
+    {
+        cout << "Format: image0 depth0 image1 depth1" << endl;
+        cout << "Depth file must be 16U image stored depth in mm." << endl;
+        return -1;
+    }
+
+    Mat colorImage0 = imread( argv[1] );
+    Mat depth0 = imread( argv[2], -1 );
+
+    Mat colorImage1 = imread( argv[3] );
+    Mat depth1 = imread( argv[4], -1 );
+
+    if( colorImage0.empty() || depth0.empty() || colorImage1.empty() || depth1.empty() )
+    {
+        cout << "Data (rgb or depth images) is empty.";
+        return -1;
+    }
+
+    Mat grayImage0, grayImage1, depthFlt0, depthFlt1/*in meters*/;
+    cvtColor( colorImage0, grayImage0, CV_BGR2GRAY );
+    cvtColor( colorImage1, grayImage1, CV_BGR2GRAY );
+    depth0.convertTo( depthFlt0, CV_32FC1, 1./1000 );
+    depth1.convertTo( depthFlt1, CV_32FC1, 1./1000 );
+
+    TickMeter tm;
+    Mat Rt;
+
+    vector<int> iterCounts(4);
+    iterCounts[0] = 7;
+    iterCounts[1] = 7;
+    iterCounts[2] = 7;
+    iterCounts[3] = 10;
+
+    vector<float> minGradMagnitudes(4);
+    minGradMagnitudes[0] = 12;
+    minGradMagnitudes[1] = 5;
+    minGradMagnitudes[2] = 3;
+    minGradMagnitudes[3] = 1;
+
+    const float minDepth = 0; //in meters
+    const float maxDepth = 3; //in meters
+    const float maxDepthDiff = 0.07; //in meters
+
+    tm.start();
+    bool isFound = cv::RGBDOdometry( Rt, grayImage0, depthFlt0, Mat(),
+                                     grayImage1, depthFlt1, Mat(),
+                                     cameraMatrix, iterCounts, minGradMagnitudes,
+                                     minDepth, maxDepth, maxDepthDiff );
+    tm.stop();
+
+    cout << "Rt = " << Rt << endl;
+    cout << "Time = " << tm.getTimeSec() << " sec." << endl;
+
+    if( !isFound )
+    {
+        cout << "Rigid body motion cann't be estimated for given RGBD data."  << endl;
+        return -1;
+    }
+
+    Mat warpedImage0;
+    warpImage<Point3_<uchar> >( colorImage0, depthFlt0, Rt, cameraMatrix, distCoeff, warpedImage0 );
+
+    imshow( "im0", colorImage0 );
+    imshow( "warped_im0", warpedImage0 );
+    imshow( "im1", colorImage1 );
+    waitKey();
+
+    return 0;
+}