VSLAM算法（二）：PnP求解 3D-2D 相机位姿及 BA优化算法

BA优化模型如下：优化变量（空间位置P和相机位姿），边（P在相机平面的投影像素坐标）

             

 // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false );
 
 cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

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>(); // 线性方程求解器
	std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());

	//Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
	//std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
	std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));	   // 矩阵块求解器

	//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(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 );
	//添加3D路标点
    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;
}

PnP 函数原型：

void solvePnP(InputArray objectPoints,
              InputArray imagePoints,
              InputArray cameraMatrix, 
              InputArray distCoeffs,
              OutputArray rvec, 
              OutputArray tvec,
              bool useExtrinsicGuess=false, 
              int flags = CV_ITERATIVE
             )
参数解释：
objectPoints：世界坐标系中的3D点坐标，单位mm
imagePoints：图像坐标系中点的坐标，单位像素
cameraMatrix：相机内参矩阵
distCoeffs：畸变系数
rvec：旋转矩阵——》是向量形式，需用Rodrigues转换成旋转矩阵形式
tvec：平移矩阵
useExtrinsicGuess：是否输出平移矩阵和旋转矩阵，默认为false
flags：SOLVEPNP _ITERATIVE、SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP
----------------------------------------------------------------------------------------
objectPoints - 世界坐标系下的控制点的坐标，vector<Point3f>的数据类型在这里可以使用
imagePoints - 在图像坐标系下对应的控制点的坐标。vector<Point2f>在这里可以使用
cameraMatrix - 相机的内参矩阵
distCoeffs - 相机的畸变系数
以上两个参数通过相机标定可以得到。相机的内参数的标定参见：http://www.cnblogs.com/star91/p/6012425.html
rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
flags - 默认使用CV_ITERATIV迭代法

作者：喻茸sophie
链接：https://www.jianshu.com/p/b97406d8833c

ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
/*
  * 这个m的类型:DMatch
  * DMatch主要用来储存匹配信息的结构体，query是要匹配的描述子，train是被匹配的描述子；
  * 在Opencv中进行匹配时
  * void DescriptorMatcher::match( const Mat& queryDescriptors,
                                   const Mat& trainDescriptors, 
                                   vector<DMatch>& matches, 
                                   const Mat& mask 
                                 ) const
  * match函数的参数中位置在前面的为query descriptor，后面的是 train descriptor
  * 例如：query descriptor的数目为20，train descriptor数目为30，则DescriptorMatcher::match后的vector<DMatch>的size为20
  * 若反过来，则vector<DMatch>的size为30
 */ 
struct CV_EXPORTS_W_SIMPLE DMatch 
{ 
    //默认构造函数，FLT_MAX是无穷大 
    //#define FLT_MAX         3.402823466e+38F        /* max value */ 
    CV_WRAP DMatch() : queryIdx(-1), trainIdx(-1), imgIdx(-1), distance(FLT_MAX) {} 
    //DMatch构造函数 
    CV_WRAP DMatch( int _queryIdx, int _trainIdx, float _distance ) : 
            queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(-1), distance(_distance) {} 
    //DMatch构造函数 
    CV_WRAP DMatch( int _queryIdx, int _trainIdx, int _imgIdx, float _distance ) : 
            queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(_imgIdx), distance(_distance) {} 

    //queryIdx为query描述子的索引，match函数中前面的那个描述子 
    CV_PROP_RW int queryIdx; // query descriptor index 
    //trainIdx为train描述子的索引，match函数中后面的那个描述子 
    CV_PROP_RW int trainIdx; // train descriptor index 
    //imgIdx为进行匹配图像的索引 
    //例如已知一幅图像的sift描述子，与其他十幅图像的描述子进行匹配，找最相似的图像，则imgIdx此时就有用了。 
    CV_PROP_RW int imgIdx;   // train image index 
    //distance为两个描述子之间的距离 
    CV_PROP_RW float distance; 
    //DMatch比较运算符重载，比较的是DMatch中的distance，小于为true，否则为false 
    // less is better 
    bool operator<( const DMatch &m ) const 
    { 
        return distance < m.distance; 
    } 
};  
原文：https://blog.csdn.net/robinhjwy/article/details/77801950