SLAM--位姿图优化和PnP求解

一 、位姿图优化

1.1 推导

我们把相机位姿和空间点的优化称为BA 优化，这种优化在小空间、短时间内效果不错，当特征点的随时间变多时，BA的计算效率就会下降，为了保证效率，我们可以在实际过程中只对位姿进行优化。（另外如果一定需要BA优化，可以开启两个线程，一个BA线程，一个VO线程。）这里推导位姿的优化：

我们规定第$i$个相机的位姿为$T_{w,i}$，所以有：
$$\Delta T_{i,j}=T_{w,i}^{-1}\cdot T_{w,j}$$
为了简单表示，我们省略w：
$$\Delta T_{i,j}=T_i^{-1}\cdot T_j$$

指数映射到李代数：
$$\begin{gathered} \exp (\Delta {\xi }_{ij}^{\wedge })=\exp (-{\xi }_i^{\wedge })\cdot \exp ({\xi }_j^{\wedge }) \\ ⟹\Delta {\xi }_{ij}=\ln [\exp (-{\xi }_i^{\wedge })\cdot \exp ({\xi }_j^{\wedge }){]}^{\vee } \end{gathered}$$
我们定义误差函数：
$$e_{ij}({\xi }_i,{\xi }_j)=\ln [\exp (-{\xi }_{ij}^{\wedge })\exp (-{\xi }_i^{\wedge })\cdot \exp ({\xi }_j^{\wedge }){]}^{\vee }$$
这里有两个要优化的变量${\xi }_i,{\xi }_j$，由泰勒展开：
$$e_{ij}(\Delta {\xi }_i\oplus {\xi }_i,\Delta {\xi }_j\oplus {\xi }_j)\approx e_{ij}({\xi }_i,{\xi }_j)+\frac{\partial e_{ij}}{\partial [\delta {\xi }_i^T,\delta {\xi }_j^T{]}^T}\cdot [\delta {\xi }_i^T,\delta {\xi }_j^T{]}^T$$
即：
$$e_{ij}(\Delta {\xi }_i\oplus {\xi }_i,\Delta {\xi }_j\oplus {\xi }_j)\approx e_{ij}({\xi }_i,{\xi }_j)+\frac{\partial e_{ij}}{\partial \delta {\xi }_i}\delta {\xi }_i+\frac{\partial e_{ij}}{\partial \delta {\xi }_j}\delta {\xi }_j$$
这里：
$$\{\begin{array}{r}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}=-J_r^{-1}(e_{ij})\cdot Ad(T_j)\\ \frac{\partial e_{ij}}{\partial \delta {\xi }_j}=J_r^{-1}(e_{ij})\cdot Ad(T_j)\end{array}\text{\hspace{1em}(3)}$$
这里的$J_r$比较复杂，一般不会直接求解，但可以用单位矩阵近似或者：
$$J_r^{-1}\approx I+\frac{1}{2}\left[\begin{array}{cc}{\varphi }_e^{\wedge }& {\rho }_e^{\wedge }\\ 0& {\varphi }_e^{\wedge }\end{array}\right]$$

一般的，我们可以利用g2o来进行位姿优化或者利用OpenCV的pnp求解函数求解pnp问题。

 
 

1.2 利用g2o位姿图优化

首先，初始化求解器：

    //构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    //
    optimizer.setAlgorithm(algorithm);

第二，添加SE3的顶点：

g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId(index);
v->read(fin);
v->setFixed(index == 0);//固定第一个点
optimizer.addVertex(v);

第三，添加SE3–SE3的边：

g2o::EdgeSE3 *e = new g2o::EdgeSE3();
e->setId(edgeCnt++);
e->setVertex(0, optimizer.vertices()[idx1]);
e->setVertex(1, optimizer.vertices()[idx2]);
optimizer.addEdge(e);

最后，优化：

optimizer.setVerbose(true);//打开调试输出
optimizer.initializeOptimization();
optimizer.optimize(30);

1.3 位姿图优化例程

源码来源：视觉SLAM十四讲第10章------高翔：pose_graph_g2o_SE3.cpp

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>

using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> Block;  // 6x6 BlockSolver
    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); // 线性方程求解器
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // SE3 顶点
            g2o::VertexSE3 *v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            g2o::EdgeSE3 *e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");

    return 0;
}

 
 
 

二、 位姿优化或PnP的求解

2.1 推导

假设有两个位姿Pose1和Pose2，Pose1观测数据为$u_1$，相机坐标系下的数据位置$P_1$；Pose2观测数据为$u_2$，相机坐标系下的$P_2$未知。世界坐标为$P_w$，由相机的投影关系可得：
$$\begin{array}{rl}{u}_1& =\frac{1}{s_1}K{T}_{1,w}{P}_w=\frac{1}{s_1}K{P}_1\\ & \\ u_2& =\frac{1}{s_2}K{T}_{2,w}{P}_w\end{array}$$
这里为了估计从相机1到2的变换，我们需要把相机1固定，假定$P_1$就是$P_w$，相机坐标系1就是世界坐标系，所以又可以写成：
$$u_1=\frac{1}{s_1}K{P}_w$$
这时我们要估计的$T_{2,w}$就是$T_{2,1}$，也就是从1到2的变换矩阵；

设函数:
$$u_2({\xi }_{21},P_1)=\frac{1}{s_2}K\exp ({\xi }_{21}^{\wedge })\cdot P1$$
由于是位姿图优化，这里不考虑优化特征点P1的位姿，则函数可表示为：
$$u_2({\xi }_{21})=\frac{1}{s_2}K\exp ({\xi }_{21}^{\wedge })\cdot P1$$

所以在g2o实现图优化时，位姿${\xi }_{21}$就是我们要优化的顶点；$P_1$是固定的顶点，不优化；位姿 ${\xi }_{21}$到 $P_1$就是我们需要设置的边。
 
 

2.2 利用g2o优化PnP

首先，初始化求解器：

    //构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    //
    optimizer.setAlgorithm(algorithm);

第二，添加顶点g2o::VertexSE3Expmap：

//顶点
    g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(g2o::SE3Quat());
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

第三，添加边g2o::EdgeSE3ProjectXYZOnlyPose：

//边
    for(size_t i = 0;i<pts_2d_eigen.size();i++)
    {
        g2o::EdgeSE3ProjectXYZOnlyPose* edge = new g2o::EdgeSE3ProjectXYZOnlyPose();
        edge->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setMeasurement(pts_2d_eigen[i]);
        edge->fx = fx;
        edge->fy = fy;
        edge->cx = cx;
        edge->cy = cy;
        edge->Xw = Eigen::Vector3d(pts_3d_eigen[i][0],pts_3d_eigen[i][1],pts_3d_eigen[i][2]);
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        edge->setRobustKernel(rk);
        optimizer.addEdge(edge);
    }

最后，优化：

optimizer.setVerbose(false);
optimizer.initializeOptimization();
optimizer.optimize(30);

 

 

2.3 PnP位姿优化源码

参考代码：作者高翔 ：slambook/ch7/pose_estimation_3d2d.cpp
链接：https://github.com/zouyuelin/SLAM_Learning_notes/tree/main/testPnPSolve

CMakeLists:

cmake_minimum_required(VERSION 3.5)

project(testPnPSolve LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)


set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

set(CMAKE_INCLUDE_CURRENT_DIR ON)
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# OpenCV
find_package( OpenCV 4.3.0 REQUIRED)
# Eigen3
find_package( Eigen3 REQUIRED )
# 寻找G2O
find_package( G2O REQUIRED )
#Sophus
find_package( Sophus REQUIRED)
#Cholmod
find_package( Cholmod REQUIRED)

INCLUDE_DIRECTORIES(
                ${CHOLMOD_INCLUDE_DIR}
                ${OpenCV_INCLUDE_DIRS}
                ${EIGEN3_INCLUDE_DIRS}
                ${Sophus_INCLUDE_DIRS})

add_executable(testPnPSolve main.cpp)
TARGET_LINK_LIBRARIES(testPnPSolve
         ${OpenCV_LIBS}
         ${Sophus_LIBRARIES}
         ${CHOLMOD_LIBRARIES}
         g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba
         )

 message(STATUS "OPENCV is :${OpenCV_INCLUDE_DIRS}")
 message(STATUS "OPENCV version is :${OpenCV_VERSION}")
 message(STATUS "the cholmod is ${CHOLMOD_INCLUDE_DIR} ${CHOLMOD_LIBRARIES}")

main.cpp

/*
 * 本程序不对特征点的位置优化，只考虑优化相机的位姿
 * 作者：zyl
 */


#include <iostream>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>

#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>

#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>

#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/slam3d/se3quat.h>

#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/core/eigen.hpp>

#include <Eigen/Core>

#include <sophus/se3.h>

#include <vector>
#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);

//相机内参
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

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 solvePnPWithG2o(vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &pts_3d_eigen,vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> pts_2d_eigen,
              Mat &K,Sophus::SE3 &pose);

void solvePnPBA(vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &pts_3d_eigen,vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> pts_2d_eigen,
              Mat &K,Sophus::SE3 &pose);


int main(int argc,char**argv)
{
    if(argc <4)
    {
        cout<<"输入参数方法：**/testOptimizerG2o ./1.png ./2.png ./1_depth.png\n";
        return 0;
    }
    Mat img_1 = imread(argv[1],IMREAD_COLOR);
    Mat img_2 = imread(argv[2],IMREAD_COLOR);
    //深度图加载
    Mat depth_1 = imread(argv[3], IMREAD_UNCHANGED);

    vector<KeyPoint> keypoints_1, keypoints_2;
    //匹配得到的路标点
    vector<Point2f> points_1,points_2;
    vector<Point3f> points_1_3d;
    vector<DMatch> matches;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //milliseconds 毫秒
    cout<<"匹配耗时:"<<delay_time.count()<<"秒"<<endl;

    for(auto m:matches)
    {
       ushort d = depth_1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
       if (d == 0)   // 去除差的深度点
              continue;
       float dd = d / 5000.0;
       Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
       points_1_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));

       points_1.push_back(keypoints_1[m.queryIdx].pt);
       points_2.push_back(keypoints_2[m.trainIdx].pt);
    }

    //转化为Eigen,传入到G2o中
    vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> pts_3d_eigen;
    vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> pts_2d_eigen;
    for (size_t i = 0; i < points_1_3d.size(); ++i) {
        pts_3d_eigen.push_back(Eigen::Vector3d(points_1_3d[i].x, points_1_3d[i].y, points_1_3d[i].z));
        pts_2d_eigen.push_back(Eigen::Vector2d(points_2[i].x, points_2[i].y));
      }
    cout<<"特征点的数量："<<pts_3d_eigen.size()<<"  "<<pts_2d_eigen.size()<<endl;


    //*******************************G2o优化--->EdgeSE3ProjectXYZOnlyPose*******************************/
    t1 = chrono::steady_clock::now();
    Sophus::SE3 pose;
    solvePnPWithG2o(pts_3d_eigen,pts_2d_eigen,K,pose);
    t2 = chrono::steady_clock::now();
    delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp by using g2o(EdgeSE3ProjectXYZOnlyPose) time_cost: " << delay_time.count() << " seconds." << endl;
    cout << "pose estimated by g2o(EdgeSE3ProjectXYZOnlyPose) =\n" << pose.matrix() << endl;

    //*******************************G2o优化--->EdgeProjectXYZ2UV*******************************/
    t1 = chrono::steady_clock::now();
    solvePnPBA(pts_3d_eigen,pts_2d_eigen,K,pose);
    t2 = chrono::steady_clock::now();
    delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp by using g2o(EdgeProjectXYZ2UV) time_cost: " << delay_time.count() << " seconds." << endl;
    cout << "pose estimated by g2o(EdgeProjectXYZ2UV) =\n" << pose.matrix() << endl;

    //********************************opencv优化***************************/
    t1 = chrono::steady_clock::now();
    Mat r, t, R;
    cv::solvePnP(points_1_3d, points_2, K, Mat(), r, t, false);     // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Rodrigues(r, R);                                            // r为旋转向量形式，用Rodrigues公式转换为矩阵
    t2 = chrono::steady_clock::now();
    delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve pnp in opencv time_cost: " << delay_time.count() << " seconds." << endl;
    Eigen::Matrix3d R_eigen;
    Eigen::Vector3d t_eigen;
    cv::cv2eigen(R,R_eigen);
    cv::cv2eigen(t,t_eigen);
    pose = Sophus::SE3(R_eigen,t_eigen);
    cout << "pose estimated by opencv =\n" << pose.matrix() << endl;

    //*********输出匹配图3s*************//
    Mat img_keypoints;
    drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_keypoints);
    imshow("matches",img_keypoints);
    waitKey(3000);
    return 0;
}

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;

    //创建ORB检测
    Ptr<Feature2D> detector_ORB = ORB::create();

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置 计算 BRIEF 描述子
    detector_ORB->detect(img_1,keypoints_1);
    detector_ORB->detect(img_2,keypoints_2);

    detector_ORB->compute(img_1,keypoints_1,descriptors_1);
    detector_ORB->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;
    }

    cout<<"-- Max dist : "<< max_dist<<endl;
    cout<<"-- Min dist : "<< min_dist<<endl;


  for (int i = 0; i < descriptors_1.rows; i++)
  {
    if (match[i].distance <= 0.4*max_dist)
    {
      matches.push_back(match[i]);
    }
  }

}

void solvePnPWithG2o(vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &pts_3d_eigen,vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> pts_2d_eigen,
              Mat &K,Sophus::SE3 &pose)
{
    double fx = K.at<double>(0, 0);
    double fy = K.at<double>(1, 1);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);

    //构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    //
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);

    //顶点
    g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(g2o::SE3Quat());
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

    //边
    for(size_t i = 0;i<pts_2d_eigen.size();i++)
    {
        g2o::EdgeSE3ProjectXYZOnlyPose* edge = new g2o::EdgeSE3ProjectXYZOnlyPose();
        edge->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setMeasurement(pts_2d_eigen[i]);
        edge->fx = fx;
        edge->fy = fy;
        edge->cx = cx;
        edge->cy = cy;
        edge->Xw = Eigen::Vector3d(pts_3d_eigen[i][0],pts_3d_eigen[i][1],pts_3d_eigen[i][2]);
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        edge->setRobustKernel(rk);
        optimizer.addEdge(edge);
    }

    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    pose = Sophus::SE3(vSE3->estimate().rotation(),vSE3->estimate().translation());
}

void solvePnPBA(vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &pts_3d_eigen,vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> pts_2d_eigen,
              Mat &K,Sophus::SE3 &pose)
{
    //****************************BA优化过程*********************
    //构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    //
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);


    //添加位姿顶点

    g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap;
    v->setId(0);
    v->setFixed(false);
    v->setEstimate(g2o::SE3Quat());
    optimizer.addVertex(v);

    //添加特征点顶点
    for(int i=1;i<pts_3d_eigen.size();i++)
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId(i); //已经添加过两个位姿的顶点了
        v->setEstimate(pts_3d_eigen[i]);
        v->setFixed(true); //固定，不优化
        v->setMarginalized(true);//把矩阵块分成两个部分，分别求解微量
        optimizer.addVertex(v);
    }

    //添加相机参数
    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);

    //添加边,第一帧和第二帧
    for(size_t i = 1;i<pts_3d_eigen.size();i++)
    {

        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i)));
        edge->setVertex(1,v);
        edge->setMeasurement(pts_2d_eigen[i]);
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0,0);//这句必要
        optimizer.addEdge(edge);
    }

    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    pose = Sophus::SE3(v->estimate().rotation(),v->estimate().translation());

    //****************************BA优化过程*********************
}

运行结果：

-- Max dist : 94
-- Min dist : 4
匹配耗时:0.122066秒
特征点的数量：120  120
solve pnp by using g2o(EdgeSE3ProjectXYZOnlyPose) time_cost: 0.000715604 seconds.
pose estimated by g2o(EdgeSE3ProjectXYZOnlyPose) =
   0.997824  -0.0509852   0.0418087   -0.130716
  0.0498885    0.998393   0.0268684 -0.00658621
 -0.0431114  -0.0247242    0.998764   0.0633622
          0           0           0           1
solve pnp by using g2o(EdgeProjectXYZ2UV) time_cost: 0.000613693 seconds.
pose estimated by g2o(EdgeProjectXYZ2UV) =
   0.997841  -0.0509975   0.0413823   -0.130026
  0.0499196    0.998397    0.026676 -0.00629541
 -0.0426763  -0.0245527    0.998787   0.0633322
          0           0           0           1
solve pnp in opencv time_cost: 0.000365883 seconds.
pose estimated by opencv =
    0.99798  -0.0521689   0.0362579   -0.122198
  0.0512588    0.998357   0.0255927 -0.00461646
 -0.0375335  -0.0236824    0.999015   0.0641551
          0           0           0           1

 
 

参考

1.视觉SLAM十四讲----高翔

2.ORB_SLAM2源码