高翔Slambook第七讲代码解读（三角测量）
在前面几期中，小绿简单的解读了第七讲的几个程序，运行这些程序或调用这些程序包装成的函数可以实现：
两帧图像中特征点的寻找与匹配；
两帧图像所对应的相机位姿变化的求解，包括2d-2d、3d-2d、3d-3d三种求解模式。
现在我们回过头来解读第七讲最后一个程序：triangulation.cpp。这个程序，顾名思义是进行三角化处理，具体来讲则是在求解相机位姿变化R、t后，利用这个位姿变化反过来去求取特征点在空间中的具体位置，表现为在相机坐标系下的3d坐标。我们先来看一下子函数声明：
void find_feature_matches (
const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches );
void pose_estimation_2d2d (
const std::vector<KeyPoint>& keypoints_1,
const std::vector<KeyPoint>& keypoints_2,
const std::vector< DMatch >& matches,
Mat& R, Mat& t );
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
);
// 像素坐标转相机归一化坐标
Point2f pixel2cam( const Point2d& p, const Mat& K );
其中，find_feature_matches这个函数已经是老生常谈，功能是用来求取两帧图像中的特征点并进行特征点匹配；pose_estimation_2d2d函数用来以2d-2d方式求取相机位姿变化（因为只有不知道特征点的3d信息时才需要三角测量进行深度值的计算）；pixel2cam函数用来将特征点的像素坐标转换成归一化平面坐标。这里，只有triangulation函数是一个新面孔，在这里阅读以下形参信息，可以预测其功能是通过已知的特征点2d坐标与特征点配对信息，以及刚求解得到的相机位姿变化，来求取特征点的3d坐标。这里3d坐标是在前一帧还是当前帧的相机坐标系下的，咱们过会读代码可以做出确定。先来看一下主函数：
int main ( int argc, char** argv )
{
if ( argc != 3 )
{
cout<<"usage: triangulation img1 img2"<<endl;
return 1;
}
//-- 读取图像
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;
//-- 估计两张图像间运动
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 );
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;
}
主函数中前半部分代码依旧是读取图像、计算特征点并进行匹配、进而通过对极几何求取了相机位姿变化R、t，这时调用triangulation函数进行三角测量，求取的特征点3d坐标存储于Point3d类容器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 );
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;
}前后两帧图像对应两种不同的验证方式：
前一帧图像，先将特征点的2d坐标投影到归一化平面坐标，再将三角化得到的3d坐标除以其深度信息来计算其归一化坐标（这里可以看出来三角化处理得到的3d坐标是位于前一帧相机坐标系下的），并进行对比；
后一帧图像，同样是先将特征点的2d坐标投影到归一化平面坐标，再将前一帧相机坐标系下的3d点进行R、t位姿变换，计算出特征点在当前帧相机坐标系下的坐标，再除以其深度值来计算归一化坐标，进而进行比较。（79对特征点对共比较了79次，在结果展示时我会截取一部分。）
接下来我们来看一下子函数triangulation的定义。
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_<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)
);
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 );
}
}首先，将平移矩阵t放到旋转矩阵R右侧，增广成3×4的变换矩阵，这里这个变换矩阵更具体来讲为projection matrix（投影矩阵）；进而使用pixel2cam将两组2d特征点的像素坐标转化成归一化平面坐标；最后，调用OpenCV提供的三角化处理函数triangulatePoints:
Mat pts_4d;
cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );这里提供一下OpenCV中的函数声明：
void triangulatePoints(
InputArray projMatr1,
InputArray projMatr2,
InputArray projPoints1,
InputArray projPoints2,
OutputArray points4D
);其中，由于以前一帧为参考，则前一帧到前一帧本身的投影矩阵projMatr1为3×3的单位阵与三维零列向量构成的增广阵；当前帧到参考帧（前一帧）的投影矩阵projMatr2为R和t的增广。该函数无返回值，但会修改并存储一个4×n的Mat类矩阵points4d（本函数中为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 );
}最后，通过循环，逐列提取pts_4d中每个特征点经过三角化计算得到的齐次坐标，由于其具有尺度不变性，需要通过归一化处理，最后取其前三维并压入存储特征点3d坐标的容器points。我们来看一下程序运行结果（为了简洁小绿剔除了一些输出信息）：
可以看出通过三角测量解算出的3d坐标点，分别在前一帧与当前帧中投影得到的归一化坐标，与原始特征点2d坐标根据相机内参解算出的归一化坐标相差很小，误差在小数点后3位左右。
好了，到此为止高翔Slambook第七讲的代码小绿已经和大家全部概览了一遍，希望能在巩固C++基础的同时，对SLAM系统中一些基本功能的实现有一个初步的了解和理解。比如，在寻找特征点时可以调一下find_feature_matches，在进行相应的位姿求解时可以参考2d-2d、3d-2d、3d-3d求解模式，以及后续的非线性优化中借鉴其中的g2o图优化模块等等。
小绿将会继续带大家阅读Slambook中的代码，希望大家能够不离不弃。谢谢！