samples/cpp/rgbdodometry.cpp

   1 #include "opencv2/imgproc/imgproc.hpp"
   2 #include "opencv2/calib3d/calib3d.hpp"
   3 #include "opencv2/contrib/contrib.hpp"
   4 #include "opencv2/highgui/highgui.hpp"
   5
   6 #include <cstdio>
   7 #include <iostream>
   8 #include <ctime>
   9
  10 using namespace cv;
  11 using namespace std;
  12
  13 static
  14 void cvtDepth2Cloud( const Mat& depth, Mat& cloud, const Mat& cameraMatrix )
  15 {
  16     const float inv_fx = 1.f/cameraMatrix.at<float>(0,0);
  17     const float inv_fy = 1.f/cameraMatrix.at<float>(1,1);
  18     const float ox = cameraMatrix.at<float>(0,2);
  19     const float oy = cameraMatrix.at<float>(1,2);
  20     cloud.create( depth.size(), CV_32FC3 );
  21     for( int y = 0; y < cloud.rows; y++ )
  22     {
  23         Point3f* cloud_ptr = (Point3f*)cloud.ptr(y);
  24         const float* depth_prt = (const float*) depth.ptr(y);
  25         for( int x = 0; x < cloud.cols; x++ )
  26         {
  27             float z = depth_prt[x];
  28             cloud_ptr[x].x = (x - ox) * z * inv_fx;
  29             cloud_ptr[x].y = (y - oy) * z * inv_fy;
  30             cloud_ptr[x].z = z;
  31         }
  32     }
  33 }
  34
  35 template<class ImageElemType>
  36 static void warpImage( const Mat& image, const Mat& depth,
  37                        const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
  38                        Mat& warpedImage )
  39 {
  40     const Rect rect = Rect(0, 0, image.cols, image.rows);
  41
  42     vector<Point2f> points2d;
  43     Mat cloud, transformedCloud;
  44
  45     cvtDepth2Cloud( depth, cloud, cameraMatrix );
  46     perspectiveTransform( cloud, transformedCloud, Rt );
  47     projectPoints( transformedCloud.reshape(3,1), Mat::eye(3,3,CV_64FC1), Mat::zeros(3,1,CV_64FC1), cameraMatrix, distCoeff, points2d );
  48
  49     Mat pointsPositions( points2d );
  50     pointsPositions = pointsPositions.reshape( 2, image.rows );
  51
  52     warpedImage.create( image.size(), image.type() );
  53     warpedImage = Scalar::all(0);
  54
  55     Mat zBuffer( image.size(), CV_32FC1, FLT_MAX );
  56     for( int y = 0; y < image.rows; y++ )
  57     {
  58         for( int x = 0; x < image.cols; x++ )
  59         {
  60             const Point3f p3d = transformedCloud.at<Point3f>(y,x);
  61             const Point p2d = pointsPositions.at<Point2f>(y,x);
  62             if( !cvIsNaN(cloud.at<Point3f>(y,x).z) && cloud.at<Point3f>(y,x).z > 0 &&
  63                 rect.contains(p2d) && zBuffer.at<float>(p2d) > p3d.z )
  64             {
  65                 warpedImage.at<ImageElemType>(p2d) = image.at<ImageElemType>(y,x);
  66                 zBuffer.at<float>(p2d) = p3d.z;
  67             }
  68         }
  69     }
  70 }
  71
  72 int main(int argc, char** argv)
  73 {
  74     float vals[] = {525., 0., 3.1950000000000000e+02,
  75                     0., 525., 2.3950000000000000e+02,
  76                     0., 0., 1.};
  77
  78     const Mat cameraMatrix = Mat(3,3,CV_32FC1,vals);
  79     const Mat distCoeff(1,5,CV_32FC1,Scalar(0));
  80
  81     if( argc != 5 && argc != 6 )
  82     {
  83         cout << "Format: image0 depth0 image1 depth1 [transformationType]" << endl;
  84         cout << "Depth file must be 16U image stored depth in mm." << endl;
  85         cout << "Transformation types:" << endl;
  86         cout << "   -rbm - rigid body motion (default)" << endl;
  87         cout << "   -r   - rotation rotation only" << endl;
  88         cout << "   -t   - translation only" << endl;
  89         return -1;
  90     }
  91
  92     Mat colorImage0 = imread( argv[1] );
  93     Mat depth0 = imread( argv[2], -1 );
  94
  95     Mat colorImage1 = imread( argv[3] );
  96     Mat depth1 = imread( argv[4], -1 );
  97
  98     if( colorImage0.empty() || depth0.empty() || colorImage1.empty() || depth1.empty() )
  99     {
 100         cout << "Data (rgb or depth images) is empty.";
 101         return -1;
 102     }
 103
 104     int transformationType = RIGID_BODY_MOTION;
 105     if( argc == 6 )
 106     {
 107         string ttype = argv[5];
 108         if( ttype == "-rbm" )
 109         {
 110             transformationType = RIGID_BODY_MOTION;
 111         }
 112         else if ( ttype == "-r")
 113         {
 114             transformationType = ROTATION;
 115         }
 116         else if ( ttype == "-t")
 117         {
 118             transformationType = TRANSLATION;
 119         }
 120         else
 121         {
 122             cout << "Unsupported transformation type." << endl;
 123             return -1;
 124         }
 125     }
 126
 127     Mat grayImage0, grayImage1, depthFlt0, depthFlt1/*in meters*/;
 128     cvtColor( colorImage0, grayImage0, COLOR_BGR2GRAY );
 129     cvtColor( colorImage1, grayImage1, COLOR_BGR2GRAY );
 130     depth0.convertTo( depthFlt0, CV_32FC1, 1./1000 );
 131     depth1.convertTo( depthFlt1, CV_32FC1, 1./1000 );
 132
 133     TickMeter tm;
 134     Mat Rt;
 135
 136     vector<int> iterCounts(4);
 137     iterCounts[0] = 7;
 138     iterCounts[1] = 7;
 139     iterCounts[2] = 7;
 140     iterCounts[3] = 10;
 141
 142     vector<float> minGradMagnitudes(4);
 143     minGradMagnitudes[0] = 12;
 144     minGradMagnitudes[1] = 5;
 145     minGradMagnitudes[2] = 3;
 146     minGradMagnitudes[3] = 1;
 147
 148     const float minDepth = 0.f; //in meters
 149     const float maxDepth = 4.f; //in meters
 150     const float maxDepthDiff = 0.07f; //in meters
 151
 152     tm.start();
 153     bool isFound = cv::RGBDOdometry( Rt, Mat(),
 154                                      grayImage0, depthFlt0, Mat(),
 155                                      grayImage1, depthFlt1, Mat(),
 156                                      cameraMatrix, minDepth, maxDepth, maxDepthDiff,
 157                                      iterCounts, minGradMagnitudes, transformationType );
 158     tm.stop();
 159
 160     cout << "Rt = " << Rt << endl;
 161     cout << "Time = " << tm.getTimeSec() << " sec." << endl;
 162
 163     if( !isFound )
 164     {
 165         cout << "Rigid body motion cann't be estimated for given RGBD data."  << endl;
 166         return -1;
 167     }
 168
 169     Mat warpedImage0;
 170     warpImage<Point3_<uchar> >( colorImage0, depthFlt0, Rt, cameraMatrix, distCoeff, warpedImage0 );
 171
 172     imshow( "image0", colorImage0 );
 173     imshow( "warped_image0", warpedImage0 );
 174     imshow( "image1", colorImage1 );
 175     waitKey();
 176
 177     return 0;
 178 }