3D+2D位姿估计

一、读取两帧图像，并获得特征点以及匹配后的特征点

    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

二、深度图+rgb建立3D点

1. 读取第一帧的深度图

   Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );

    

2.  输入内参矩阵

   Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
   

    

3. 从图1中找到匹配后的关键点坐标（x,y），通过（x,y）匹配到图1的特征图中，获得该特征点的深度d。
   将关键点像素坐标转换为相机归一化坐标记为p1;
   保存第一帧图像的3D点（x, y, z）存入pts_3d;
   保存第二帧对应匹配点的2D点（x, y）存入pts_2d;
   此时就完成了3D-2D的配对。
   代码为：

       vector<Point3f> pts_3d;
       vector<Point2f> pts_2d;
       for ( DMatch m:matches )
       {
           ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
           if ( d == 0 )   // bad depth
               continue;
           float dd = d/5000.0;
           Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
           pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
           pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
       }

4. 通过以上3D-2D的配对点求解R,t， 该过程叫做求解PnP
   代码为：

       Mat r, t;
       solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
       Mat R;
       cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
       cout<<"R="<<endl<<R<<endl;
       cout<<"t="<<endl<<t<<endl;

   
    

5. 通过以上R,t以及3D点可以计算重投影误差，然后让重投影误差最小构建最小而成，通过g2o方法求解。得到更加准确的R,t值。
    

   bundleAdjustment ( pts_3d, pts_2d, K, R, t );