【来源】视觉SLAM十四讲，作者 高翔
三角测量是SLAM中，利用相机运动估计特征点空间位置的过程。
本节旨在解决以下问题：
在SLAM中，利用对极几何约束估计相机运动之后，我们还需要通过三角测量来估计地图点的深度。三角测量（三角化）指的是，通过在两处观测同一个点的夹角，从而确定该点的距离。
SLAM中主要用三角化来估计像素点的距离。
按照对极几何的定义，设$x_1,x_2$为两个特征点的归一化坐标，那么他们满足：
$$
s_1x_1=s_2Rx_2+t.
$$
经过对极几何之后，已得到了运动$R,t$，需要求解两个特征点的深度$s_1$,$s_2$。
两个深度可以分开算。若先算$s_2$,那么对上市两个做成一个$x_1$^,得：
该式子左侧为0，右侧可看成是$s_2$的一个方程，可以根据它直接求$s_2$。有了$s_2$,$s_1$也非常容易求出。预测就可以得到两帧下的深度，即确定了空间坐标。
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
);
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 )
{
Mat T1 = (Mat_<double> (3,4) <<
1,0,0,0,
0,1,0,0,
0,0,1,0);
Mat T2 = (Mat_<double> (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));
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point2d> 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 );
}
}
同时，在main函数中增加三角测量部分，并验证重投影关系：
int main (int argc, char∗∗ argv)
{
// .....
// 三角化
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;
// 第2幅图
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
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;
}
// ...
}
我们打印了每个空间点在两个相机坐标系下的投影坐标与像素坐标——相当于P 的投影位置与看到的特征点位置。由于误差的存在，它们会有一些微小的差异。以下是某一特征点的信息：
point in the first camera frame: [0.0844072, 0.0734976]
point projected from 3D [0.0843702, 0.0743606], d=14.9895
point in the second camera frame: [0.0431343, 0.0459876]
point reprojected from second frame: [0.04312769812378599, 0.04515455276163744, 1]
可以看到，误差的量级大约在小数点后第3 位。可以看到，三角化特征点的距离大约为15。
但由于尺度不确定性，我们并不知道这里的15 究竟是多少米。
要提高三角化的精度，主要有两种方法：
总而言之，增大平移，会导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。