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存在尺度不確定的問題，兩個結果存在一定的尺度（對極幾何）