SALM14讲-后端1-code详细注释

目录
概述
1.1 状态估计的概率解释
1.2 线性系统和KF
1.3 非线性优化和EKF
2 BA与图优化
2.1 投影模型和BA代价函数
2.2 BA的求解
2.3 稀疏性和边缘化
2.4 鲁棒核函数
2.5 小结
3 实践：g2o
3.1 BA数据集
3.2 g2o求解BA
3.3 求解
概述：
前端能够根据相邻的两幅图像判断出此时此刻的位姿，是暂时的；那么后端需要对前端测量以及计算的结果进行矫正，不仅用过去的信息，也用未来的信息更新自己，希望能够得到一个长时间的正确状态。
在运动方程和观测方程中，如果把位姿和路标看成随机变量，就可以把问题变为已知观测数据和运动数据情况下，如何确定状态量的分布问题，是一个状态估计问题。假设噪声和状态量服从高斯分布，只需要估计它的均值和方差，即可确定状态量的分布。
注：刻画分布用到均值和协方差矩阵，此处均值代表着状态估计值，协方差矩阵代表着不确定性。
完成后端优化可以使用滤波器，也可以使用非线性优化，本讲对两种方法都做了推导。
理解批量式 和 渐进式 处理方法

前端给到的是带有噪声的，比较粗糙的数据（换句话说视觉里程计VO得到的是粗糙的位姿估计，有累计误差，因为仅仅依靠相邻帧），后端使得平均误差更小，后端一般是一个最大似然，最大后验概率。
渐进式------EKF
批量式------非线性优化

依靠运动估计去推到轨迹是很粗糙的，累计误差会越来越大，没有约束； 观测虽然也有不确定性，但是可以依靠观测约束不确定性的增长，误差均分到整个过程。



似然可以通过观测方程推出
先验比较麻烦，先条件概率展开

假设一阶马尔可夫性，
第一项：K时刻状态只与K-1时刻有关 ------->（运动方程）。
第二项：状态估计------->（观测方程）。非线性优化

线性系统---- h 和 f函数是线性函数，线性方程性质：高斯分布的线性组合还是线性分布。
高斯分布，需要均值和协方差就可以刻画改分布长什么样。其中均值一般指状态估计的值，协方差是估计的不确定度。

卡尔曼滤波器线性的推导





预测： K-1时刻后验推导K时刻先验， 依靠运动方程。
更新： K时刻先验推导K时刻后验， 依靠观测方程。

非线性系统和EKF
非线性系统，高斯分布下，在工作点附近线性展开，得到EKF。



BA与图优化
书本定义：所谓的 Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程 ，简称为 BA。
批量式处理方式：
1 先定义误差；
2 平方和（最小二乘），找梯度（梯度下降）；
3 迭代使其收敛。

图模表示：优化量用节点表示，运动/观测用边来表示。
观测方程，其中包含畸变矫正，如下：


每个观测只关系两个变量，其中一个是相机，一个是路标，导致h具有特殊结构。
后续理论省略。。。。。。。。

实践g2o
详细代码注释
定义图模型的节点和边，节点表示相机位姿pose和路标点landmark，边表示相机路标之间的观测。

#include <Eigen/Core>
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"

#include "ceres/autodiff.h"

#include "tools/rotation.h"
#include "common/projection.h"

// 定义节点 和 边
// 节点用相机和路标来表示， 边表示它们之间的预测。
// 此处使用自动求导， 所以更新量看作向量加法。    之前是通过两个SE（3）相乘然后转化成新的SE（3），但是使用的是左绕动模型

//定义相机位姿顶点类，由于相机内参也作为优化变量，所以包含了：
//此处可能是三个外参（f，cx，cy），应该不是三个畸变系数。 另外还有3个参数的平移，3个参数的旋转。一共九个量，9维，类型为Eigen::VectorXd
// 相机顶点
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCameraBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl ( const double* update )
    {
      
        //由于update是个double类型数组，而增量需要的是个矩阵，
        //所以用update构造一个增量矩阵v，下面更新估计值时，直接将v加上就好了。
        //ConstMapType定义如下：typedef const Eigen::Map<const Derived,Unaligned>ConstMapType;
        //发现就是一个typedef，还是Map类型。 
        //对于动态矩阵，上方的位姿，VectorXd构造时要传入维度，VertexCameraBAL::Dimension就是位姿顶点的维度，也就是定义时传入的模板参数9。 
      
        Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
        _estimate += v;
    }

};


// 空间点顶点
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl ( const double* update )
    {
       //而静态矩阵，上方的landmark，Vector3d构造时就不用传入维度了，直接传出需要转化的数组就好了。
      
        Eigen::Vector3d::ConstMapType v ( update );
        _estimate += v;
    }
};

// 边
//BAL观测边，边即误差，继承自基础二元边。这里误差应该是重投影的像素误差
// 参数为：误差维度2维，误差类型为Eigen::Vector2d，连接两个顶点：VertexCameraBAL和VertexPointBAL(也就是说误差和这两个优化变量有关)

class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    // 使用G2O数值导， 不像书上调用的ceres的自动求导功能
    virtual void computeError() override   // The virtual function comes from the Edge base class. Must define if you use edge.
    {
      // 这里将第0个顶点， 相机位姿取出来
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
	// 这里将第1个顶点，空间点位置取出来
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

	//将相机位姿估计值，空间点位姿估计值 传给了重载的()运算符，这个重载，将计算好的结果输出到_error.data()，完成了误差的计算
        ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );

    }

    // 这里即为重载的()函数，为模板函数，需要数据为相机位姿指针，空间点位置指针，用于承接输出误差的residuals。
    // 上面调用时，用的_error.data()承接，完成误差计算。
    // 这个模板类其实还是用的重投影误差
    
    template<typename T>
    bool operator() ( const T* camera, const T* point, T* residuals ) const
    {
        T predictions[2];
	 //这个函数就是反应的投影过程，camera参数，point参数，然后用predictions承接算得的像素坐标。这里也发现就是二维的
        CamProjectionWithDistortion ( camera, point, predictions );
        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );

        return true;
	// 小总结一下，从computeError()一直到这里，搞得这一些就是为了计算一个重投影误差，
        // 误差的计算被写进了重载的()中，投影过程被写进了CamProjectionWithDistortion()中
	
    }  

 //这里重写线性增量方程，也就是雅克比矩阵
 
    virtual void linearizeOplus() override
    {
        // use numeric Jacobians
        // BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
        // return;
        
        // using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians

        //使用ceres的自动求导，不然系统将调用g2o的数值求导
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
	
 //这里贴上AutoDiff的定义，发现是一个模板结构体，模板参数为代价函数类型，模板类型，代价函数的各参数维度(这里就两个了，相机顶点维度，空间点维度)
        /*template <typename Functor, typename T,
                int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
                int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
        struct AutoDiff {
            static bool Differentiate(const Functor& functor,
                                      T const *const *parameters,
                                      int num_outputs,
                                      T *function_value,
                                      T **jacobians) {...}*/

        //这里来一个typedef,将模板类简化定义一下，定义成BalAutoDiff
        //看一下模板参数：
        //EdgeObservationBAL，就是代价函数类型，这里就是边的类型了
        //模板类型为double,基本数据的类型
        //VertexCameraBAL::Dimension和VertexPointBAL::Dimension就是对应的两个N0和N1，误差函数参数的维度
        //这里直接把维度取出来了(Dimension即是取得维度)，也可以直接输入9和3
        
        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

	 //定义一个行优先的double类型矩阵，大小为Dimension*VertexCameraBAL::Dimension,也就是2*9。这里就是误差对相机的导数
        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
	 //定义一个行优先的double类型矩阵，大小为Dimension*VertexPointBAL::Dimension,也就是2*3。这里就是误差对空间点的导数
        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
	 //double*类型的数组，成员为double*,这里装了相机估计值数组指针和空间点估计值数组指针。
        double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
	 //雅克比矩阵为两块导数拼合起来的，一块是误差对相机的导数，一块是误差对空间点的导数。也就是上方定义的2*9的dError_dCamera和2*3的dError_dPoint
        double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
	
        double value[Dimension];
	
	 //这里就是一直所说的利用ceres的现行求导，这个Differentiate()就是在AutoDiff结构体中定义的。
        /*static bool Differentiate(const Functor& functor,
                                  T const *const *parameters,
                                  int num_outputs,
                                  T *function_value,
                                  T **jacobians) {...}*/
        //看一下参数：
        //const Functor& functor，代价函数，这里也就是这个边类了，直接用*this
        //T const *const *parameters，参数列表，就是上面定义的有两个double指针的parameters数组，这两个指针一个指向相机参数数组，一个指向空间点数组
        //int num_outputs，输出的维度，这里就是边的维度Dimension，也就是2维
        //T *function_value，误差函数functor的输出值，用于承接functor的输出，也就是*this计算出来的误差。
        //T **jacobians，这就是最终要求的雅克比矩阵了。用于承接。
        
        bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
        // copy over the Jacobians (convert row-major -> column-major)
	//雅克比矩阵到这里就计算完成了，最后就是赋值给_jacobianOplusXi和_jacobianOplusXj了。
	
        if ( diffState )
        {
            _jacobianOplusXi = dError_dCamera;
            _jacobianOplusXj = dError_dPoint;
        }
        else
        {
	        //不成功就输出警告，并将雅克比矩阵置为0矩阵。
	  
            assert ( 0 && "Error while differentiating" );
            _jacobianOplusXi.setZero();
            _jacobianOplusXj.setZero();
        }
    }
};

投影模型注释：

#ifndef PROJECTION_H
#define PROJECTION_H

#include "tools/rotation.h"

// camera : 9 dims array with 
// [0-2] : angle-axis rotation 
// [3-5] : translateion
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
// point : 3D location. 
// predictions : 2D predictions with center of the image plane. 
 
/***
 * 去畸变，世界坐标 转 相机坐标 转 像素坐标， 基本跟书上一致
 * 基本思想：
 * P = R * X + t  从世界坐标系转相机坐标系
 * p = -p / p.z 
 * p` = f * r(p) * p 转到像素坐标
 * 
 * 相机畸变该处只考虑径向畸变，流程：
 * 1）将三维空间大点投影到归一化平面
 * 2）对归一化平面上的点进行畸变矫正
 * 3）将矫正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。
 */
template<typename T>
inline bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions){
    // Rodrigues' formula
    T p[3];
    AngleAxisRotatePoint(camera, point, p);
    // camera[3,4,5] are the translation
    // 旋转矩阵表示空间点
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

    // Compute the center fo distortion
    // xp = -x/z
    // yp = -y/z
    T xp = -p[0]/p[2];
    T yp = -p[1]/p[2];

    // Apply second and fourth order radial distortion
    const T& l1 = camera[7];
    const T& l2 = camera[8];

    T r2 = xp*xp + yp*yp;
    T distortion = T(1.0) + r2 * (l1 + l2 * r2);

    const T& focal = camera[6];  // 焦距长度
    predictions[0] = focal * distortion * xp;
    predictions[1] = focal * distortion * yp;

    return true;
}
#endif // projection.h

主文件g2o_bundle.cpp

#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;
// 给求解器模块定义维度 ， pose： 九维    landmark： 三维
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;

// set up the vertexs and edges for the bundle adjustment. 
// 问题构建函数，传入一个BALProblem类型指针，稀疏求解器指针，参数类引用BuildProblem&
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..
        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..
        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();

        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 
        
        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<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(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、 BundleParams 说明
 * 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<VertexCameraBAL*>(optimizer->vertex(i));
        //auto pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
      
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(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<VertexPointBAL*>(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<BalBlockSolver>& 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<BalBlockSolver>& solver_ptr, const BundleParams& params)
{
    //std::cout<<"Set Linear Solver .."<< std::endl;
    g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
    
    if(params.linear_solver == "dense_schur" ){
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    }
    else if(params.linear_solver == "sparse_schur"){
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }
    

    solver_ptr = std::make_shared<BalBlockSolver>(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<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
    //使用稠密计算方法
    if(params.linear_solver == "dense_schur" ){
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    }
    else if(params.linear_solver == "sparse_schur"){
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
	 //让solver对矩阵排序保持稀疏性
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(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);
}




/***
 * 解决BAL问题
 * 将.txt文件传入，最后将优化后的点云输出
 */

//开始优化，这个优化函数参数就是待优化文件和优化参数
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;
	      
    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);
    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(&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
    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 <path for dataset>";
        return 1;
    }

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

编译完成后得到两个文件一个优化前一个优化后

使用Meshlab打开.ply文件
优化前：

优化后：