SLAM 入门之《SLAM 14讲》笔记

本人小白一枚，因选了搭建3D虚拟环境的课题需要学习SLAM相关知识，被迫在一个星期之内学完了《SLAM 14讲》，从此感觉到了SLAM的有趣和神奇。为了时常能温习书中所涉及的原理与模型，也为了能帮助广大刚入这个坑的小白们，便想写这篇书中每章内容的摘要与一些知识的理解。

2. 第二讲：初识SLAM

• 视觉SLAM框架
• Linux下程序的编译与运行

2.1. SLAM框架

数据传感器

视觉里程计

回环检测

非线性优化

建图

• 视觉里程计（Visual Odometry，VO）：前端，估算相邻图像间相机的运动。
• 非线性优化（Optimization）：后端，优化得到的相机位姿。
• 回环检测（Loop Closing）：判断相机是否到过先前位置，传给后端以修正误差。
• 建图（Mapping）：根据估计轨迹建立所需的地图。

2.2. Linux下程序的编译

参见 CmkakeLists.txt 语法介绍

---

3. 第三讲：三维空间刚体运动

• 刚体在三维空间里的旋转
• 刚体在三维空间里的旋转加平移

本章代码使用了Eigen须在 “.cpp” 文件开头加上：

#include 
#include 
#include 

3.1. 刚体在三维空间里的旋转

Eigen中矩阵的基本操作：

Eigen::Matrix<type, row, colon> //typedef: Matrix3d, Vector3d...
Eigen::MatrixXd //unkown size matrix
matrix(i, j) //the value of i th row and j th colon
//the operations
matrix.transpose(); matrix.trace(); matrix.sum(); matrix.inverse(); matrix.determinant(); 
//find the proper values and vectors
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(matrix.transport()*matrix);
solver.eigenvalues();
solver.eigenvectors();
//solve Matrix_NN * x = v_Nd
x = matrix_NN.colpivHouseholderQr().solve(v_Nd);

基本数学知识：
$$[xyz{]}^T=\text{p},{\text{p}}^{\Lambda }=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$$

3.1.1. 旋转矩阵 $\text{R}$

设刚体上某点 $\text{x}=[x_1,x_2,x_3{]}^T$，在经过旋转之后变成 ${\text{x}}^{\prime }=[x_1^{\prime },x_2^{\prime },x_3^{\prime }{]}^T$，我们可以用旋转矩阵 $\text{R}$ 来表示该旋转，则有 ${\text{x}}^{\prime }=\text{R}.\text{x}$。

旋转矩阵 $\text{R}$ 是一个正交矩阵（Matrice Orthogonale），即有性质 ${\text{R}}^T\text{R}=\text{I}$.

3.1.2. 旋转向量 $\theta \text{n}$

旋转向量 $\theta \text{n}$意为刚体绕 $\text{n}$ 旋转 $\theta$ 。根据 Rodrigues’s Formula ， 旋转矩阵与旋转向量的转化关系如下:

$$\text{R}=cos(\theta )\text{I}+(1-cos(\theta )\text{n}{\text{n}}^T+sin(\theta ){\text{n}}^{\Lambda })$$

$$\{\begin{array}{c}tr(\text{R})=1+2cos(\theta )\\ \text{R}\text{n}=\text{n}\end{array}$$

Eigen::AngleAxisd rotation_vector(theta, Eigen::Vector3d(x, y, z)); //rotation vector
rotation_matrix = rotation_vector.toRotationMatrix(); //transform rotation vector to rotation matrix
x_rotated = rotation_vector*x; //calculate rotated vector x'
x_rotated = rotation_matrix*x; //calculate rotated vector x'

3.1.3. 四元数（Quaternion）

以下与书中相同，设四元数第一项为实数项。设在三维空间某一点 $\text{p}=[0,x,y,z]$ ，旋转向量 $\theta \text{n}$ 可记为 $\text{q}=[cos(\theta ),\text{n}sin(\theta )]$。

则旋转后的向量 ${\text{p}}^{\prime }=\text{q}\text{p}{\text{q}}^{-1}$。

四元数与旋转矩阵的转换关系如下（并不唯一）：

设 $\text{q}=q_0+q_1\text{i}+q_2\text{j}+q_3\text{k}$

则有 $$\text{R}=\left[\begin{array}{ccc}1-2q_2^2-q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$

或者 $$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$$

q = Eigen::Quaterniond(w, x, y, z) //create a quaternion from four values
q = Eigen::Quaterniond(rotation_vector) //create a quaternion from rotaion vector
q = Eigen::Quaterniond(rotation_matrix) //create a quaternion from rotaion matrix
x_rotated = q * x //calculate rotated vector x'

3.2. 刚体在三维空间里的旋转加平移

3.2.1. 变换矩阵 $\text{T}$

设刚体上某点 $\text{x}=[x_1,x_2,x_3{]}^T$，在经过旋转平移之后变成 ${\text{x}}^{\prime }=[x_1^{\prime },x_2^{\prime },x_3^{\prime }{]}^T$，则有 ${\text{x}}^{\prime }=\text{R}\text{x}+\text{t}$，这里的 $\text{t}$ 为平移向量。

我们记 $\text{T}=\left[\begin{array}{cc}\text{R}& \text{t}\\ {\text{0}}^T& 1\end{array}\right]$，然后把 ${\text{x}}^{\prime }$ 与 $\text{x}$ 写成齐次坐标的形式， 即 $\text{x}=[x_1,x_2,x_3,1{]}^T,{\text{x}}^{\prime }=[x_1^{\prime },x_2^{\prime },x_3^{\prime },1{]}^T$，则有 ${\text{x}}^{\prime }=\text{T}\text{x}$。

//create a transform matrix
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);
/*
T.rotate(rotation_matrix);
T.rotate(quaternion);
*/
T.pretranslate(Eigen::Vector3d(x, y, z));

---

4. 第四讲：李群与李代数

• 李群与李代数
• BCH公式与近似形式
• 扰动模型（左扰动）

本章代码使用了Eigen须在 “.cpp” 文件开头加上：

#include "sophus/so3.h"
#include "sophus/se3.h"

4.1. 李群与李代数

三维旋转矩阵构成了特殊正交群：$SO(3)=\{\text{R}\in {\mathbb{R}}^{3\times 3}|{\text{R}}^T\text{R}=\text{I},det(\text{R})=1\}$，
变换矩阵构成了特殊欧式群：$SE(3)=\{\text{T}=\left[\begin{array}{cc}\text{R}& \text{t}\\ {\text{0}}^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\text{R}\in SO(3),\text{t}\in {\mathbb{R}}^3\}$，

一个小公式： ${\text{a}}^{\Lambda }=\text{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\text{A}}^{\text{V}}=\text{a}$

李代数：
$so(3)=\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\Lambda }\in {\mathbb{R}}^{3\times 3}\}$，
$se(3)=\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in so(3),{\xi }^{\Lambda }=\left[\begin{array}{cc}{\varphi }^{\Lambda }& \rho \\ {\text{0}}^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\}$

指数和对数映射：
$SO(3)$ 与 $so(3)$：

• $exp(\theta {\text{a}}^{\Lambda })=cos(\theta )\text{I}+(1-cos(\theta )\text{a}{\text{a}}^T+sin(\theta ){\text{a}}^{\Lambda })$
• $\theta =arccos\frac{tr(\text{R})-1}{2},\text{R}\text{a}=\text{a}$

$SE(3)$ 与 $se(3)$：

• $exp({\xi }^{\Lambda })=\left[\begin{array}{cc}exp({\varphi }^{\Lambda })& J\rho \\ {\text{0}}^T& 1\end{array}\right],J=\frac{sin(\theta )}{\theta }\text{I}+(1-\frac{sin(\theta )}{\theta })\text{a}{\text{a}}^T+\frac{1-cos(\theta )}{\theta }{\text{a}}^{\Lambda }$
• $\theta =arccos\frac{tr(\text{R})-1}{2},\text{R}\text{a}=\text{a},\text{t}=J\rho$

Sophus::SO3 SO3_R(R); //construct SO_R by a rotation matrix
Sophus::SO3 SO3_v(0, 0, M_PI/2); //construct SO_R by a rotation vector, using  std::M_PI/2
Sophus::SO3 SO3_q(q); //construct SO_R by a quaternion
Eigen::Vector3d so3 = SO3_R.log(); //logarithm map
Sophus::SO3::hat(so3); //so3^
Sophus::SO3::vee(SO3); //SO3v
Sophus::SO3::exp(so3); //exp(so3^)

Sophus::SE3 SE3_Rt(R, t); //construct SE_Rt by a rotation matrix and a translation vector(Eigen::Vector3d)
Sophus::SE3 SE3_qt(q, t); //construct SE_Rt by a quaternion and a translation vector

//how to update
Eigen::Matrix<double, 6, 1> update_se3;
update_se3.setZero();
update_se3(0, 0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;

4.2. BCH公式与近似模型

BCH近似公式：
$$ln(exp({\varphi }_1^{\Lambda })exp({\varphi }_2^{\Lambda }){)}^{\text{V}}\approx \{\begin{array}{c}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+\varphi 2)if{\varphi }_{1}issmall\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+\varphi 1)if{\varphi }_{2}issmall\end{array}$$

$J_l=J=\frac{sin(\theta )}{\theta }\text{I}+(1-\frac{sin(\theta )}{\theta })\text{a}{\text{a}}^T+\frac{1-cos(\theta )}{\theta }{\text{a}}^{\Lambda }$，

$J_l^{-1}=\frac{\theta }{2}cot(\frac{\theta }{2})\text{I}+(1-\frac{\theta }{2}cot(\frac{\theta }{2}))\text{a}{\text{a}}^T-\frac{\theta }{2}{\text{a}}^{\Lambda }$，

$J_r(\varphi )=J_l(-\varphi )$

微小扰动公式：

$exp(\Delta {\varphi }^{\Lambda })exp({\varphi }^{\Lambda })=exp((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\Lambda })$

$exp((\varphi +\Delta \varphi {)}^{\Lambda })=exp((J_l\Delta \varphi {)}^{\Lambda })exp({\varphi }^{\Lambda })=exp({\varphi }^{\Lambda })exp((J_r\Delta \varphi {)}^{\Lambda })$

$exp(\Delta {\xi }^{\Lambda })exp({\xi }^{\Lambda })=exp((\xi +{\mathcal{J}}_l^{-1}\Delta \xi {)}^{\Lambda })$

$exp({\xi }^{\Lambda })exp(\Delta {\xi }^{\Lambda })=exp((\xi +{\mathcal{J}}_r^{-1}\Delta \xi {)}^{\Lambda })$

4.3. 扰动模型（左扰动）

$SO(3)$ 上的李代数求导：

$$\frac{\partial \text{R}\text{p}}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\Lambda })exp({\varphi }^{\Lambda })\text{p}-exp({\varphi }^{\Lambda })\text{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{(1+{\phi }^{\Lambda })exp({\varphi }^{\Lambda })\text{p}-exp({\varphi }^{\Lambda })\text{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{{\phi }^{\Lambda }\text{R}\text{p}}{\phi }=-(\text{R}\text{p}{)}^{\Lambda }$$

$SE(3)$ 上的李代数求导：

$$\frac{\partial \text{T}\text{p}}{\partial \delta \xi }=\left[\begin{array}{cc}\text{I}& -(\text{R}\text{p}+\text{t}{)}^{\Lambda }\\ {\text{0}}^T& {\text{0}}^T\end{array}\right]$$

---

5. 第五讲：相机与图像

• 相机模型
• 相机的标定

本章要用到OpenCV，PCL与boost，PCL用于拼接云点图，OpenCV的具体用法请参照OpenCV官网。

使用boost快速读取图片：

#include 
boost::format fmt(./%s/%d.%s);
cv::imread((fmt%"director_name"%(number)%"file_type").str(), -1);

5.1. 相机模型

5.1.1. 针孔相机模型

设像素平面上某点 ${\text{P}}_{uv}=[u,v{]}^T$，其对应的在世界坐标系下的某点 ${\text{P}}_w=[X,Y,Z{]}^T$，在相机归一化平面上对应的点为 ${\text{P}}_c=[x,y,1{]}^T$。

我们有：
$$\{\begin{array}{c}u=f_x\frac{X}{Z}+c_x\\ \\ v=f_y\frac{Y}{Z}+c_y\end{array}$$

即使 ${\text{P}}_{uv}=[u,v{]}^T$ 化为齐次形式 ${\text{P}}_{uv}=[u,v,1{]}^T$，

$$Z{\text{P}}_{uv}=\text{K}\text{T}{\text{P}}_w$$
或者：
$${\text{P}}_{uv}=\text{K}{\text{P}}_c$$
在这两个式子中 $\text{K}$ 为相机的内参矩阵（Intrinsic） 即为 $\text{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$。

在G2O中内参 $f_x$ 与 $f_y$ 均为 $f$。

5.1.2. 畸变

径向畸变：
$$\{\begin{array}{c}{x}_{corrected}=x(1+k_1{r}^2+k_2{r}^2+k_3{r}^6)\\ \\ y_{corrected}=y(1+k_1{r}^2+k_2{r}^2+k_3{r}^6)\end{array}$$
则有：
$$\{\begin{array}{c}u=f_x{x}_{corrected}+c_x\\ \\ v=f_y{y}_{corrected}+c_y\end{array}$$
这里$x$，$y$，$x_{corrected}$，$y_{corrected}$ 均为归一平面上的点。

切向畸变：
$$\{\begin{array}{c}{x}_{corrected}=x+2p_{1}xy+p_2(r^2+2x^2)\\ \\ y_{corrected}=y+p_1(r^2+2y^2)+2p_{2}xy\end{array}$$

5.2. 相机的标定

具体原理参见：
摄像机内参标定《A Flexible New Technique for Camera Calibration》；
摄像机-激光雷达静态外参联合标定《Extrinsic calibration of a camera and laser range finder (improves camera calibration)》；
注意结合运动信息，物体的运动与激光雷达的旋转扫描同时发生；
运动补偿激光雷达与相机之间的标定；

因为某些原因，我现在只做了手机相机的标定，在这里就不详述了，准备令写一篇文章记录一下相机标定的过程。用Matlab或者OpenCV都能简单的实现单目相机的标定。

---

6. 第六讲：非线性优化

• 状态估计问题
• 最小二乘问题
• 用Ceres与G2O求解最小二乘问题

本章为《SLAM 14讲》中最后的与数学理论相关的章节，也是之后用的最多的一部分。

6.1. 状态估计问题

对于经典SLAM模型，它由一个状态方程和运动方程构成：
$$\{\begin{array}{c}{\text{x}}_k=f({\text{x}}_{k-1},{\text{u}}_k)+{\text{w}}_k\\ \\ {\text{z}}_{k,j}=h({\text{y}}_j,{\text{x}}_k)+{\text{v}}_{k,j}\end{array}$$

相机位姿为 $x_k$，可以用 ${\text{T}}_k$ 或是 $exp({\xi }_k^{\Lambda })$ 表示。路标位置为 $y_j$ 。$w_k$ 和 $v_{k,j}$ 为噪声项，且满足高斯分布：
$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$$
把所有待估计的变量放到一个“状态变量”中：
$$x=\{x_1,...,x_N,y_1,...,y_M\}$$

则即是估计 $P(x|z,u)$ 的概率分布，或是估计 $P(x|z)$ 的概率分布，我们认为大多数情况下我们没有测量运动的传感器 ，只考虑观测方程带来的数据。

根据叶贝斯法则，我们有：
$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x)$$

后验概率最大化（Maximize a Posterior）：
$$x_{MAP}^{\ast }=argmaxP(x|z)=argmaxP(z|x)P(x)$$

最大似然估计（Maximize Likelyhood Estimation）：
$$x_{MLE}^{\ast }=argmaxP(z|x)$$

6.2. 最小二乘问题

6.2.1. 引出

高维高斯分布：$P(\text{x})=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(\text{x}-\mu {)}^T{\Sigma }^{-1}(\text{x}-\mu ))$

对于之前的求最大似然估计的问题即为：
$$x^{\ast }=argmin((z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{z,j}-h(x_k,y_j)))$$

因此对于所有的运动和任意的观测，我们定义数据与估计值之间的误差：
$$\begin{gathered} e_{v,k}=x_k-f(x_{k-1},u_k) \\ {e}_{y,j,k}=z_{k,j}-h(x_k,y_j) \end{gathered}$$

并求该误差的平方和：
$$J(x)=\sum _k{e}_{v,k}^T{R}_k^{-1}{e}_{v,k}+\sum _k\sum _j{e}_{y,k,j}^T{Q}_{k,j}^{-1}{e}_{y,k,j}$$
于是我们得到了一个总体意义下的最小二乘问题！

6.2.2. 非线性最小二乘问题的一般解法 - 迭代法

下面以最简单的最小二乘问题为例：${\min }_x\frac{1}{2}||f(x)|{|}_2^2$

1. 给定某个初值 $x_0$ 。
2. 对于第 $k$ 次迭代，寻找增量 $\Delta x_k$ ，使得 $||f(x_k+\Delta x_k)|{|}_2^2$ 最小。
3. 若 $\Delta x_k$ 足够小，则停止。
4. 否则，令 $x_{k+1}=x_k+\Delta x_k$ ，返回到 2 。

6.2.3. 一阶和二阶梯度法

将目标函数在 $x$ 附近展开：
$$||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x$$

保留一阶梯度： $\Delta x^{\ast }=-J^T(x)$

保留二阶梯度： $H\Delta x=-J^T$

6.2.4. Gauss-Newton

将 $f(x)$进行一阶泰勒展开：
$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$

代入最小二乘问题的式子便得到了：
$$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x)$$

或把左右两边系数分别定义为 $H$ 和 $g$，则有：
$$H\Delta x=g$$

Gauss-Newton 的算法步骤如下：

1. 给定某个初值 $x_0$ 。
2. 对于第 $k$ 次迭代，求出雅可比矩阵 $J(x_k)$ 和误差 $f(x_k)$ 。
3. 求解增量方程：$H\Delta x=g$ 。
4. 若 $\Delta x_k$ 足够小，则停止，否则，令 $x_{k+1}=x_k+\Delta x_k$ ，返回到 2 。

6.2.5. Levenberg-Marquadt

使用 $\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$ 来判断泰勒近似是否够好。$\rho$ 接近于 1 ，则近似很好。如果 $\rho$ 太小， 则实际减小的值远小于近似减小的值，则需要缩小近似范围。反之，则说明实际下降的比预计的更大，我们要放大近似的范围。

Levenberg-Marquadt 的算法步骤如下：

1. 给定某个初值 $x_0$ 。
2. 对于第 $k$ 次迭代，求解：
   $$\underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^2，s.t.||D\Delta x_k|{|}^2⩽\mu$$这里 $\mu$ 是信赖区间半径，$D$ 为对角矩阵（Diagonal）把增量限制在椭球中。
3. 计算$\rho$ 。
4. 若 $\rho >\frac{3}{4}$ ，则 $\mu =2\mu$ 。
5. 若 $\rho <\frac{1}{4}$ ，则 $\mu =\frac{1}{2}\mu$ 。
6. 如果 $\rho$ 大于某阈值，则认为近似可行。令 $x_{k+1}=x_k+\Delta x_k$ 。
7. 判断算法是否收敛。如不收敛则返回到 2 。

对于 2 ，我们也可以求解：
$$\underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^2+\frac{\lambda }{2}||D\Delta x_k|{|}^2$$也相当于是计算增量的线性方程：
$$(H+\lambda D^{T}D)\Delta x=g$$

6.3. 用Ceres与G2O求解最小二乘问题

6.3.1. Ceres

详见Ceres官网。

栗子：$y=exp(a{x}^2+bx+c)$ 函数的拟合

#include 
#include 
#include 

// Cost Function Model
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    //calculate residual
    template <typename T>
    bool operator() (
        const T* const abc, // Model parameter, 3 dimensions
        T* residual ) const // residual
    {
        residual[0] = T ( _y ) - ceres::exp ( ab[0]*T ( _x ) *T ( _x ) + ab[1]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y datas
};

...

// construct least square problem
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
    ceres::CostFunction *costfunction = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 2, 1> (new CURVE_FITTING_COST (x_data[i], y_data[i]));
    problem.AddResidualBlock ( costfunction,
        new ceres::CauchyLoss(0.5), // core function
        abc  // estimate parameters
    );
}
    
// set solver
ceres::Solver::Options options; 
options.linear_solver_type = ceres::DENSE_QR;  // use QR to solve update function
options.minimizer_progress_to_stdout = true;   // output to cout

ceres::Solver::Summary summary;                // optimization information
ceres::Solve ( options, &problem, &summary );  // begin to optimize

// output the result
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
for ( auto a:abc ) cout<<a<<" ";

6.3.2. G2O

同样的栗子：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

//set vertex, template: dimension, type of optimized parameter
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl()
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // update
    {
        _estimate += Eigen::Vector3d(update);
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

//error Model template parameter：observed datas' dimension，type，type of linked vertex
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // compute error
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x value， y is _measurement
};

...

// construct optimization graph
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // dimension of optimizing parameter is 3, error is 1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // linear equation solver
Block* solver_ptr = new Block( linearSolver ); // Matrix block solver
// Method: Gauss-Newton, LB, Dogleg
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer;     // graph model
optimizer.setAlgorithm( solver );   // set solver
optimizer.setVerbose( true );       // set output verbose
    
// add vertex to the graph
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
    
// add eges to the graph
for ( int i=0; i<N; i++ )
{
	CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
    edge->setId(i);
    edge->setVertex( 0, v );                // set linked vertex
    edge->setMeasurement( y_data[i] );      // set mesurement
    edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix
    optimizer.addEdge( edge );
}
    
// optimizing
cout<<"start optimization"<<endl;
optimizer.initializeOptimization();
optimizer.optimize(100);
// output the result
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

---

7. 第七 - 八 - 九讲：视觉里程计

• 特征点法
• 2D-2D：对极几何
• 3D-2D：PnP
• 3D-3D：ICP
• 直接法
• 设计前端

这部分主要介绍了视觉里程计中的一些模型，即如何从一堆照片中求解相机的位姿，从而求出相机大概的运动轨迹，以求得空间点的大致坐标。

7.1. 特征点法

特征点由关键点和描述子两部分组成。常用的ORB特征由FAST角点和BRIEF描述子组成。特征点的原理在此就不复述了，用OpenCV就可以简单的匹配到两张图片之间的特征点。

具体代码见 "slambook/cha7/feature_extraction.cpp"

7.2. 2D-2D：对极几何

对极约束：
$${\text{x}}_2^T{\text{t}}^{\Lambda }\text{R}{\text{x}}_1=0$$

这里式子中的 $x_i$ 均是归一化平面上的坐标，重新代入 $p_1,p_2$ 有：
$${\text{p}}_2^T{\text{K}}^{-T}{\text{t}}^{\Lambda }\text{R}{\text{K}}^{-1}{\text{p}}_1=0$$

我们把中间部分记作两个矩阵：基础矩阵 $\text{F}$ 和本质矩阵 $\text{E}$，进一步简化对极约束：
$$\text{E}={\text{t}}^{\Lambda }\text{R},\text{F}={\text{K}}^{-T}\text{E}{\text{K}}^{-1},{\text{x}}_2^T\text{E}{\text{x}}_1={\text{p}}_2^T\text{F}{\text{p}}_1=0$$

我们可以通过八点法求得 $\text{E}$ ，再通过奇异值（SVD）分解即可求得 $\text{t}$ 与 $\text{R}$ 。

单应矩阵 $\text{H}$ 描述了平面到平面之间的映射关系，在单目相机的标定中有用到：${\text{p}}_2=\text{H}{\text{p}}_1$ ，也可以用类似八点法的方式来确定。

三角测量： 对于两个归一化平面上的点 ${\text{x}}_1$ 和 ${\text{x}}_2$ ，那么它们满足：
$$s_1{\text{x}}_1=s_2\text{R}{\text{x}}_2+\text{t}$$稍微转化则有：
$$s_1{\text{x}}_1^{\Lambda }{\text{x}}_1=0=s_2{\text{x}}_1^{\Lambda }\text{R}{\text{x}}_2+{\text{x}}_1^{\Lambda }\text{t}$$

于是便可以求出深度 $s_2$ ，用同样的方法也可以求出 $s_1$ 。另外，三角测量是由平移得到的，纯旋转无法使用三角测量。

7.3. 3D-2D：PnP

7.3.1. DLT：直接线性变换

直接从某个空间点 $\text{P}=(X,Y,Z,1{)}^T$ ，和与之对应的特征点（归一化平面） ${\text{x}}_1=(u_1,v_1,1{)}^T$ ，利用 $\text{x}=\text{R}\text{P}+\text{t}$来求解相机的位姿。

7.3.2. P3P

根据相似三角形，我们很容易可以求出：
$$\begin{gathered} (1-u)y^2-u{x}^2-cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0 \\ (1-w)x^2-w{y}^2-cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0 \end{gathered}$$

7.3.3. Bundle Adjustment

构建最小二乘问题，使重投影误差最小：
$${\xi }^{\ast }=arg\underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n||{\text{u}}_i-\frac{1}{s_i}\text{K}exp({\xi }^{\Lambda }){\text{P}}_i|{|}_2^2$$

由 $e(x+\Delta x)\approx e(x)+J\Delta x$ 可知，我们需要求 $J$ ， 在这里 $J$ 应该是一个 $2\times 6$ 的矩阵。令 ${\text{P}}^{\prime }=(exp({\xi }^{\Lambda })\text{P}{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$ 。
$$\frac{\partial \text{e}}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_{x}Y}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$
$$\frac{\partial \text{e}}{\partial \text{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\text{R}$$

之后便可以用G2O求解这个最小二乘问题。

7.4. 3D-3D：ICP

构建的最小二乘问题为 ${\min }_{\text{R},\text{t}}J=\frac{1}{2}{\sum }_{i=1}^n||({\text{p}}_i-(\text{R}{\text{p}}_i^{\prime }+\text{t}))|{|}_2^2$ 。

7.4.1. SVD方法

1. 计算两组点的质心位置 $\text{p},{\text{p}}^{\prime }$ ，然后计算每个点的去质心坐标：
   $${\text{q}}_i={\text{p}}_i-\text{p},{\text{q}}_i^{\prime }={\text{p}}_i^{\prime }-{\text{p}}^{\prime }$$
2. 根据 $${\text{R}}^{\ast }=arg\underset{\text{R}}{\min }\frac{1}{2}\sum _{i=1}^n||{\text{q}}_i-\text{R}{\text{q}}_i^{\prime }|{|}^2=arg\underset{\text{R}}{\min }\frac{1}{2}\sum _{i=1}^n{\text{q}}_i^T{\text{q}}_i+{\text{q}}_i^{\prime T}{\text{q}}_i^{\prime }-2{\text{q}}_i^T\text{R}{\text{q}}_i^{\prime }=arg\underset{\text{R}}{\min }-tr(\text{R}\sum _{i=1}^n{\text{q}}_i^{\prime }{\text{q}}_i^T)$$ 计算旋转矩阵。
3. 根据旋转矩阵计算 $\text{t}$ 。

7.4.2. BA方法

求导式子为：
$$\frac{\partial \text{e}}{\partial \delta \xi }=-\left[\begin{array}{cc}\text{I}& -(exp({\xi }^{\Lambda }){\text{p}}_i^{\prime }{)}^{\Lambda }\\ \text{0}& \text{0}\end{array}\right]$$

7.5. 直接法

7.5.1. 引出

为了克服特征点法的一些缺点，比如：耗时，丢弃了大部分可能有用的图像信息，相机有时会运动到特征缺失的地方，我们有以下几种思路：

• 只计算关键点，不计算描述子。使用 光流法（Optical Flow） 来跟踪特征点的运动。
• 只计算关键点，不计算描述子。使用 直接法（Direct Method） 来计算特征点在下一时刻图像的位置。
• 根据像素灰度的差异，直接计算相机的运动。

在直接法（后两种）中我们通过最小化 光度误差（Photometric error） 来优化相机的运动。

7.5.2. Lucas-Kanade 光流

灰度不变假设： 同一个空间点的像素灰度值，在各个图像中是固定不变的。

设
$${\text{I}}_x=\frac{\partial \text{I}}{\partial x},{\text{I}}_y=\frac{\partial \text{I}}{\partial y},u=\frac{dx}{dt},v=\frac{dy}{dt},{\text{I}}_t=\frac{\partial \text{I}}{\partial t}$$

根据灰度不变假设，我们有:
$${\text{I}}_{x}u+{\text{I}}_{y}v=-{\text{I}}_{t}or\left[\begin{array}{cc}{\text{I}}_x& {\text{I}}_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-{\text{I}}_t$$

令：
$$\text{A}=\left[\begin{array}{c}{\left[\begin{array}{cc}{\text{I}}_x& {\text{I}}_y\end{array}\right]}_1\\ ...\\ {\left[\begin{array}{cc}{\text{I}}_x& {\text{I}}_y\end{array}\right]}_k\end{array}\right],\text{b}=\left[\begin{array}{c}{\text{I}}_{t1}\\ ...\\ {\text{I}}_{t2}\end{array}\right]$$

则有：
$$\text{A}\left[\begin{array}{c}u\\ v\end{array}\right]=-\text{b}$$求其最小二乘解即可。

7.5.3. 直接法

光度误差 记为：
$$e={\text{I}}_1({\text{p}}_1)-{\text{I}}_2({\text{p}}_2)$$

对于多个空间点 $P_i$ ，相机位姿估计问题变为：
$$\underset{\xi }{\min }J(\xi )=\sum _{i=1}^N{e}_i^T{e}_i,e_i={\text{I}}_1({\text{p}}_{1,i})-{\text{I}}_2({\text{p}}_{2,i})$$

误差相对于李代数的雅可比矩阵为：
$$J=-\frac{\partial {\text{I}}_2}{\partial \text{u}}\frac{\partial \text{u}}{\partial \delta \xi }$$

7.6. 设计前端

详见《SLAM 14讲》：第九讲 - 视觉里程计完整实现过程 1.0。

---

8. 第十 - 十一讲：后端

• KF滤波和EKF滤波
• BA 与图优化
• 位姿图的优化
• 因子图优化

8.2. BA与图优化

详见这篇文章：SLAM后端：优化空间点、相机位姿与参数 - 《SLAM 14讲》第十讲

---

9. 第十二讲：回环检测

• 基本概念
• 词袋模型与字典
• 相似度计算

9.1. 基本概念

回环检测结果分类：

算法 \ 事实	是回环	不是回环
是回环	真阳性（True Positive）	假阳性（False Postive）
不是回环	假阴性（False Negative）	真阴性（True Negative）

准确率： $Precision=\frac{TP}{TP+FP}$

召回率： $Recall=\frac{TP}{TP+FN}$

为了评价算法的好坏，我测试它在各种配置下的 $P$ 值和 $R$ 值，然后做出一条 Precision-Recall 曲线。

9.2. 词袋模型和字典

K-means 的步骤：

1. 随机取 $k$ 个中心点：$c_1,..c_k$ ；
2. 对每一个样本,计算与每个中心点之间的距离,取最小的作为它的归类；
3. 重新计算每个类的中心点。
4. 如果每个中心点都变化很小,则算法收敛,退出;否则返回 1。

用 $k$ 叉树来表达字典的步骤：

1. 在根节点,用 K-means 把所有样本聚成 $k$ 类(实际中为保证聚类均匀性会使用k-means++)。这样得到了第一层。
2. 对第一层的每个节点,把属于该节点的样本再聚成 $k$ 类,得到下一层。
3. 依此类推,最后得到叶子层。叶子层即为所谓的 Words。

用 BoW 创建字典：

#include "DBoW3/DBoW3.h"
#include 
#include 
#include 
#include 
#include 
#include 
using namespace cv;
using namespace std;
int main( int argc, char** argv )
{
    // read the image 
    cout<<"reading images... "<<endl;
    vector<Mat> images; 
    for ( int i=0; i<10; i++ )
    {
        string path = "./data/"+to_string(i+1)+".png";
        images.push_back( imread(path) );
    }
    // detect ORB features
    cout<<"detecting ORB features ... "<<endl;
    Ptr< Feature2D > detector = ORB::create();
    vector<Mat> descriptors;
    for ( Mat& image:images )
    {
        vector<KeyPoint> keypoints; 
        Mat descriptor;
        detector->detectAndCompute( image, Mat(), keypoints, descriptor );
        descriptors.push_back( descriptor );
    }
    
    // create vocabulary 
    cout<<"creating vocabulary ... "<<endl;
    DBoW3::Vocabulary vocab;
    vocab.create( descriptors );
    cout<<"vocabulary info: "<<vocab<<endl;
    vocab.save( "vocabulary.yml.gz" );
    cout<<"done"<<endl;    
    return 0;
}

9.3. 相似度计算

TF-IDF（Term Frequency-Inverse Document Frequency）：

我们统计某个叶子节点 $w_i$ 中的特征数量相对于所有特征数量的比例,作为 IDF 部分。假设所有特征数量为 $n$，$w_i$ 数量为 $n_i$ ，那么该单词的 IDF 为：
$$ID{F}_i=\log (\frac{n}{n_i})$$

TF 部分则是指某个特征在单个图像中出现的频率。假设图像 A 中,单词 $w_i$ 出现了 $n_i$ 次,而一共出现的单词次数为 $n$，那么 TF 为：
$$T{F}_i=\frac{n_i}{n}$$

于是 $w_i$ 的权重等于 TF 乘 IDF 之积：
$${\eta }_i=T{F}_i\times ID{F}_i$$

于是对于图像 A 我们便可以这样描述：
$$A=\{(w_1,{\eta }_1),...,(w_N,{\eta }_N)\}={\text{v}}_A$$

则对于给定的 ${\text{v}}_A$ 与 ${\text{v}}_B$ 我们可以这样计算他们的差异：
$$s({\text{v}}_A-{\text{v}}_B)=2\sum _{i=1}^N|{\text{v}}_{Ai}|+|{\text{v}}_{Bi}|-|{\text{v}}_{Ai}-{\text{v}}_{Bi}|$$

*这部分代码就不详述了

关键帧的处理：

这部分还没有细究 ，曾经看到过一篇论文，不知道有没有用：《一种基于时空切片的SLAM关键帧提取方法》

---

10. 第十三讲：建图

终于写到了最后的最后，SLAM 的最终目的。《SLAM 十四讲》中这章占比很小，也只是介绍了几种滤波器，然后给了几个栗子。因为项目用的是RGB-D相机，所以对于建图部分，我略去了单目相机的部分。

理由同 9 ，这部分还未开始实践，暂且留空。