三角测量并验证三角化点与特征点的重投影关系

三角测量并验证三角化点与特征点的重投影关系

首先根据  求出R， t;

然后根据  求出 s1, s2 两帧图像里的Z坐标值，代表深度

 

一. 输入参数为  keypoints_1, keypoints_2, matches, R, t

    输出参数为 points->point(x, y, z)

    代码为：

triangulation( keypoints_1, keypoints_2, matches, R, t, points );

二. 

1.  T1(E, 0)3行4列； T2(R, t) 3行4列；K相机内参矩阵
2.  将两帧图向中匹配好的特征点的像素坐标转换为相机归一化坐标
   代码为：

       vector<Point2f> pts_1, pts_2;
       for ( DMatch m:matches )
       {
           // 将像素坐标转换至相机坐标
           pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) );
           pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) );
       }

3. 输入数据 ：T1, T2, pts_1, pts_2,
   输出数据：pts_4d
   cv::triangulatePoints函数接受的参数是两个相机位姿和特征点在两个相机坐标系下的坐标，输出三角化后的特征点的3D世界坐标(也可能是第一帧相机坐标下的3D,不太确定，因为第一帧的时候，像极坐标系和世界坐标系重合)。
   代码为：

       Mat pts_4d;
       cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );

   此时的pts_4d为齐次坐标，将其转换为其次坐标，为每一个数都除以第4个元素，然后取出前3个元素。
   代码为：

       // 转换成非齐次坐标
       for ( int i=0; i<pts_4d.cols; i++ )
       {
           Mat x = pts_4d.col(i);
           x /= x.at<float>(3,0); // 归一化
           Point3d p (
               x.at<float>(0,0), 
               x.at<float>(1,0), 
               x.at<float>(2,0) 
           );
           points.push_back( p );
       }

    

4. 得到points(x, y, z)3D坐标

三、计算重投影误差

1.  将第一帧像素坐标转化为相机归一化坐标
2.  将points(x, y, z)3D坐标转化为，第一帧相机归一化坐标
   代码为：

           // Pc归一化坐标->由像素坐标和K获得
           Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
           // Pc归一化坐标->由Poind3d坐标除以z获得
           Point2d pt1_cam_3d(
               points[i].x/points[i].z, 
               points[i].y/points[i].z 
           );

3.  将第二帧像素坐标转化为相机归一化坐标

   Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );

   
   （以上1， 2， 3都不是重点，只是作为对比，第4步才是真正的重投影过程）
    

4.  通过方程 对第一帧3D坐标进行重投影，得到第二帧3D坐标
   代码为：

   Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t;

    

 通过4中计算得到的第二帧3D点，除以z得到第二帧相机归一化坐标

pt2_trans /= pt2_trans.at<double>(2,0);