本文首先介绍如何使用OpenCV中的PnP求解3D-2D位姿变换，再介绍如何使用g2o对前面得出的结果进行集束调整（Bundle Adjustment，BA）。
一、3D-2D相机位姿估计：PnP
PnP（Perspective-n-Point）描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。对应到SLAM问题上，在初始化完成后，前一帧图像的特征点都已经被三角化，即已经知道了这些点的3D位置。那么新的帧到来后，通过图像匹配就可以得到与那些3D点相对应的2D点，再根据这些3D-2D的对应关系，利用PnP算法解出当前帧的相机位姿。
PnP问题有多种求解方法，包括P3P、直接线性变换（DLT）、EPnP（Efficient PnP）、UPnP等等，而且它们在OpenCV中都有提供。
下面我们就来看代码吧。
二、PnP求解相机位姿代码
这里直接使用深度图中给出的像素深度来计算3D点坐标，以简化代码。调用cv::solvePnP即可求出旋转向量和平移向量。
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/1000.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 );
}
三、Bundle Adjustment
现在，终于要对前面得出的结果做优化了。在SLAM中，优化是系统稳定运行的核心，没有优化的结果误差会越来越大，根本无法长时间运行。在本文集之前发表的文章中，曾使用Ceres和g2o对曲线拟合做非线性优化，现在我们把同样的方法用到3D-2D相机位姿估计上。这种优化方法用在SLAM上时称为集束调整（Bundle Adjustment，BA）。“集束调整”名称的含义是说，通过调整相机的姿态使3D路标点发出的光线都能汇聚到相机的光心。
回顾g2o图优化库的使用方法，将优化变量作为顶点，误差项作为边，构造一个图。在本文的BA问题中，顶点是3D点（认为深度图中的3D点有测量误差）和相机位姿，误差项是重投影误差。幸运的是，g2o特地为SLAM问题提供了许多封装好的顶点类和边类。在本文的BA问题中需要用到的3D点顶点类、相机位姿顶点类和重投影误差类，我们都可以直接使用，而不需要像曲线拟合那样自定义类以及重写误差计算方法了。
四、BA代码
下面给出BA函数的代码，它的流程就是初始化g2o求解器、定义顶点（包括位姿顶点和所有3D点顶点）、定义边、开始优化。
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 );
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提供的相机位姿顶点类VertexSE3Expmap、3D路标点类VertexSBAPointXYZ和重投影误差边类EdgeProjectXYZ2UV，这大大降低了程序的代码量。如果对这几个类的具体实现感兴趣，可以查看g2o源码。这里稍微提一下，相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，而不是使用旋转矩阵和平移矩阵。这是因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。在重投影误差边类EdgeProjectXYZ2UV中，已经为相机位姿和3D点坐标推导了雅克比矩阵，以计算迭代的增量方向。关于这部分的理论知识，强烈建议参考高翔《视觉SLAM十四讲》的第4讲李群和李代数以及第7讲视觉里程计1的第8节。
最后，完整代码见GitHub：https://github.com/jingedawang/FeatureMethod
五、参考资料
《视觉SLAM十四讲》第4讲 李群与李代数 高翔
《视觉SLAM十四讲》第7讲 视觉里程计1 高翔
Bundle adjustment Wikipedia
