最近看完了《视觉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;
}