版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lemianli/article/details/82011504
三角测量
使用对极约束估计了相机的运动，在得到运动之后，我们需要用相机的运动估计特征点的空间位置。在单目SLAM中，仅仅通过单张图像无法获得像素深度信息，需要通过三角测量来估计其深度。
上图中，O1P
和 O2P 理论上交与P点，但是由于噪声的存在，往往无法相交。可以通过最小二乘求解。设 x1,x2
为俩个特征点的归一化坐标，那他们满足：
s2x2=s1Rx1+t
由于我们已经知道R和t，想要求解的是俩个特征点的深度s1,s2
。当然这俩个深度可以分开求，若要求s1，则左乘 x∧
，得到
s2x∧2x2=0=s1x∧2Rx1+x∧2t
扫描二维码关注公众号，回复：
4142085 查看本文章
以上方程中，只需要对右边求方程即可，求出s1
，但是考虑到噪声的问题，估计的R，t
不一定为精确值，所以更常见的是求最小二乘，而不是求零解。
以上过程，可以通过代码来实现，以下只是主函数里面的代码，显示了主要流程：
int main ( int argc, char** argv )
{
//-- 读取图像
Mat img_1 = imread ( "/home/hansry/Slam_Book/src/Test_trian/data/1.png", CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( "/home/hansry/Slam_Book/src/Test_trian/data/2.png", CV_LOAD_IMAGE_COLOR );
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
//对俩张图像，ORB特征检测，改进FAST关键点，BRIEF描述子，汉明距离（位数）进行匹配
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
//-- 通过匹配点，估计两张图像间运动，通过对极约束，计算基础矩阵、本质矩阵和单应矩阵，从而求出R，t
Mat R,t;
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
//-- 三角化测量，通过匹配点求出基于相机坐标系的空间点
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++ )
{
//通过第一张图片像素点和内参转化的归一化平面上的点
Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
//通过三角化测量求出来的点然后归一化平面
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z
);
cout<<"point in the first camera frame: "<<pt1_cam<<endl;
cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl;
// 第二个图同样的，通过在第二张图匹配点像素点进行转化得到的归一化平面上的点
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
//通过R，t求出三角化测量求出P点在第二个相机坐标系下的三维坐标值
Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t;
//三维点归一化平面上的点
pt2_trans /= pt2_trans.at<double>(2,0);
cout<<"point in the second camera frame: "<<pt2_cam<<endl;
cout<<"point reprojected from second frame: "<<pt2_trans.t()<<endl;
cout<<endl;
}
return 0;
}通过三角测量及像素上的匹配点，我们可以求出路标即P点基于第一个相机坐标系的三维坐标点，然后通过对极约束求出的R，t可以求出在第二个相机坐标系上路标P点的三维坐标值。
讨论：三角平移是由平移得到的，有平移才会有对极几何约束的三角形。因此，纯旋转是无法使用三角测量的，对极约束将永远满足。在平移时，三角测量有不确定性，会引出三角测量的矛盾。
如上图所示，当平移很小时，像素上的不确定性将导致较大的深度不确定性，平移较大时，在相同的相机分辨率下，三角化测量将更精确。三角化测量的矛盾：平移增大，会导致匹配失效，平移太小，三角化精度不够。