高翔Slambook第七讲代码解读（3d-2d位姿估计）
高翔Slambook第七讲代码解读（3d-2d位姿估计）
上回咱们读完了pose_estimation_2d2d.cpp这个文件，基本上明白了通过对极几何计算相机位姿变换的过程，简单地说就是：你给我两帧图像，我给你算个R、t。
我们按部就班，跟着小绿来看一下接下来要读的程序——pose_estimation_3d2d。
这里小绿简单的拿两张图来看一下2d-2d与3d-2d在本质上的区别：
↑两张平面图
↑一张平面图+一张深度图 与一张平面图
这个程序，顾名思义，便是已知一帧图像中特征点的3d位置信息，以及另一帧图像中特征点的2d位置信息，进行相机的位姿变换计算。其中，3d位置信息是指该特征点所对应的真实物体点，在当前相机坐标系下的坐标；2d位置信息则是特征点的像素坐标。这里3d位置信息是由RGB-D相机提供的深度信息进行计算得到的。
我们来看一下子函数声明和主函数：
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
using namespace std;
using namespace cv;
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 );
// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
void bundleAdjustment (
const vector<Point3f> points_3d,
const vector<Point2f> points_2d,
const Mat& K,
Mat& R, Mat& t
);
int main ( int argc, char** argv )
{
if ( argc != 5 )
{
cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<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;
// 建立3D点
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数，单通道图像
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0;
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}
cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
Mat r, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
cout<<"R="<<endl<<R<<endl;
cout<<"t="<<endl<<t<<endl;
cout<<"calling bundle adjustment"<<endl;
bundleAdjustment ( pts_3d, pts_2d, K, R, t );
return 0;
}可以看到一共使用了三个子函数：find_feature_matches、pixel2cam和bundleAdjustment。这里引用白白的一张图：
图中的BA指的就是Bundle Adjustment（光束平差法，即最小化重投影误差），在这里针对本程序则封装为一个函数进行调用。在这三个子函数中，find_feature_matches即特征点匹配，用来匹配两帧图像中的特征点；pixel2cam即像素坐标到归一化平面坐标变换，用来转换坐标：这两杆数是我们研读过的，在此不做赘述。只有第三个子函数bundleAdjustment是个新面孔，的确，他是3d-2d位姿估计中的重点。bundleAdjustment函数无返回值，形参为存储Point3f类对象的容器points_3d、存储Point2f类对象的容器points_2d、Mat类矩阵K、R和t。通过const限定符可以推算该函数是要修改引用调用的R和t，即通过一组点的3d坐标、一组点的2d坐标求取相机位姿变换。我们先来看看主函数，最后再对bundleAdjustment进行梳理。
主函数
if ( argc != 5 )
{
cout<<"usage:pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
return 1;
}这里表示需要额外传入四个参数：img1、img2、depth1和depth2。其实在3d-2d匹配过程中，我们只需要前一帧的深度信息，因此可以将argc的判断改为4，不再传入depth2也是可以的。
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );读入了argv[3]对应的参数，即depth_1.png，前一帧的深度信息图，调用cv::imread存入一个480*640的Mat类变量d1。
// 建立3D点
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushortd = d1.ptr<unsigned short>(int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd,p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}首先定义了两个容器，分别用于存储点的3d坐标和2d坐标，故容器内元素类型分别为Point3f和Point2f。注意，这里将容器命名为pts_3d与pts_2d并不是说其坐标值是double类型的，而是3-dimention与2-dimention。
那么在接下来的循环中，我们就是要将matches中每一对点的坐标分别存入3d容器与2d容器中去。这里我们需要使用前一帧图像中特征点的深度信息，因此需要在深度信息矩阵d中提取出来该特征点所对应的深度信息，于是定义了一个无符号整型变量d，并进行了如下初始化：
ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];我们分离出其原型：
ushort d = d1.ptr<unsignedshort> ( row )[ cloumn ];其中使用.ptr函数访问Mat类对象d1的第row行首地址，[ column ]表示本行的第column个对象，整体来看就是获取了d1内第row行第column列的元素的值，存储为uchar类型。注意，图像中第m行第n列的数据（即像素坐标为(m,n)）存储在Mat类对象中，其数据将位于第n行第m列，因此比方说我们要看看像素坐标为(0,1)的灰度值，就需要去找一下灰度矩阵第2行第1列的值，即img.ptr<uchar>(1)[0]。在写程序时不要写反行和列，因为编译时不会报错，但得到的结果会出错，甚至产生越界。
其实这里我们也可以用上一期所学的，使用.at()函数来访问Mat类对象内部的值：
ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];其结果是一样的（注意keypoints内存储的特征点横纵坐标虽然是像素坐标，为整数，但仍是float类型的，需要强转为int类型）。
float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );在得到深度信息后，我们将其除以一个常数方便计算（取5000也可能是内定的，这个数可以改，成反比例影响t的求取，对R的结果无影响）。进而，将需要计算3d坐标点（前一帧）的特征点的像素坐标转化为归一化平面坐标，并结合深度信息计算相机坐标系下的坐标：
最终存于Point3f类的容器pts_3d中。
// 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat r, t;
solvePnP (pts_3d, pts_2d, K, Mat(), r, t, false );
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵这里神奇的事情出现了：我们调用OpenCV提供的solvePnP函数（并结合罗德里格斯变换），直接求出了旋转矩阵R和平移向量t。好了，大功告成，3d-2d位姿求取拿pnp就直接搞定了。然而，只使用pnp解算出的R、t往往具有较大误差，只能作为估计值，实际应用中还需要构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）（高翔Slambook原话）。下面我们来看一下这个最关键的子函数bundleAdjustment。
bundleAdjustment
void bundleAdjustment (
const vector< Point3f > points_3d,
const vector< Point2f > points_2d,
const Mat& K,
Mat& R, Mat& t )
{
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
// pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
// 线性方程求解器
// Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
// g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
// vertex
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
R_mat,
Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
) );
optimizer.addVertex ( pose );
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
optimizer.addVertex ( point );
}
// parameter: camera intrinsics
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
camera->setId ( 0 );
optimizer.addParameter ( camera );
// edges
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;
cout<<endl<<"after optimization:"<<endl;
cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}这里需要注意，由于g2o版本改动，高翔的代码会报错（我给注释上了），按照网上的方案修改后运行无误。
将节点定义为李代数形式的第二帧相机位姿，与所有特征点的空间位置；将边定义为每个3D点在第二个相机中的投影。位姿使用李代数形式，为6自由度；空间坐标点为3自由度，因而参数为6、3。这里将参数块求解器重命名为Block（也有的程序将其命名为BlockSlover_6_3等等）。
迭代方式选择列文伯格马夸尔特（LM法），将之前使用PnP求解出的R、t进行传入后，在此作为BundleAdjustment的迭代初值。进而设置好图模型中的节点与边，打开求解器，并指定迭代次数上限为100次，开始迭代求解。最后解出优化后的变换矩阵T。
其实关于使用g2o的BA模块，小绿实在难以深入阐述甚至理解其中的很多代码。包括3d-2d中使用的BA模块，以及下一个.cpp中3d-3d的BA模块，小绿认为完全可以在定义好所需要的类后（视情况需要），将BA模块作为一个函数封装进行调用，即输入给定的3d或2d坐标和相机内参（视情况需要），以及迭代初值R、t，就可以按照已经设定好的图模型进行图优化处理。
本程序在传入R、t之后，虽然没有加const限定符并使用引用调用&，但事实上没有修改（优化）R、t的值，而是直接cout了优化后的变换矩阵T。我们来看一下程序的运行结果：
对比T的左上方3×3矩阵，与PnP直接求得的R，我们可以发现差距并不大；对比T右侧3×1矩阵，与PnP直接求得的t，我们发现差距也不大。但毕竟是经过了一个最小二乘优化问题的求解，既然有些微小的变化，我们宁愿相信BA是起着优化R、t求解的作用的。
好了，本次的3d-2d位姿求解程序解读就这样草草收尾（g2o看不懂，小绿强行将其黑箱化）。在实际的SLAM工程中，优化部分占着极大的比重，不仅因为其计算量巨大，更在于定位、建图的精确程度基本由后端优化的精良所决定。使用图优化理论进行非线性优化其实特别重要，因此g2o库的使用、图模型的建立、边与节点的定义、求解器的定义与初始化与具体使用等细节操作...等等等等，这些小绿都需要日后完善。
最后，希望在SLAM道路上的各位敢于攻坚。共勉。
相关阅读