三角测量

7.5 三角测量

• 上节通过对极几何约束获得了相机的运动。
  本节运用相机运动获得特征点空间位置。

• 通过单张图像无法获得图像的深度信息。用三角测量估计深度（比较容易理解）

• 红线是O1相机的像素p1对应的所有可能点。蓝线是O2相机像素p2对应的所有可能点。两像素在照片上是同一位置。因此理论上应该交于一点，这个点就是P的空间位置。
  
  由于噪声影响，两个点无法相交，因此求一个最可能的解。
  经过一系列操作解出 X1和X2 。
  书中X1 和X2 为特征点的归一化坐标，是一个列向量。
  
  

程序：

程序中给出了一个新的函数，triangulation。
函数理解：
输入特征点，匹配向量，输出每两个匹配特征点对应的3D坐标。

void triangulation ( 
    const vector< KeyPoint >& keypoint_1, 
    const vector< KeyPoint >& keypoint_2, 
    const std::vector< DMatch >& matches,
    const Mat& R, const Mat& t, 
    vector< Point3d >& points )
    // 参数包括关键点，匹配向量，矩阵R t
{
    Mat T1 = (Mat_<float> (3,4) <<
        1,0,0,0,
        0,1,0,0,
        0,0,1,0);
    Mat T2 = (Mat_<float> (3,4) <<
        R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
        R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
        R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0)
    );
	 这儿的T1 和 T2 如果说是转移矩阵的话应该是 4*4 的。所以不是
	 看程序后面有个opencv函数    cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );  这个函数的输入是3*4 的。

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );	 相机内参
    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) );
    }
    
    Mat pts_4d;
    cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
    
    // 转换成非齐次坐标
    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 );
    }
}

在主函数中应用：

//-- 三角化
vector<Point3d> points;
triangulation( keypoints_1, keypoints_2, matches, R, t, points );

//-- 验证三角化点与特征点的重投影关系
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
for ( int i=0; i<matches.size(); i++ ) 	
	
// 对每个匹配点进行验证。 输出每张图的每个匹配像素在图中的位置和从计算出的P点投影到图像中的位置。
// 若没有噪声，两个点应该重合。