PnP问题的求解方法有很多，例如，用3对点估计位姿的P3P、直接线性变换法（DLT），EPnP(Efficient PnP)，UPnP等；
非线性优化的方式，构建最小二乘问题并迭代进行求解，即万金油式的Bundle Adjustment。
本节组要介绍DLT，P3P，并给出EPnP求解位置的示例。
主要内容
1. 直接线性变换法 （P158-159）
1） 一对匹配点可以产生两个约束（尺度不确定）
2） 由于t一共有12维，最少通过6对匹配点即可实现矩阵T的线性求解。
3） 当匹配点大于6时，也可以使用SVD等方法对超定方程求最小二乘解。
4） 解是一般矩阵，需要把结果从矩阵空间重新投影到SE(3)的流行上，转换成旋转和平移两部分（QR分解）。
5） 即使内参未知，也能用PnP去估计K，R，T三个量，未知量增多，效果会差一些。
2. P3P（P159-161）
原理：利用三角形相似的性质，求解相机坐标系下归一下平面上的坐标点的深度信息，即得到相机坐标系下的3D坐标，最后把问题转化成一个3D-3D的位姿估计问题。(PnP问题转化成ICP问题)
1）需要3对匹配点+1对验证点（从可能的解中选出正确的一个）。
2）方程组求解：吴消元法（得到4组解，利用验证点进行选取）。
3）P3P存在的问题：
3.1） 只利用三个点的信息，当点数多于3时，无法利用更多的信息。
3.2）噪声或者误匹配，算法失效。
3. 更多的改进方法EPnP，UPnP
利用更多信息，迭代方式，减小噪声影响
通常做法：先使用P3P/EPnP等方法估计相机的位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）
4 代码实现
1）solvePnP使用：输入，输出参数的理解。
CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );
2）根据已经匹配好的特征点中，选取深度信息有效的点，生成3D坐标，再从第二幅图中生成对应点的像素坐标，结合内参矩阵K，调用函数进行计算。
3） 函数返回的结果为旋转向量，需要转化成旋转矩阵。（罗德里格斯公式）
参考链接
相机标定（3） opencv中solvePnPRansac()和solvePnP()计算外参数
代码
#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, cv::SOLVEPNP_EPNP); // 调用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 ); } 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 ) { //-- 初始化 Mat descriptors_1, descriptors_2; // used in OpenCV3 Ptr<FeatureDetector> detector = ORB::create(); Ptr<DescriptorExtractor> descriptor = ORB::create(); // use this if you are in OpenCV2 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); //-- 第一步:检测 Oriented FAST 角点位置 detector->detect ( img_1,keypoints_1 ); detector->detect ( img_2,keypoints_2 ); //-- 第二步:根据角点位置计算 BRIEF 描述子 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); descriptor->compute ( img_2, keypoints_2, descriptors_2 ); //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离 vector<DMatch> match; // BFMatcher matcher ( NORM_HAMMING ); matcher->match ( descriptors_1, descriptors_2, match ); //-- 第四步:匹配点对筛选 double min_dist=10000, max_dist=0; //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 for ( int i = 0; i < descriptors_1.rows; i++ ) { double dist = match[i].distance; if ( dist < min_dist ) min_dist = dist; if ( dist > max_dist ) max_dist = dist; } printf ( "-- Max dist : %f \n", max_dist ); printf ( "-- Min dist : %f \n", min_dist ); //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限. for ( int i = 0; i < descriptors_1.rows; i++ ) { if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) { matches.push_back ( match[i] ); } } } Point2d pixel2cam ( const Point2d& p, const Mat& K ) { return Point2d ( ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) ); } 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; }
结果及分析
匹配的点数，以及用EPnP最终计算的位姿参数：
3d-2d pairs: 75 R= [0.9979059095501266, -0.05091940089110203, 0.03988747043653947; 0.04981866254253317, 0.9983623157438158, 0.02812094175376488; -0.04125404886078182, -0.02607491352884363, 0.998808391202765] t= [-0.1267821389557959; -0.008439496817520795; 0.06034935748888202]
将该结果和对极几何求解出来的结果进行对比：
R两者比较接近，t存在尺度不确定的问题，两个结果存在一定的尺度（对极几何）