第十章g2o_custombundle/g2o_bundle.cpp

最新推荐文章于 2022-05-26 09:31:59 发布

robinhjwy

最新推荐文章于 2022-05-26 09:31:59 发布

阅读量1.8k

点赞数 3

分类专栏： 14讲程序注释

本文链接：https://blog.csdn.net/robinhjwy/article/details/78189240

版权

本文档详细介绍了g2o_custombundle/g2o_bundle.cpp的主要功能，包括BuildProblem()函数用于向g2o添加顶点和边并设置初始值，SetSolverOptionsFromFlags()函数用于构建优化器。优化过程通过SolveProblem()启动，该函数执行后完成整个优化任务。在main()中，仅需调用SolveProblem()即可完成工作。

摘要由CSDN通过智能技术生成

这个程序就是核心的g2o优化的主程序了，前面定义好了数据读写类，参数设定类，这里直接用就好了。
还是分了几个函数：
首先是BuildProblem():
这个函数主要作用就是往g2o中增加顶点和边，并将初始值添加进去。
然后是SetSolverOptionsFromFlags():这个函数就是构造优化器
还是g2o常用路数：
1、typedef块求解器的维度。
2、用线性求解器构造块求解器。
3、用块求解器构造下降策略。
4、将下降策略设定为optimizer的setAlgorithm()。

SolveProblem()就启动优化了，此函数运行完后优化就完毕了。所以main()中就是直接调用了一句这个函数就完事了。

#include <Eigen/StdVector>
#include <Eigen/Core>

#include <iostream>
#include <stdint.h>

#include <unordered_set>
#include <memory>
#include <vector>
#include <stdlib.h> 

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"

#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"

#include "g2o/solvers/structure_only/structure_only_solver.h"

#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"


using namespace Eigen;
using namespace std;

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
//给块求解器模板类定义维度并typedef，pose的维度为9维，landmark维度为3维
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;

// set up the vertexs and edges for the bundle adjustment.
//问题构建函数，传入一个BALProblem类型指针，稀疏求解器指针，参数类引用BundleParams&
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    //将bal_problem中的数据读出来
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    // Set camera vertex with initial value in the dataset.
    //将相机数据的首位置读出，用于后方数据读取
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        //这里将9维相机位姿从数组中取出来构建成矩阵，用于下面的顶点的初始值赋值
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        //开辟个新的相机顶点类指针
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        //设定初始值
        pCamera->setEstimate(temVecCamera);   // initial value for the camera i..