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"

samples/cpp/rgbdodometry.cpp

+#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;
+}