最近看完了《视觉SLAM十四讲（第2版）：从理论到实践》（高翔等著），原书分两部分，先介绍了数学基础，然后介绍了具体的SLAM实践，非常适合零基础开始学习SLAM。

作为总结，这里并不对书中的内容做太多的重复说明，本人的水平也不足以用聊聊数字概括原书精华，因而这里采用这样一种思路：提出问题，分析求解，实践应用——来说说自己的理解。

本文从研究背景出发，引出SLAM问题的数学模型；运用数学工具分析求解模型（主要是直接求导与扰动模型）；最后，对原书第13讲的代码组织进行整理，修改并注释第6讲的g2o代码以便理解和回顾。

一、问题的提法

SLAM，即同步定位与建图，顾名思义，我们需要同时估计自身的位置（定位）和环境中物体（路标）的位置（建图）。例如，在自动驾驶汽车中，传感器（摄像头，毫米波雷达，激光雷达等）测量汽车前方的道路情况和障碍物情况，构建地图（考虑到路上的行人，车辆等时刻在变化，至少得是局部地图）以及汽车在该地图中的方位，用于规划汽车的局部行驶路径以躲避障碍物，这就可以运用到SLAM技术。另外，不考虑建图时，SLAM中的视觉里程计也可以帮助我们估计汽车的运动情况。

在SLAM中，我们用“位姿”来表示“位置”，因为“位姿”不仅包括物体的空间位置还包括其朝向，通常用矩阵来表示；用空间点的集合来表示地图（点云地图，在此基础上可构建其他符合需求的地图）。典型的SLAM问题可写成如下形式的优化问题：

                          

其中， 表示位姿； 表示空间点坐标； 表示在位姿  下对  的观测结果，在视觉SLAM中，通常是图像像素点位置；函数  表示观测方程；函数  表示核函数，用于避免由于错误匹配等导致的二范数增长过快，提高系统的鲁棒性。

针对上述优化问题，我们主要需要解决以下两个问题：

1、如何构建该模型？主要涉及  和  的匹配问题，即那个空间点被那个姿态的相机“看到”？

2、如何求解该模型？考虑到  通常是矩阵，如何求雅可比矩阵？

二、分析求解

1、问题1：如何构建模型

在原书第13讲，作者给出的做法是：

1） 初始化。检测第一帧图像的特征点并计算对应的空间点（称之为“路标”）。此时，检测到的“路标”便被第一帧对应的位姿  看到。

2） 匹配相邻两帧图像的特征点，并统计数量。如果匹配的数量足够多，则不增加新的特征点（也就不增加新的路标），因此  观测到的路标包含于  观测到的路标集合之中，具体哪个路标被  看到由匹配情况确定。

3） 如果匹配的数量太少，则重新计算特征点，增加相应的路标，即  观测到了新增加的所有路标。

2、问题2：如何优化

首先指出，对于一般优化问题，待优化变量为向量，此时我们可以通过矩阵求导法则得到雅可比矩阵（相当于导数），然后运用梯度下降法或者高斯牛顿法等优化方法，求得最优解。在优化过程，关键是找到待优化变量每次迭代时的增量。（原书第6讲）

针对问题2，我们有两种选择：1）用某种向量表示位姿，这样在优化过程中便不存在对矩阵求导的问题；2）利用某种方式求位姿（变换矩阵）的增量。

第一种方法中，我们可以采用 [旋转向量；平移向量] 或者 [单位四元数；平移向量] 的形式表示位姿。此时，观测方程  的形式比较复杂，也不容易写出其对待优化变量的导数（雅可比矩阵）。但是，幸运的是，求导可借助 Ceres, g2o 等优化库提供的自动求导工具，我们只需要计算  即可。原书作者在第9讲《后端1》中即采用了这种做法。

第二种方法中，我们引入李群李代数，先将矩阵（李群）映射到向量（李代数），从而实现求导，然后再更新位姿。

通常，我们有：

                                                    

则：

                              

（上述公式涉及齐次坐标和非齐次坐标的转换；采用非正式记号，因为并不能对矩阵直接求导）

                                              ，  是向量，此项符合求导规则

因此，问题的关键在与求：

                                                                   

引入李代数  ，我们有：

                                                              

则：

                                                     

这里，我们指出：

                                            

1）直接求导

我们将  在  处做一阶泰勒展开 ：

                                         

其中  为  在  处的导数。根据原书6.2.2节，在一次迭代中求解：

                                                 

即可求得本次迭代的最优增量   。

的计算如下：

                                            

根据原书第四讲《李群与李代数》，可得：

可见，这里计算  的形式十分复杂。同时，我们强调，用这里的  求得的增量  可直接叠加用于计算李代数上的更新值 ，但：

                                                               

我们可以直接得到李群上的更新值：

                                                      

我们可以考虑始终用李代数表示位姿，这实际上就相当于第一种方法（但不等同，因为    才表示平移向量）。

2）扰动模型

我们令：

                                             

在已知本次迭代的初始值  时，上式可看作 的函数。因  是小量，我们将其在 0 处一阶泰勒展开：

                             

其中  为  在  处的导数。的计算如下（参考第4讲）：

                                   

                                                

                                                

用这里的 求得的增量  不可直接叠加用于计算李代数上的更新值，即 ，但我们有李群上的增量：

                                                             

从而李群上的更新值：

                                                          

跟直接求导相比，扰动模型更简单，同时也方便使用矩阵来表示位姿。

根据上述过程，扰动模型相当于在李群上施加了一个扰动  并在李代数上求扰动结果关于  的在  处的导数。

三、设计框架

至此，我们知道如何建模：

                                 

以及可以求解该模型了。

具体实现上，参考作者在第13讲给出的设计框架和代码，整理SLAM模块组成如下图所示。实际工程肯定比这个复杂和细致的多，这里仅作理解用。整个模块可按照接口，功能，组件，支持分为四层，将每一层包含的元素分别定义成类，编写成头文件并加以引用；上层元素类的成员通常包括下层元素类，每个元素类的方法根据算法确定。最后，作者定义了一个Config类用于链接数据。  

该讲的实现效果如下所示：

                    

考虑到实际工程中，有不同的前端、后端及回环检测和校正策略，这里仅对图优化库 g2o 的代码进行注释，以便更好地掌握该通用工具。该代码取自第6讲，但是做了修改，采用二元边而不是一元边（应用时没有必要这么做）以便注释。代码如下：

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

//****** 利用 g2o 拟合曲线方程：y = exp(ax^2+bx+c) + w, w为高斯噪声 **************

/* g2o 使用步骤:
 * 1. 自定义顶点和边:通过继承的方式；或使用库中预定义的类型
 * 2. 配置求解器
 * 3. 添加顶点和边
 * 4. 求解
*/

// 1. 通过继承 g2o::BaseVertex 和 g2o::BaseBinaryEdge 自定义边

/// 设计两个顶点类，其中 [a;b] 为一个二维向量顶点类，c 为另一个double顶点类                                        
class CurveFittingVertex1: public g2o::BaseVertex<2, Eigen::Vector2d> // 二维向量顶点类
                                             < 顶点维度，数据类型 > 
{ 
public :
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW 结构体包含eigen成员必须进行宏定义 EIGEN_MAKE_ALIGNED_OPERATOR_NEW, 保证内存对齐
  
  virtual void setToOriginImpl() { _estimate << 0, 0; } //重置

  virtual void oplusImpl(const double *update) { _estimate += Eigen::Vector2d(update); }  顶点更新函数，利用增量更新顶点
  
  virtual bool read(istream &in) {}  无读写操作，留空
  virtual bool write(ostream &out) const {}
};

class CurveFittingVertex2 : public g2o::BaseVertex<1,double>  double顶点类
{
public:
  virtual void setToOriginImpl() { _estimate = 0.0; }
  virtual void oplusImpl(const double *update) { _estimate += update[0]; }
  virtual bool read(istream &in) {}  无读写操作，留空
  virtual bool write(ostream &out) const {}
};

/// 二元边，连接二维向量顶点和double顶点
class CurveFittingEdge: public g2o::BaseBinaryEdge<1, double, CurveFittingVertex1,CurveFittingVertex2>
                                              < 观测值维度，观测值类型，连接的顶点类1，连接的顶点类2 > 
{
public :
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  CurveFittingEdge(double x) : BaseBinaryEdge(), _x(x) {} CurveFittingEdge类的构造函数
  
  void computeError() 计算边所代表的误差
  {
   const CurveFittingVertex1 *v1 = static_cast<const CurveFittingVertex1 *>( _vertices[0] );  static_cast 类型转换，把父类的_vertice 转换为子类的CurveFittingVertex
   const CurveFittingVertex2 *v2 = static_cast<const CurveFittingVertex2 *>( _vertices[1] );
    _vertices[] 为边的成员，存储顶点信息；对二元边，_vertices[] 的大小为2
   
   const Eigen::Vector2d ab = v1->estimate();  获取v1顶点的当前值
   const double c = v2->estimate();  获取v2顶点的当前值
   
  // _error(0,0) = _measurement - std::exp( ab(0,0)*_x*_x + ab(1,0)*_x + c );  也可以采用下述方式赋值
   _error << _measurement - exp(ab[0]*_x*_x + ab[1]*_x + c);  _error 的类型为 Eigen::Matrix<double, D, 1, Eigen::ColMajor>
                                                                _measurement 的类型为声明明边类型时的观测值类型，这里是 double
  }
  
  virtual void linearizeOplus() override 计算雅可比矩阵J,用于计算海塞矩阵 H=J^T*J 和 误差 b=-J^T*_error
  {
    const CurveFittingVertex1 *v1 = static_cast<const CurveFittingVertex1 *>(_vertices[0]);
    const CurveFittingVertex2 *v2 = static_cast<const CurveFittingVertex2 *>(_vertices[1]);
    
    const Eigen::Vector2d ab = v1->estimate();
    const double c = v2->estimate();
    
    double y = exp(ab[0]*_x*_x + ab[1]*_x + c);
    _jacobianOplusXi[0] = -_x*_x*y;  _error 对 ab 的导数，为 1x2 的矩阵
    _jacobianOplusXi[1] = - _x*y;
    _jacobianOplusXj[0] = -y;  _error 对 c 的导数，为 1x1 的矩阵
    
    //** J = [_jacobianOplusXi, _jacobianOplusXj]    
  }
  
  virtual bool read(istream &in) {}
  virtual bool write(ostream &out) const {}
  
public :
  double _x; // x值，y值为 _ measurement
};

int main(int argc, char **argv)
{
  // 生成数据
  double a = 1.0, b = 2.0, c = 1.0; /// 真实参数值
  int N = 100;                      /// 数据点个数
  double w_sigma = 1.0;             /// 噪声Sigma值
  cv::RNG rng;                      /// OpenCV随机数产生器
 
  vector<double> x_data, y_data;    /// 数据序列
 
  cout << "generating data: " << endl;
  for (int i=0; i<N; i++)
  {
    double x = i/100.0; // i 是int型，要除以100.0 保证结果为double
    x_data.push_back(x);
    y_data.push_back( exp(a*x*x + b*x +c) + rng.gaussian(w_sigma) );
    cout << x_data[i] << "\t" << y_data[i] << endl;
  }
  
  // 2. 配置求解器
  /// ref: https://www.jianshu.com/p/36f2eac54d2c
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<2,1>> BlockSolverType;
  /// 定义块求解器，将用于生成 H 和 b 的结构;       pose,landmark：都表示变量的维度
  /// 将生成 H 和 b 结构 ：
  ///                       H = Hpp Hpl   b = bp
  ///                           Hlp Hll       bl
  BlockSolverType::LinearSolverType *linearSolverType = new g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>(); 
  ///定义线性求解器； 这里用到了 Schur 消元，参见第9讲，线性求解器的结构与 Hpp 相同，即 ::PoseMatrixType
    
  BlockSolverType *solver_ptr = new BlockSolverType(linearSolverType); //生成块求解器对象
   
  g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); /// 配置算法
  g2o::SparseOptimizer optimizer; /// 声明图模型（优化器）；
  optimizer.setAlgorithm(solver); /// 把（求解器+算法）导入模型
  optimizer.setVerbose(true); /// 输出到调试
  
  // 3. 添加顶点和边
  CurveFittingVertex1 *v1 = new CurveFittingVertex1();
  v1->setEstimate( Eigen::Vector2d(0,0) ); /// 顶点赋初值
  v1->setId(0);
  optimizer.addVertex(v1); /// 添加顶点，Id为0
   
  CurveFittingVertex2 *v2 = new CurveFittingVertex2();
  v2->setEstimate(0); /// 顶点赋初值
  v2->setId(1);
  v2->setMarginalized(true); /// ! ! ! ! 这里必须要设置边缘化，因为 v2 对应 landmark, 在配置求解器时被消元了
  optimizer.addVertex(v2); /// 添加顶点，Id为1
   
  for (int i=0; i<N; i++) /// 100组数据，100条边
  {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]); ///构造CurveFittingEdge类的对象edge
    edge->setId(i);
    edge->setVertex(0,v1); /// edge连接顶点 v1 和 v2
    edge->setVertex(1,v2);
    edge->setMeasurement(y_data[i]); /// 观测值
    edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );/// 信息矩阵，权重
    optimizer.addEdge(edge);
  }
   
  cout << "Start optimization" << endl;
  
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  // 4. 求解
  optimizer.initializeOptimization();
  optimizer.optimize(100); /// 最大迭代次数100
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
  cout << "Solve time cost = " << time_used.count() << " seconds. " << endl;
   
  Eigen::Vector2d ab_estimate = v1->estimate();
  double c_estimate = v2->estimate();
  cout << "True value of model: a = 1.0, b = 2.0, c = 1.0\n";
  cout << "Estimated model: \n" << "a_estimated = " << ab_estimate[0] 
                                << "\nb_estimated = " << ab_estimate[1]
                                << "\nc_estimated =" << c_estimate << endl;
   
  return 0;  
}