第十章g2o_custombundle/g2o_bundle.cpp

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

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

#include 
#include 

#include 
#include 

#include 
#include 
#include 
#include  

#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 VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
//给块求解器模板类定义维度并typedef，pose的维度为9维，landmark维度为3维
typedef g2o::BlockSolver9,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..
        //设定ID
        pCamera->setId(i);                    // set id for each camera vertex

        // remeber to add vertex into optimizer..
        optimizer->addVertex(pCamera);

    }

    // Set point vertex with initial value in the dataset.
    //同样，这里将路标数据的首位置读出，用于后面读取
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for(int j = 0; j < num_points; ++j)
    {
        //同样，将数组中的路边数据读出构建成矩阵
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        //开辟个新的路标顶点指针
        VertexPointBAL* pPoint = new VertexPointBAL();
        //设定初始值
        pPoint->setEstimate(temVecPoint);   // initial value for the point i..
        //设定ID，不能跟上面的相机顶点重复，所以加了个相机个数，直接往后排
        pPoint->setId(j + num_cameras);     // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex

        // remeber to add vertex into optimizer..
        //由于路标要被边缘化计算，所以设置边缘化属性为true
        pPoint->setMarginalized(true);
        //将顶点添加进优化器
        optimizer->addVertex(pPoint);
    }

    // Set edges for graph..
    //取出边的个数
    const int  num_observations = bal_problem->num_observations();
    //取出边数组首位置
    const double* observations = bal_problem->observations();   // pointer for the first observation..

    //用观测个数控制循环，来添加所有的边
    for(int i = 0; i < num_observations; ++i)
    {
        //开辟边内存指针
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        //由于每条边要定义连接的顶点ID，所以这里将cameraID和pointID取出
        const int camera_id = bal_problem->camera_index()[i]; // get id for the camera; 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point 

        //???what?
        if(params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }

        // set the vertex by the ids for an edge observation
        // 这里对每条边进行设置：
        // 连接的两个顶点
        bal_edge->setVertex(0,dynamic_cast(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1,dynamic_cast(optimizer->vertex(point_id)));
        //信息矩阵，这里依旧是单位阵
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        //设置默认值，就是将观测数据读进去
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i+1]));

        //将边添加进优化器
        optimizer->addEdge(bal_edge) ;
    }

}



//再看一下程序各个类作用：
//BALProblem跟优化数据txt对接，负责txt的读取、写入，同时还有生成PLY点云文件的功能
//BundleParams类负责优化需要的参数值，默认值设定和用户命令行输入等功能。
//整体这样归类之后，所有优化数据就去BALProblem类对象中询问，参数就去BundleParams类对象询问。

//这个函数的作用是将优化后的结果再写入到BALProblem类中，
//注意，在BALProblem类中，定义的所有读取写入功能都是BALProblem类与txt数据的，并没有优化后的数据与BALProblem的，
//所以这里定义了之后，就会产生优化后的数据类BALProblem，这样再跟txt或者PLY对接的话就很容易了。
//参数很容易理解，被写入的BALProblem*,优化器
void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer)
{
    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();

    //用mutable_cameras()函数取得相机首地址，用于后面的数据写入
    double* raw_cameras = bal_problem->mutable_cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        //将相机顶点取出，这里说一下为什么要做这一步指针类型转化，因为optimizer->vertex(i)返回的类型是个vertex*指针类型，
        //需要将其转化成VertexCameraBAL*才能访问估计值，直接像下面的用法会报错：
        //optimizer->vertex(i)-> estimate();
        //原程序是下面这样写的，但是感觉这里用auto比较方便一些，并且也更能体现pCamera仅是个承接的功能。
        //VertexCameraBAL* pCamera = dynamic_cast(optimizer->vertex(i));
        auto pCamera = dynamic_cast(optimizer->vertex(i));
        Eigen::VectorXd NewCameraVec = pCamera->estimate();
        //取得估计值之后，就可以memcpy()了，这里当然是一个9维的数组，长度上很明显是9*double
        memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
    }

    //同理在point上也是一样，不再细说
    double* raw_points = bal_problem->mutable_points();
    for(int j = 0; j < num_points; ++j)
    {
        VertexPointBAL* pPoint = dynamic_cast(optimizer->vertex(j + num_cameras));
        Eigen::Vector3d NewPointVec = pPoint->estimate();
        memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
    }
}

//this function is  unused yet..
void SetMinimizerOptions(std::shared_ptr& solver_ptr, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
    //std::cout<<"Set Minimizer  .."<< std::endl;
    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr.get());
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr.get());
    }
    else 
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
    //std::cout<<"Set Minimizer  .."<< std::endl;
}

//this function is  unused yet..
void SetLinearSolver(std::shared_ptr& solver_ptr, const BundleParams& params)
{
    //std::cout<<"Set Linear Solver .."<< std::endl;
    g2o::LinearSolver* linearSolver = 0;

    if(params.linear_solver == "dense_schur" ){
        linearSolver = new g2o::LinearSolverDense();
    }
    else if(params.linear_solver == "sparse_schur"){
        linearSolver = new g2o::LinearSolverCholmod();
        dynamic_cast* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }


    solver_ptr = std::make_shared(linearSolver);
    std::cout <<  "Set Complete.."<< std::endl;
}

//求解设置：使用哪种下降方式，使用哪类线性求解器
/**
 * 设置求解选项,其实核心就是构建一个optimizer
 * @param bal_problem 优化数据
 * @param params 优化参数
 * @param optimizer 稀疏优化器
 */
void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{   
    BalBlockSolver* solver_ptr;

    g2o::LinearSolver* linearSolver = nullptr;

    //使用稠密计算方法
    if(params.linear_solver == "dense_schur" )
    {
        linearSolver = new g2o::LinearSolverDense();
    }
    //使用稀疏计算方法
    else if(params.linear_solver == "sparse_schur")
    {
        linearSolver = new g2o::LinearSolverCholmod();
        //让solver对矩阵排序保持稀疏性
        dynamic_cast* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }


    //将线性求解器对象传入块求解器中，构造块求解器对象
    solver_ptr = new BalBlockSolver(linearSolver);
    //SetLinearSolver(solver_ptr, params);

    //SetMinimizerOptions(solver_ptr, params, optimizer);
    //将块求解器对象传入下降策略中，构造下降策略对象
    g2o::OptimizationAlgorithmWithHessian* solver;
    //根据参数选择是LM还是DL
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
    }
    else //这里有一道防呆，没有输入下降策略或者输入错误时，输出报警并退出
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    //将下降策略传入优化器的优化逻辑中，至此，一个优化器就构建好了
    optimizer->setAlgorithm(solver);
}

//开始优化，这个优化函数参数就是待优化文件和优化参数
void SolveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    //.num_cameras()返回num_cameras_ 值，显示相机数量
    //.num_points()返回num_points_ 值，显示路标数量
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    //.num_observations()返回num_observations_ 值，显示观测边的数量
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;

    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty())
    {
        //优化前将BALProblem类中的数据生成一下点云数据，因为优化后，这个类中的数据会被覆盖
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;

    // add some noise for the intial value
    srand(params.random_seed);
    //这里发现就用到了Normalize()，感觉就是对数据的处理，
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma, params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;


    //创建一个稀疏优化器对象
    g2o::SparseOptimizer optimizer;
    //用SetSolverOptionsFromFlags()对优化器进行设置
    SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
    //设置完后，用BuildProblem()进行优化，参数也很清晰了：数据，优化器，参数
    BuildProblem(&bal_problem, &optimizer, params);


    std::cout << "begin optimizaiton .."<< std::endl;
    // perform the optimizaiton
    //开始优化
    optimizer.initializeOptimization();
    //输出优化信息
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete.. "<< std::endl;
    // write the optimized data into BALProblem class
    //优化完后，将优化的数据写入BALProblem类，此时这个类中原始数据已经被覆盖，不过没关系，在优化前，它已经生成过PLY点云文件了
    WriteToBALProblem(&bal_problem, &optimizer);

    // write the result into a .ply file.
    if(!params.final_ply.empty())
    {
        //优化后，将优化后的数据生成点云文件
        bal_problem.WriteToPLYFile(params.final_ply);
    }

}

int main(int argc, char** argv)
{
    //在这里搞参数时就很简单了，因为BundleParams类中自带了BA用的所有参数，并且都有默认值，
    //由argc,argv构造也是类构造函数决定的，需要读一下命令行中有没有用户自定义的参数值，有读进来将默认值覆盖
    BundleParams params(argc,argv);  // set the parameters here.

    //判断一下，如果输入的数据为空的话，肯定报警了。
    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input ";
        return 1;
    }

    //main()中直接调用SolveProblem()就好了，传入数据和优化参数
    SolveProblem(params.input.c_str(), params);

    //到这里看出一些设计思想，主函数中代码很少，其实要的就是这种效果，通过后台类的设计，在使用时，就传入待优化数据和需要的参数就能出结果
    //任何功能模块在使用端都希望是这种用法，简单直接，不care函数内部实现逻辑，调用时一句话，传参出结果。
    //函数黑箱内部的实现也是分块去实现，不同的功能写在不同的类中，类比函数好的一点是不光有功能，还能存储数据。
    //设计思想值得借鉴，感觉已经是一个基本后端小框架了，不是之前仅为了练习写的demo。
    return 0;
}