目录

  • 一 、位姿图优化
    • 1.1 推导
    • 1.2 利用g2o位姿图优化
    • 1.3 位姿图优化例程
  • 二、 位姿优化或PnP的求解
    • 2.1 推导
    • 2.2 利用g2o优化PnP
    • 2.3 PnP位姿优化源码
  • 参考

一 、位姿图优化

1.1 推导

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

我们规定第iii个相机的位姿为Tw,iT_{w,i}Tw,i​，所以有：
ΔTi,j=Tw,i−1⋅Tw,j\Delta T_{i,j}=T_{w,i}^{-1} \cdot T_{w,j} ΔTi,j​=Tw,i−1​⋅Tw,j​
为了简单表示，我们省略w：
ΔTi,j=Ti−1⋅Tj\Delta T_{i,j}=T_{i}^{-1} \cdot T_{j} ΔTi,j​=Ti−1​⋅Tj​

指数映射到李代数：
exp⁡(Δξij∧)=exp⁡(−ξi∧)⋅exp⁡(ξj∧)⟹Δξij=ln⁡[exp⁡(−ξi∧)⋅exp⁡(ξj∧)]∨\exp(\Delta \xi_{ij}^\land) = \exp(-\xi_i ^\land)\cdot \exp(\xi_j ^\land)\\ ~\\ \Longrightarrow \Delta \xi_{ij} = \ln[\exp(-\xi_i ^\land)\cdot \exp(\xi_j ^\land)]^\vee exp(Δξij∧​)=exp(−ξi∧​)⋅exp(ξj∧​) ⟹Δξij​=ln[exp(−ξi∧​)⋅exp(ξj∧​)]∨
我们定义误差函数：
eij(ξi,ξj)=ln⁡[exp⁡(−ξij∧)exp⁡(−ξi∧)⋅exp⁡(ξj∧)]∨e_{ij}(\xi_i,\xi_j)=\ln[\exp(-\xi_{ij} ^\land) \exp(-\xi_i ^\land)\cdot \exp(\xi_j ^\land)]^\vee eij​(ξi​,ξj​)=ln[exp(−ξij∧​)exp(−ξi∧​)⋅exp(ξj∧​)]∨
这里有两个要优化的变量ξi,ξj\xi_i,\xi_jξi​,ξj​，由泰勒展开：
eij(Δξi⊕ξi,Δξj⊕ξj)≈eij(ξi,ξj)+∂eij∂[δξiT,δξjT]T⋅[δξiT,δξjT]Te_{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 eij​(Δξi​⊕ξi​,Δξj​⊕ξj​)≈eij​(ξi​,ξj​)+∂[δξiT​,δξjT​]T∂eij​​⋅[δξiT​,δξjT​]T
即：
eij(Δξi⊕ξi,Δξj⊕ξj)≈eij(ξi,ξj)+∂eij∂δξiδξi+∂eij∂δξjδξje_{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 eij​(Δξi​⊕ξi​,Δξj​⊕ξj​)≈eij​(ξi​,ξj​)+∂δξi​∂eij​​δξi​+∂δξj​∂eij​​δξj​
这里：
{∂eij∂δξi=−Jr−1(eij)⋅Ad(Tj)∂eij∂δξj=Jr−1(eij)⋅Ad(Tj)(3)\left \{ \begin{aligned} \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{aligned} \right.\tag{3} ⎩⎪⎪⎨⎪⎪⎧​∂δξi​∂eij​​=−Jr−1​(eij​)⋅Ad(Tj​)∂δξj​∂eij​​=Jr−1​(eij​)⋅Ad(Tj​)​(3)
这里的JrJ_rJr​比较复杂，一般不会直接求解，但可以用单位矩阵近似或者：
Jr−1≈I+12[ϕe∧ρe∧0ϕe∧]J_r^{-1}\approx I+\frac 1 2 \begin{bmatrix} \phi_e^\land & \rho_e^\land \\ 0 & \phi_e^\land \end{bmatrix} Jr−1​≈I+21​[ϕe∧​0​ρe∧​ϕe∧​​]

一般的，我们可以利用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;}// 设定g2otypedef 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观测数据为u1u_1u1​，相机坐标系下的数据位置P1P_1P1​；Pose2观测数据为u2u_2u2​，相机坐标系下的P2P_2P2​未知。世界坐标为PwP_wPw​，由相机的投影关系可得：
u1=1s1KT1,wPw=1s1KP1u2=1s2KT2,wPw\begin{aligned} 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{aligned} u1​ u2​​=s1​1​KT1,w​Pw​=s1​1​KP1​=s2​1​KT2,w​Pw​​
这里为了估计从相机1到2的变换，我们需要把相机1固定，假定P1P_1P1​就是PwP_wPw​，相机坐标系1就是世界坐标系，所以又可以写成：
u1=1s1KPwu_1 = \frac 1 {s_1} KP_w u1​=s1​1​KPw​
这时我们要估计的T2,wT_{2,w}T2,w​就是T2,1T_{2,1}T2,1​，也就是从1到2的变换矩阵；

设函数:
u2(ξ21,P1)=1s2Kexp⁡(ξ21∧)⋅P1u_2(\xi_{21},P_1) = \frac 1 {s_2}K\exp(\xi_{21}^\land)\cdot P1 u2​(ξ21​,P1​)=s2​1​Kexp(ξ21∧​)⋅P1
由于是位姿图优化，这里不考虑优化特征点P1的位姿，则函数可表示为：
u2(ξ21)=1s2Kexp⁡(ξ21∧)⋅P1u_2(\xi_{21}) = \frac 1 {s_2}K\exp(\xi_{21}^\land)\cdot P1 u2​(ξ21​)=s2​1​Kexp(ξ21∧​)⋅P1

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

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.1307160.0498885    0.998393   0.0268684 -0.00658621-0.0431114  -0.0247242    0.998764   0.06336220           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.1300260.0499196    0.998397    0.026676 -0.00629541-0.0426763  -0.0245527    0.998787   0.06333220           0           0           1
solve pnp in opencv time_cost: 0.000365883 seconds.
pose estimated by opencv =0.99798  -0.0521689   0.0362579   -0.1221980.0512588    0.998357   0.0255927 -0.00461646-0.0375335  -0.0236824    0.999015   0.06415510           0           0           1

参考

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

2.ORB_SLAM2源码

SLAM--位姿图优化和PnP求解相关推荐

  1. 机器人学习--网友资料系列 激光SLAM建图、粒子滤波定位和位姿图优化

    一.移动机器人自主导航的前提是在未知环境中先构建地图 (目前市内很多用的2D激光雷达,构建栅格地图,相当于立体空间中的某个水平面高度的切面) 一般用的是2D 激光SLAM算法 构建概率栅格占用地图: ...

  2. 视觉SLAM笔记（56） 位姿图优化

    视觉SLAM笔记(56) 位姿图优化 1. g2o 原生位姿图 2. 李代数上的位姿图优化 3. 关于后端优化 1. g2o 原生位姿图 下面来演示使用 g2o 进行位姿图优化 首先,用 g2o_vi ...

  3. SLAM | 使用三维位姿图优化减少单目视觉里程计（3D Visual Odometry）定位轨迹的漂移（附源代码）

    ===================================================== github:https://github.com/MichaelBeechan CSDN: ...

  4. 从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码

    点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 小白:师兄师兄,最近我在看SLAM的优化算法,有种方法叫" ...

  5. (02)Cartographer源码无死角解析-(53) 2D后端优化→位姿图优化理论(SPA)讲解、核型函数调用流程

    讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...

  6. 激光SLAM之图优化理论

    1.常用的两种优化方法介绍 SLAM问题的处理方法主要分为滤波和图优化两类.滤波的方法中常见的是扩展卡尔曼滤波.粒子滤波.信息滤波等,熟悉滤波思想的同学应该容易知道这类SLAM问题是递增的.实时的处理 ...

  7. 科普SLAM之位姿图优化建图

    科普SLAM之位姿图优化建图 本讲重点 位姿图优化建图的基本定义(什么是PGO ?) 位姿图优化建图的研究意义(为什么需要PGO?) 位姿图优化建图的基本原理(PGO如何产生的?) 位姿图优化建图的算 ...

  8. 论文精读 | slam中姿态估计的图优化方法比较

    一. 摘要 对于位置环境中的自主导航问题,同步定位与建图(Simultaneous localization and mapping, SLAM)是一个非常重要的工具框架.根据SLAM字面含义可以得知 ...

  9. SLAM中姿态估计的图优化方法比较（g2o/Ceres/GTSAM/SE-Sync）

    编辑 | 深蓝AI 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 后台回复[SLAM综述]获取视觉SLAM.激光SLAM.RGBD-SLAM等多篇综述! 本 ...

  10. 计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)

    前面介绍了基于EKF的SLAM算法.EKF算法由于状态向量,协方差矩阵的大小随着特征点(路标)的增长而迅速增长,导致其不太适合大场景的应用.本文描述基于图优化的SLAM算法.目前由于SLAM图的稀疏性 ...

最新文章

  1. RelativeLayout布局，不希望文本盖住其他组件
  2. Python 用smtplib库发邮件报错：[WinError 10061] 由于目标计算机积极拒绝，无法连接。解决办法
  3. 为什么可积不一定可导_为什么一定要办理焊工证？不办会怎么样？
  4. 更改printk打印级别【转】
  5. linux的常用操作——静态库
  6. 1105: 判断友好数对（函数专题）
  7. java 三个点_Java，参数中的3个点
  8. vue传递数组对象_详解vue组件三大核心概念
  9. Spring Security使用数据库管理资源整理
  10. TypeError at /**/ ** missing 1 required positional argument: '**'
  11. win10+Vmware14+Centeros7.6 mini网络设置
  12. EXCEL统计不重复值的数量
  13. 转 Still,yet和already的用法
  14. 面试--拼多多面试--后台开发实习生
  15. AspNetPager常用属性及用法 / URLRewrite伪静态与AspNetPager分页控件的结合
  16. 反向代理是什么意思?正向代理和反向代理的区别是什么？
  17. 街景地图工作是如何工作的
  18. 计算机控制系统的模拟控制器,导 读 利用计算机代替常规的模拟控制器，使它成为控制系统的一个组成部分，这种有计算机参加控制的系统简称为计算机控制系统。...
  19. 小文本——Cookies
  20. 从libc-2.27.so[7ff3735fd000+1e7000]崩溃回溯程序段错误segfault

热门文章

  1. python lxml_python – lxml使用命名空间而不是ns0,ns1,
  2. postgresql 数据库连接数查询
  3. js foreach用法_36 个JS 面试题为你助力金九银十(面试必读)
  4. 用java写出死锁的例子_【面试】请写一个java死锁的例子-Go语言中文社区
  5. 计算机专业学不学ps,慎重！不建议你报的院校专业，是因为真的考不上！
  6. SQL order by的用法
  7. 盘点遥测终端RTU怎么分类？
  8. LNMP的运维追踪技巧总结
  9. jvm学习--类加载器
  10. IOS自动化定位方式