SLAM十四讲第十章g2o_custombundle注释

g2o_custombundle—g2o自适应求导的BA

工程组织结构

包括common，ceres，data以及g2o求解程序g2o_bal_class和g2o_bundle

一、 common：

通用工具模块。
包含一些通用的基本工具，如命令读取和解析，会用到的简单数学工具（生成随机数、旋转各种形式的转换），参数设置，基本投影模型以及点云数据集文件的读写。
分别对应五部分：**flags、tools、BundleParams、projection、BALProblem **

1. flags文件夹中包括两个文件command_args.h和command_args.cpp。
   实现解析用户命令的功能，基本工具。
   定义了一个类CommandArgs，该类是用来解析用户输入的参数，对参数提供默认值以及文档说明.
   类型通用,适合任何输入命令解析，BundleParams类的功能基础
2. BundleParams.h
   这个文件就是定义BA优化中用到的所有参数变量。并利用CommandArgs类进行变量管理。比如赋默认值等。
   这里再啰嗦一遍，CommandArgs类只是一个变量(这里就是BA用到的所有参数了)管理类，真正的变量(这里就是参数集了)定义是在这个头文件中，也就是所有参数都定义在BundleParams这个结构体中。
   主要是调用commandargs的函数功能,对BA问题参数进行解析给出描述以及默认值
3. tools包括random和rotation.给出了生成随机数的基本功能工具以及旋转各形式转换相关基本工具.
4. projection实现了带有畸变的相机投影过程，其中使用了相机参数camera，是一个9维的列表，将point点云转化为重投影预测值predictions（是以图像中心作为坐标原点，并非以左上角，倒像取负，并非用的正面归一化平面坐标）。
   其中camera的0-2为旋转向量，3-5平移向量，6焦距，7-8径向畸变
5. BALProblem类。包含BALProblem.h和BALProblem.cpp两部分。存储读写优化数据（待优化、优化后）。
   这整个类的功能就是对原始的txt数据进行分割存储，然后提供对txt数据的读取写入和生成PLY文件功能。
   存储部分：相机维度，路标点维度，相机个数，路标点个数，观测值个数，待优化参数个数（相机维度相机个数+点维度点个数）mutable存储优化后数据（可变）

二、ceres：使用ceres的自动数值求导功能模块

三、data：存放txt格式数据，文件头为相机个数、路标点个数、观测个数，然后列举对每一个路标点的不同相机的观测数据

四、g2o求解程序：

包括g2o_bal_class.h和g2o_bundle.cpp

1. g2o_bal_class

节点表示待优化的参数，边表示误差（观测）。
继承于基类，自定义节点和边。

1. 相机节点：9维除了3维旋转3维平移，焦距及两个畸变系数也需要优化，类型为Eigen::VectorXd.
   其中增量函数中的Eigen::VectorXd::ConstMapType v ( update,VertexCameraBAL::Dimension );
   将c++中的数组映射为对应的eigen矩阵类型，VectorXd后需要维度信息。
   更新为直接相加。
2. 路标节点：三维 Vector3d类型
3. 观测误差边：继承自基础二元边。重投影像素误差。
   维度为2，类型Eigen::Vector2d，连接两个顶点VertexCameraBAL和VertexPointBAL(也就是说误差和这两个优化变量有关)
   其中误差计算函数：连接的两个顶点0为相机，1为空间点。
   提取后使用重载的括号函数计算误差.传入的参数为相机顶点的估计值数据,路标点的估计值数据,将计算好的结果输出到_error.data().（重载括号模板函数定义误差类型为double*，使用.data将数据转化为double数组）
   ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
   计算雅克比矩阵，使用ceres的数值雅克比,否则调用g2o的自动数值求导。
   提取顶点之后调用autodiff模板类，模板参数为：边，基本数据类型，N0：node0维度，N1：node1维度，将其typedef为BalAutoDiff。误差对相机导数2×9，误差对空间点导数2×3，定义对应存储导数的矩阵。
   定义元素为double类型的指针数组，存储相机数据数组指针，路标点数据数组指针
   定义元素为double类型的指针数组，存储两个雅克比矩阵数据数组指针。
   使用autodiff结构体中的静态成员函数求导函数，其中第一个变量对应模板中第一个参数就是该边,直接使用this,第二个参数为数据数组的指针数组,第三个参数为维度,value承接误差（double）,jacobians承接得到的雅克比矩阵,返回bool值
   bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );

2. g2o_bundle

全局BA，引入参数设置，文件读写函数，以及节点和边的定义。

2.1 包含四个部分的函数：

1. BuildProblem：读取数据，设置节点和边，构建图优化问题
2. WriteToBALProblem：将优化后的数据写入BALProblem类中
3. SetSolverOptionsFromFlags/SetMinimizerOptions/SetLinearSolver：设置优化求解器
4. SolveProblem:优化求解

2.2 main函数组织结构
第一步：从命令行读取并解析参数，声明类BundleParams，同时文件名也为该类的一个参数，同时读取有没有用户自己定义的参数，否则使用BundleParams中的默认值
第二步：调用SolveProblem（文件名，参数类），完成优化求解

2.3 BuildProblem问题建立
传入的参数为已经读取好数据的问题类指针BALProblem*，优化器指针g2o::SparseOptimizer*，参数类const BundleParams&
第一步：将bal_problem中的数据读取出来（const），主要是包括点、相机数量及对应维度。
第二步：读取相机数据头位置指针，向图中加入节点（ｉ从０到相机数量，每次将raw_cameras + camera_block_size * i位置，camera_block_size大小的数值映射为向量作为新顶点的估计值，并设置顶点id，在优化器（图）中加入顶点）
第三步：读取路标点数据头位置指针（比相机偏移了相机数量×相机维度），向图中加入节点（j从０到路标点数量，每次将rraw_points + point_block_size * j位置，point_block_size大小的数值映射为向量作为新顶点的估计值，并设置顶点id(j+相机数量)，对路标进行边缘化操作（schur消元），设置边缘化属性为true，在优化器（图）中加入顶点）
第四步：向图中加入边。提取边的个数等于观测个数。获取观测值的头位置指针。遍历。新建边，取出连接的顶点的id，设置鲁棒核函数，将取出的一对顶点连接起来，相机设为0，路标设为1，设置信息矩阵为单位矩阵，添加测量值为该次观测数据，在优化器（图）中加入边

2.4 WriteToBALProblem将优化后的结果写入BALProblem类，便于转为ply文件
传入的参数为BALProblem*，和优化器g2o::SparseOptimizer*
第一步：将bal_problem中的数据读取出来（const），主要是包括点、相机数量及对应维度。
第二步：读取可变相机数据头位置指针，用于数据写入。遍历。将相机顶点取出，这里说一下为什么要做这一步指针类型转化，因为optimizer->vertex(i)返回的类型是个vertex指针类型，需要将其转化成VertexCameraBAL才能访问估计值，直接像下面的用法会报错：optimizer->vertex(i)-> estimate();。取出值就可以memcpy()了，这里当然是一个9维的数组，长度上很明显是9*double，地址位置为raw_cameras + i\ * camera_block_size，另外将取出的eigen矩阵型数据使用.data()化为double*
第三步：读取可变路标点数据头位置指针。遍历，提取路标点顶点（id偏移相机个数），强制类型转为子类型指针后取出数据，使用memcpy()

2.5 SetSolverOptionsFromFlags设置求解器
设置求解器，参数为数据类，参数类const BundleParams&，优化器g2o::SparseOptimizer*：
第一步：设置块求解器维度为9,3
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;
第二步：设置线性求解器g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver稠密稀疏
第三步：将线性求解器对象传入块求解器中，构造块求解器对象
BalBlockSolver* solver_ptr = new BalBlockSolver(linearSolver);//std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));
第四步：将块求解器传入下降策略，构造下降策略对象g2o::OptimizationAlgorithmWithHessian* solver;根据置信域参数选择lm还是dl，选择OptimizationAlgorithmLevenberg/OptimizationAlgorithmDogleg
第五步：将下降策略传入优化器的优化逻辑中，至此，一个优化器就构建好了optimizer->setAlgorithm(solver);

2.6 SolveProblem
参数为待优化数据文件名，相关参数类const BundleParams&
第一步：读取数据文件，构造问题对象（数据类）BALProblem bal_problem(filename);同时先生成一下点云文件initial_ply，防止优化后丢失
第二步：数据归一化，预处理
第三步：创建一个稀疏优化器对象，调用 SetSolverOptionsFromFlags设置优化器
第四步：调用BuildProblem，建立图优化问题
第五步：开始优化optimizer.initializeOptimization();
optimizer.setVerbose(true);//输出优化信息
optimizer.optimize(params.num_iterations);
第六步：优化完成，调用WriteToBALProblem将优化后的结果写入BALProblem类，此时原数据被覆盖调，幸好我们之前已经生成过原数据的ply文件
第七步：将优化后的数据生成点云文件