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 <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

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);

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

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

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

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

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

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

$\left\{\begin{matrix} tr(\textbf{R})=1+2cos(\theta)\\ \textbf{R}\textbf{n}=\textbf{n} \end{matrix}\right.$

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）

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

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

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

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

则有 $\textbf{R}=\begin{bmatrix} 1-2q_2^2-q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3+2q_0q_1 \\2q_1q_3+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2\end{bmatrix}$

或者 $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. 变换矩阵 $\textbf{T}$

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

我们记 $\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1\end{bmatrix}$，然后把 $\textbf{x}'$ 与 $\textbf{x}$ 写成齐次坐标的形式， 即 $\textbf{x}=[x_1, x_2,x_3,1]^T,\textbf{x}'=[x_1', x_2', x_3',1]^T$，则有 $\textbf{x}'=\textbf{T}\textbf{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)=\{\textbf{R}\in\mathbb{R}^{3\times3} | \textbf{R}^T\textbf{R}=\textbf{I},det(\textbf{R})=1\}$，
变换矩阵构成了特殊欧式群： $SE(3)=\{\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1\end{bmatrix}\in\mathbb{R}^{4\times4}|\textbf{R}\in SO(3), \textbf{t} \in \mathbb{R}^3\}$，

一个小公式： $\textbf{a}^\Lambda=\textbf{A}=\begin{bmatrix}0 & -a_3 & a_2\\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0\end{bmatrix}, \textbf{A}^\textbf{V} =\textbf{a}$

李代数：
$so(3)=\{\phi\in\mathbb{R}^3,\Phi=\phi^\Lambda\in\mathbb{R}^{3\times3}\}$，
$se(3)=\{\xi=\begin{bmatrix} \rho \\ \phi\end{bmatrix}\in\mathbb{R}^6, \rho\in \mathbb{R}^3,\phi\in so(3), \xi^\Lambda=\begin{bmatrix}\phi^\Lambda & \rho \\ \textbf{0}^T & 0\end{bmatrix}\in\mathbb{R}^{4\times4}\}$

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

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

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

• $exp(\xi^\Lambda)=\begin{bmatrix}exp(\phi^\Lambda) & J\rho \\ \textbf{0}^T & 1\end{bmatrix},J=\frac{sin(\theta)}{\theta}\textbf{I}+(1-\frac{sin(\theta)}{\theta})\textbf{a}\textbf{a}^T+\frac{1-cos(\theta)}{\theta}\textbf{a}^\Lambda$
• $\theta=arccos\frac{tr(\textbf{R})-1}{2}, \textbf{R}\textbf{a}=\textbf{a},\textbf{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 <cmath> 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(\phi_1^\Lambda)exp(\phi_2^\Lambda))^{\textbf{V}}\approx \left\{\begin{matrix} J_l(\phi_2)^{-1}\phi_1+\phi2) \ if \ \phi_1\ is\ small\ \\ J_r(\phi_1)^{-1}\phi_2+\phi1) \ if \ \phi_2\ is\ small\ \end{matrix}\right.$

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

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

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

微小扰动公式：

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

$exp((\phi+\Delta\phi)^\Lambda)=exp((J_l\Delta\phi)^\Lambda)exp(\phi^\Lambda)=exp(\phi^\Lambda)exp((J_r\Delta\phi)^\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 \textbf{R}\textbf{p}}{\partial \varphi}=\lim_{\varphi\rightarrow 0}\frac{exp(\varphi^\Lambda)exp(\phi^\Lambda)\textbf{p}-exp(\phi^\Lambda)\textbf{p}}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{(1+\varphi^\Lambda)exp(\phi^\Lambda)\textbf{p}-exp(\phi^\Lambda)\textbf{p}}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{\varphi^\Lambda\textbf{R}\textbf{p}}{\varphi}=-(\textbf{R}\textbf{p})^\Lambda$

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

$\frac{\partial \textbf{T}\textbf{p}}{\partial \delta\xi}=\begin{bmatrix}\textbf{I} & -(\textbf{R}\textbf{p}+\textbf{t})^\Lambda\\ \textbf{0}^T & \textbf{0}^T\end{bmatrix}$

---

5. 第五讲：相机与图像

• 相机模型
• 相机的标定

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

使用boost快速读取图片：

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

5.1. 相机模型

5.1.1. 针孔相机模型

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

我们有：
$\left\{\begin{matrix} u=f_x \frac{X}{Z}+c_x\\ \\ v=f_y\frac{Y}{Z}+c_y \end{matrix}\right.$

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

$Z\textbf{P}_{uv}=\textbf{K}\textbf{T}\textbf{P}_w$
或者：
$\textbf{P}_{uv}=\textbf{K}\textbf{P}_c$
在这两个式子中 $\textbf{K}$ 为相机的内参矩阵（Intrinsic） 即为 $\textbf{K}=\begin{bmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\0 & 0 & 1\end{bmatrix}$。

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

5.1.2. 畸变

径向畸变：
$\left\{\begin{matrix} x_{corrected}=x(1+k_1r^2+k_2r^2+k_3r^6)\\ \\ y_{corrected}=y(1+k_1r^2+k_2r^2+k_3r^6) \end{matrix}\right.$
则有：
$\left\{\begin{matrix} u=f_x x_{corrected}+c_x\\ \\ v=f_yy_{corrected}+c_y \end{matrix}\right.$
这里 $x$， $y$， $x_{corrected}$， $y_{corrected}$ 均为归一平面上的点。

切向畸变：
$\left\{\begin{matrix} x_{corrected}=x+2p_1xy+p_2(r^2+2x^2)\\ \\ y_{corrected}=y+p_1(r^2+2y^2)+2p_2xy \end{matrix}\right.$

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模型，它由一个状态方程和运动方程构成：
$\left\{\begin{matrix} \textbf{x}_{k}=f(\textbf{x}_{k-1}, \textbf{u}_k)+\textbf{w}_k\\ \\ \textbf{z}_{k,j}=h(\textbf{y}_{j}, \textbf{x}_k)+\textbf{v}_{k,j} \end{matrix}\right.$

相机位姿为 $x_k$，可以用 $\textbf{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}=arg\ max\ P(x|z) =arg\ max\ P(z|x)P(x)$

最大似然估计（Maximize Likelyhood Estimation）：
$x^*_{MLE}=arg\ max\ P(z|x)$

6.2. 最小二乘问题

6.2.1. 引出

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

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

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

并求该误差的平方和：
$J(x)=\sum_{k}e_{v,k}^TR_k^{-1}e_{v,k}+\sum_{k}\sum_{j}e_{y,k,j}^TQ_{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^TH\Delta x$

保留一阶梯度： $\Delta x^*=-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)^TJ(x)\Delta x=-J(x)^Tf(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$ 次迭代，求解：
   $\min_{\Delta x_k} \frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2， s.t.||D\Delta x_k||^2 \leqslant \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 ，我们也可以求解：
$\min_{\Delta x_k} \frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2+\frac{\lambda}{2}||D\Delta x_k||^2$也相当于是计算增量的线性方程：
$(H+\lambda D^TD)\Delta x=g$

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

6.3.1. Ceres

详见Ceres官网。

栗子： $y=exp(ax^2+bx+c)$ 函数的拟合

#include <iostram>
#include <opencv2/core/.hpp>
#include <ceres/ceres.h>

// 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 <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
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：对极几何

对极约束：
$\textbf{x}^T_2\textbf{t}^\Lambda \textbf{R}\textbf{x}_1=0$

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

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

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

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

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

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

7.3. 3D-2D：PnP

7.3.1. DLT：直接线性变换

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

7.3.2. P3P

根据相似三角形，我们很容易可以求出：
$(1-u)y^2-ux^2-cos\left \langle b,c \right \rangle y+2uxy \ cos\left \langle a,b\right \rangle+1=0\\ (1-w)x^2-wy^2-cos\left \langle a,c \right \rangle x+2wxy \ cos\left \langle a,b\right \rangle+1=0$

7.3.3. Bundle Adjustment

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

由 $e(x+\Delta x) \approx e(x) +J\Delta x$ 可知，我们需要求 $J$ ， 在这里 $J$ 应该是一个 $2\times6$ 的矩阵。令 $\textbf{P}'=(exp(\xi^\Lambda)\textbf{P})_{1:3}=[X',Y',Z']^T$ 。
$\frac{\partial\textbf{e}}{\partial\delta\xi}=-\begin{bmatrix}\frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2} & f_x+\frac{f_xX^2}{Z'^2} & -\frac{f_xY}{Z'}\\0 & \frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'}\end{bmatrix}$
$\frac{\partial\textbf{e}}{\partial\textbf{P}}=-\begin{bmatrix}\frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{bmatrix}\textbf{R}$

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

7.4. 3D-3D：ICP

构建的最小二乘问题为 $\min_{\textbf{R},\textbf{t}} J=\frac{1}{2}\sum_{i=1}^{n}||(\textbf{p}_i-(\textbf{R}\textbf{p}'_i+\textbf{t}))||_2^2$ 。

7.4.1. SVD方法

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

7.4.2. BA方法

求导式子为：
$\frac{\partial\textbf{e}}{\partial\delta\xi}=-\begin{bmatrix}\textbf{I} & -(exp(\xi^\Lambda)\textbf{p}'_i)^\Lambda\\\textbf{0}&\textbf{0}\end{bmatrix}$

7.5. 直接法

7.5.1. 引出

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

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

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

7.5.2. Lucas-Kanade 光流

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

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

根据灰度不变假设，我们有:
$\textbf{I}_xu+\textbf{I}_yv=-\textbf{I}_t\ \ \ or \ \ \ \begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}\begin{bmatrix}u\\v\end{bmatrix}=-\textbf{I}_t$

令：
$\textbf{A}=\begin{bmatrix}\begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}_1\\...\\\begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}_k\end{bmatrix}, \textbf{b}=\begin{bmatrix}\textbf{I}_{t1}\\...\\\textbf{I}_{t2}\end{bmatrix}$

则有：
$\textbf{A} \begin{bmatrix}u\\v\end{bmatrix}=-\textbf{b}$求其最小二乘解即可。

7.5.3. 直接法

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

对于多个空间点 $P_i$ ，相机位姿估计问题变为：
$\min_{\xi}J(\xi)=\sum_{i=1}^Ne_i^Te_i,\ \ \ \ \ e_i=\textbf{I}_1(\textbf{p}_{1,i})-\textbf{I}_2(\textbf{p}_{2,i})$

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

7.6. 设计前端

书中这部分为第九讲内容。因为我只是粗略的看了一下，并没有仔细的看每一个代码，故先暂且略过，以后在自己实践时再慢慢补全。

---

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 <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>
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 为：
$IDF_i=\log(\frac{n}{n_i})$

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

于是 $w_i$ 的权重等于 TF 乘 IDF 之积：
$\eta_i=TF_i\times IDF_i$

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

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

*这部分代码就不详述了

关键帧的处理：

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

---

10. 第十三讲：建图

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

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