Simon's《SLAM 14讲》笔记

2. 第二讲：初识SLAM
- 2.1. SLAM框架
- 2.2. Linux下程序的编译
3. 第三讲：三维空间刚体运动
- 3.1. 刚体在三维空间里的旋转
- - 3.1.1. 旋转矩阵 R\textbf{R}R
  - 3.1.2. 旋转向量 θn\theta\textbf{n}θn
  - 3.1.3. 四元数（Quaternion）
- 3.2. 刚体在三维空间里的旋转加平移
- - 3.2.1. 变换矩阵 T\textbf{T}T
4. 第四讲：李群与李代数
- 4.1. 李群与李代数
- 4.2. BCH公式与近似模型
- 4.3. 扰动模型（左扰动）
5. 第五讲：相机与图像
- 5.1. 相机模型
- - 5.1.1. 针孔相机模型
  - 5.1.2. 畸变
- 5.2. 相机的标定
6. 第六讲：非线性优化
- 6.1. 状态估计问题
- 6.2. 最小二乘问题
- - 6.2.1. 引出
  - 6.2.2. 非线性最小二乘问题的一般解法 - 迭代法
  - 6.2.3. 一阶和二阶梯度法
  - 6.2.4. Gauss-Newton
  - 6.2.5. Levenberg-Marquadt
- 6.3. 用Ceres与G2O求解最小二乘问题
- - 6.3.1. Ceres
  - 6.3.2. G2O
7. 第七 - 八 - 九讲：视觉里程计
- 7.1. 特征点法
- 7.2. 2D-2D：对极几何
- 7.3. 3D-2D：PnP
- - 7.3.1. DLT：直接线性变换
  - 7.3.2. P3P
  - 7.3.3. Bundle Adjustment
- 7.4. 3D-3D：ICP
- - 7.4.1. SVD方法
  - 7.4.2. BA方法
- 7.5. 直接法
- - 7.5.1. 引出
  - 7.5.2. Lucas-Kanade 光流
  - 7.5.3. 直接法
- 7.6. 设计前端
8. 第十 - 十一讲：后端
- 8.2. BA与图优化
9. 第十二讲：回环检测
- 9.1. 基本概念
- 9.2. 词袋模型和字典
- 9.3. 相似度计算
10. 第十三讲：建图

本人小白一枚，因选了搭建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);

基本数学知识：
[xyz]T=p,pΛ=[0−zyz0−x−yx0][x\ \ y\ \ z]^T=\textbf{p},\ \ \textbf{p}^\Lambda=\begin{bmatrix}0&-z&y\\z&0&-x\\-y&x&0\end{bmatrix} [x y z]T=p, pΛ=⎣⎡0z−y−z0xy−x0⎦⎤

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

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

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

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

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

R=cos(θ)I+(1−cos(θ)nnT+sin(θ)nΛ)\textbf{R}=cos(\theta)\textbf{I}+(1-cos(\theta)\textbf{n}\textbf{n}^T+sin(\theta)\textbf{n}^\Lambda)R=cos(θ)I+(1−cos(θ)nnT+sin(θ)nΛ)

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

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）

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

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

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

设 q=q0+q1i+q2j+q3k\textbf{q}=q_0+q_1\textbf{i}+q_2\textbf{j}+q_3\textbf{k}q=q0+q1i+q2j+q3k

则有 R=[1−2q22−q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22]\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}R=⎣⎡1−2q22−q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤

或者 q0=tr(R)+12,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0q_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}q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21

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

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

我们记 T=[Rt0T1]\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1\end{bmatrix}T=[R0Tt1]，然后把 x′\textbf{x}'x′ 与 x\textbf{x}x 写成齐次坐标的形式，即 x=[x1,x2,x3,1]T,x′=[x1′,x2′,x3′,1]T\textbf{x}=[x_1, x_2,x_3,1]^T,\textbf{x}'=[x_1', x_2', x_3',1]^Tx=[x1,x2,x3,1]T,x′=[x1′,x2′,x3′,1]T，则有 x′=Tx\textbf{x}'=\textbf{T}\textbf{x}x′=Tx。

//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)={R∈R3×3∣RTR=I,det(R)=1}SO(3)=\{\textbf{R}\in\mathbb{R}^{3\times3} | \textbf{R}^T\textbf{R}=\textbf{I},det(\textbf{R})=1\}SO(3)={R∈R3×3∣RTR=I,det(R)=1}，
变换矩阵构成了特殊欧式群：SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}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\}SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}，

一个小公式： aΛ=A=[0−a3a2a30−a1−a2a10],AV=a\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}aΛ=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,AV=a

李代数：
so(3)={ϕ∈R3,Φ=ϕΛ∈R3×3}so(3)=\{\phi\in\mathbb{R}^3,\Phi=\phi^\Lambda\in\mathbb{R}^{3\times3}\}so(3)={ϕ∈R3,Φ=ϕΛ∈R3×3}，
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξΛ=[ϕΛρ0T0]∈R4×4}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}\}se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξΛ=[ϕΛ0Tρ0]∈R4×4}

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

exp(θaΛ)=cos(θ)I+(1−cos(θ)aaT+sin(θ)aΛ)exp(\theta\textbf{a}^\Lambda)=cos(\theta)\textbf{I}+(1-cos(\theta)\textbf{a}\textbf{a}^T+sin(\theta)\textbf{a}^\Lambda)exp(θaΛ)=cos(θ)I+(1−cos(θ)aaT+sin(θ)aΛ)
θ=arccostr(R)−12,Ra=a\theta=arccos\frac{tr(\textbf{R})-1}{2}, \textbf{R}\textbf{a}=\textbf{a}θ=arccos2tr(R)−1,Ra=a

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

exp(ξΛ)=[exp(ϕΛ)Jρ0T1],J=sin(θ)θI+(1−sin(θ)θ)aaT+1−cos(θ)θaΛ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}^\Lambdaexp(ξΛ)=[exp(ϕΛ)0TJρ1],J=θsin(θ)I+(1−θsin(θ))aaT+θ1−cos(θ)aΛ
θ=arccostr(R)−12,Ra=a,t=Jρ\theta=arccos\frac{tr(\textbf{R})-1}{2}, \textbf{R}\textbf{a}=\textbf{a},\textbf{t}=J\rhoθ=arccos2tr(R)−1,Ra=a,t=Jρ

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(ϕ1Λ)exp(ϕ2Λ))V≈{Jl(ϕ2)−1ϕ1+ϕ2)ifϕ1issmallJr(ϕ1)−1ϕ2+ϕ1)ifϕ2issmallln(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.ln(exp(ϕ1Λ)exp(ϕ2Λ))V≈{Jl(ϕ2)−1ϕ1+ϕ2) if ϕ1 is small Jr(ϕ1)−1ϕ2+ϕ1) if ϕ2 is small

Jl=J=sin(θ)θI+(1−sin(θ)θ)aaT+1−cos(θ)θaΛ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}^\LambdaJl=J=θsin(θ)I+(1−θsin(θ))aaT+θ1−cos(θ)aΛ，

Jl−1=θ2cot(θ2)I+(1−θ2cot(θ2))aaT−θ2aΛ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}^\LambdaJl−1=2θcot(2θ)I+(1−2θcot(2θ))aaT−2θaΛ，

Jr(ϕ)=Jl(−ϕ)J_r(\phi)=J_l(-\phi)Jr(ϕ)=Jl(−ϕ)

微小扰动公式：

exp(ΔϕΛ)exp(ϕΛ)=exp((ϕ+Jl−1(ϕ)Δϕ)Λ)exp(\Delta\phi^\Lambda)exp(\phi^\Lambda)=exp((\phi+J_l^{-1}(\phi)\Delta\phi)^\Lambda)exp(ΔϕΛ)exp(ϕΛ)=exp((ϕ+Jl−1(ϕ)Δϕ)Λ)

exp((ϕ+Δϕ)Λ)=exp((JlΔϕ)Λ)exp(ϕΛ)=exp(ϕΛ)exp((JrΔϕ)Λ)exp((\phi+\Delta\phi)^\Lambda)=exp((J_l\Delta\phi)^\Lambda)exp(\phi^\Lambda)=exp(\phi^\Lambda)exp((J_r\Delta\phi)^\Lambda)exp((ϕ+Δϕ)Λ)=exp((JlΔϕ)Λ)exp(ϕΛ)=exp(ϕΛ)exp((JrΔϕ)Λ)

exp(ΔξΛ)exp(ξΛ)=exp((ξ+Jl−1Δξ)Λ)exp(\Delta\xi^\Lambda)exp(\xi^\Lambda)=exp((\xi+\mathcal{J}_l^{-1}\Delta\xi)^\Lambda)exp(ΔξΛ)exp(ξΛ)=exp((ξ+Jl−1Δξ)Λ)

exp(ξΛ)exp(ΔξΛ)=exp((ξ+Jr−1Δξ)Λ)exp(\xi^\Lambda)exp(\Delta\xi^\Lambda)=exp((\xi+\mathcal{J}_r^{-1}\Delta\xi)^\Lambda)exp(ξΛ)exp(ΔξΛ)=exp((ξ+Jr−1Δξ)Λ)

4.3. 扰动模型（左扰动）

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

∂Rp∂φ=lim⁡φ→0exp(φΛ)exp(ϕΛ)p−exp(ϕΛ)pφ=lim⁡φ→0(1+φΛ)exp(ϕΛ)p−exp(ϕΛ)pφ=lim⁡φ→0φΛRpφ=−(Rp)Λ\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∂φ∂Rp=φ→0limφexp(φΛ)exp(ϕΛ)p−exp(ϕΛ)p=φ→0limφ(1+φΛ)exp(ϕΛ)p−exp(ϕΛ)p=φ→0limφφΛRp=−(Rp)Λ

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

∂Tp∂δξ=[I−(Rp+t)Λ0T0T]\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}∂δξ∂Tp=[I0T−(Rp+t)Λ0T]

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. 针孔相机模型

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

我们有：
{u=fxXZ+cxv=fyYZ+cy\left\{\begin{matrix} u=f_x \frac{X}{Z}+c_x\\ \\ v=f_y\frac{Y}{Z}+c_y \end{matrix}\right.⎩⎨⎧u=fxZX+cxv=fyZY+cy

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

ZPuv=KTPwZ\textbf{P}_{uv}=\textbf{K}\textbf{T}\textbf{P}_wZPuv=KTPw
或者：
Puv=KPc\textbf{P}_{uv}=\textbf{K}\textbf{P}_cPuv=KPc
在这两个式子中 K\textbf{K}K 为相机的内参矩阵（Intrinsic） 即为 K=[fx0cx0fycy001]\textbf{K}=\begin{bmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\0 & 0 & 1\end{bmatrix}K=⎣⎡fx000fy0cxcy1⎦⎤。

在G2O中内参 fxf_xfx 与 fyf_yfy 均为 fff。

5.1.2. 畸变

径向畸变：
{xcorrected=x(1+k1r2+k2r2+k3r6)ycorrected=y(1+k1r2+k2r2+k3r6)\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.⎩⎨⎧xcorrected=x(1+k1r2+k2r2+k3r6)ycorrected=y(1+k1r2+k2r2+k3r6)
则有：
{u=fxxcorrected+cxv=fyycorrected+cy\left\{\begin{matrix} u=f_x x_{corrected}+c_x\\ \\ v=f_yy_{corrected}+c_y \end{matrix}\right.⎩⎨⎧u=fxxcorrected+cxv=fyycorrected+cy
这里xxx，yyy，xcorrectedx_{corrected}xcorrected，ycorrectedy_{corrected}ycorrected 均为归一平面上的点。

切向畸变：
{xcorrected=x+2p1xy+p2(r2+2x2)ycorrected=y+p1(r2+2y2)+2p2xy\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.⎩⎨⎧xcorrected=x+2p1xy+p2(r2+2x2)ycorrected=y+p1(r2+2y2)+2p2xy

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模型，它由一个状态方程和运动方程构成：
{xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j\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.⎩⎨⎧xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j

相机位姿为 xkx_kxk，可以用 Tk\textbf{T}_kTk 或是 exp(ξkΛ)exp(\xi_k^\Lambda)exp(ξkΛ) 表示。路标位置为 yjy_jyj 。wk{w}_kwk 和 vk,j{v}_{k,j}vk,j 为噪声项，且满足高斯分布：
wk∼N(0,Rk),vk∼N(0,Qk,j)w_k\sim N(0,R_k),\ \ v_k\sim N(0,Q_{k,j})wk∼N(0,Rk), vk∼N(0,Qk,j)
把所有待估计的变量放到一个“状态变量”中：
x={x1,...,xN,y1,...,yM}x=\{x_1,...,x_N,y_1,...,y_M\}x={x1,...,xN,y1,...,yM}

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

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

后验概率最大化（Maximize a Posterior）：
xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)x^*_{MAP}=arg\ max\ P(x|z) =arg\ max\ P(z|x)P(x) xMAP∗=arg max P(x∣z)=arg max P(z∣x)P(x)

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

6.2. 最小二乘问题

6.2.1. 引出

高维高斯分布：P(x)=1(2π)Ndet(Σ)exp(−12(x−μ)TΣ−1(x−μ))P(\textbf{x})=\frac{1}{\sqrt{(2\pi)^Ndet(\Sigma)}}exp(-\frac{1}{2}(\textbf{x}-\mu)^T\Sigma^{-1}(\textbf{x}-\mu))P(x)=(2π)Ndet(Σ)1exp(−21(x−μ)TΣ−1(x−μ))

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

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

并求该误差的平方和：
J(x)=∑kev,kTRk−1ev,k+∑k∑jey,k,jTQk,j−1ey,k,jJ(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} J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk,j−1ey,k,j
于是我们得到了一个总体意义下的最小二乘问题！

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

下面以最简单的最小二乘问题为例：min⁡x12∣∣f(x)∣∣22\min_{x}\frac{1}{2}||f(x)||_2^2minx21∣∣f(x)∣∣22

给定某个初值 x0x_0x0 。
对于第 kkk 次迭代，寻找增量 Δxk\Delta x_kΔxk ，使得 ∣∣f(xk+Δxk)∣∣22||f(x_k +\Delta x_k)||_2^2∣∣f(xk+Δxk)∣∣22 最小。
若 Δxk\Delta x_kΔxk 足够小，则停止。
否则，令 xk+1=xk+Δxkx_{k+1}=x_k+\Delta x_kxk+1=xk+Δxk ，返回到 2 。

6.2.3. 一阶和二阶梯度法

将目标函数在 xxx 附近展开：
∣∣f(x+Δx)∣∣22≈∣∣f(x)∣∣22+J(x)Δx+12ΔxTHΔx||f(x+\Delta x)||^2_2\approx ||f(x)||_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^TH\Delta x ∣∣f(x+Δx)∣∣22≈∣∣f(x)∣∣22+J(x)Δx+21ΔxTHΔx

保留一阶梯度： Δx∗=−JT(x)\Delta x^*=-J^T(x)Δx∗=−JT(x)

保留二阶梯度： HΔx=−JTH\Delta x=-J^THΔx=−JT

6.2.4. Gauss-Newton

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

代入最小二乘问题的式子便得到了：
J(x)TJ(x)Δx=−J(x)Tf(x)J(x)^TJ(x)\Delta x=-J(x)^Tf(x) J(x)TJ(x)Δx=−J(x)Tf(x)

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

Gauss-Newton 的算法步骤如下：

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

6.2.5. Levenberg-Marquadt

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

Levenberg-Marquadt 的算法步骤如下：

给定某个初值 x0x_0x0 。
对于第 kkk 次迭代，求解：
min⁡Δxk12∣∣f(xk)+J(xk)Δxk∣∣2，s.t.∣∣DΔxk∣∣2⩽μ\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Δxkmin21∣∣f(xk)+J(xk)Δxk∣∣2，s.t.∣∣DΔxk∣∣2⩽μ这里 μ\muμ 是信赖区间半径，DDD 为对角矩阵（Diagonal）把增量限制在椭球中。
计算ρ\rhoρ 。
若 ρ>34\rho>\frac{3}{4}ρ>43 ，则 μ=2μ\mu=2\muμ=2μ 。
若 ρ<14\rho<\frac{1}{4}ρ<41 ，则 μ=12μ\mu=\frac{1}{2}\muμ=21μ 。
如果 ρ\rhoρ 大于某阈值，则认为近似可行。令 xk+1=xk+Δxkx_{k+1}=x_k+\Delta x_kxk+1=xk+Δxk 。
判断算法是否收敛。如不收敛则返回到 2 。

对于 2 ，我们也可以求解：
min⁡Δxk12∣∣f(xk)+J(xk)Δxk∣∣2+λ2∣∣DΔxk∣∣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 Δxkmin21∣∣f(xk)+J(xk)Δxk∣∣2+2λ∣∣DΔxk∣∣2也相当于是计算增量的线性方程：
(H+λDTD)Δx=g(H+\lambda D^TD)\Delta x=g(H+λDTD)Δx=g

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

6.3.1. Ceres

详见Ceres官网。

栗子：y=exp(ax2+bx+c)y=exp(ax^2+bx+c)y=exp(ax2+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 residualtemplate <typename T>bool operator() (const T* const abc, // Model parameter, 3 dimensionsT* 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 functionabc  // 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 coutceres::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_NEWvirtual 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_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// compute errorvoid 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 vertexedge->setMeasurement( y_data[i] );      // set mesurementedge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrixoptimizer.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：对极几何

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

这里式子中的 xix_ixi 均是归一化平面上的坐标，重新代入 p1,p2p_1,p_2p1,p2 有：
p2TK−TtΛRK−1p1=0\textbf{p}_2^T\textbf{K}^{-T}\textbf{t}^\Lambda \textbf{R}\textbf{K}^{-1}\textbf{p}_1=0 p2TK−TtΛRK−1p1=0

我们把中间部分记作两个矩阵：基础矩阵 F\textbf{F}F 和本质矩阵 E\textbf{E}E，进一步简化对极约束：
E=tΛR,F=K−TEK−1,x2TEx1=p2TFp1=0\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 E=tΛR, F=K−TEK−1, x2TEx1=p2TFp1=0

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

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

三角测量： 对于两个归一化平面上的点 x1\textbf{x}_1x1 和 x2\textbf{x}_2x2 ，那么它们满足：
s1x1=s2Rx2+ts_1\textbf{x}_1=s_2\textbf{R}\textbf{x}_2+\textbf{t}s1x1=s2Rx2+t稍微转化则有：
s1x1Λx1=0=s2x1ΛRx2+x1Λts_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}s1x1Λx1=0=s2x1ΛRx2+x1Λt

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

7.3. 3D-2D：PnP

7.3.1. DLT：直接线性变换

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

7.3.2. P3P

根据相似三角形，我们很容易可以求出：
(1−u)y2−ux2−cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0(1−w)x2−wy2−cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0(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 (1−u)y2−ux2−cos⟨b,c⟩y+2uxy cos⟨a,b⟩+1=0(1−w)x2−wy2−cos⟨a,c⟩x+2wxy cos⟨a,b⟩+1=0

7.3.3. Bundle Adjustment

构建最小二乘问题，使重投影误差最小：
ξ∗=argmin⁡ξ12∑i=1n∣∣ui−1siKexp(ξΛ)Pi∣∣22\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 ξ∗=arg ξmin 21i=1∑n∣∣ui−si1Kexp(ξΛ)Pi∣∣22

由 e(x+Δx)≈e(x)+JΔxe(x+\Delta x) \approx e(x) +J\Delta xe(x+Δx)≈e(x)+JΔx 可知，我们需要求 JJJ ，在这里 JJJ 应该是一个 2×62\times62×6 的矩阵。令 P′=(exp(ξΛ)P)1:3=[X′,Y′,Z′]T\textbf{P}'=(exp(\xi^\Lambda)\textbf{P})_{1:3}=[X',Y',Z']^TP′=(exp(ξΛ)P)1:3=[X′,Y′,Z′]T 。
∂e∂δξ=−[fxZ′0−fxX′Z′2−fxX′Y′Z′2fx+fxX2Z′2−fxYZ′0fyZ′−fyY′Z′2−fy−fyY′2Z′2fyX′Y′Z′2fyX′Z′]\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} ∂δξ∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX2Z′2fyX′Y′−Z′fxYZ′fyX′]
∂e∂P=−[fxZ′0−fxX′Z′20fyZ′−fyY′Z′2]R\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} ∂P∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′]R

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

7.4. 3D-3D：ICP

构建的最小二乘问题为 min⁡R,tJ=12∑i=1n∣∣(pi−(Rpi′+t))∣∣22\min_{\textbf{R},\textbf{t}} J=\frac{1}{2}\sum_{i=1}^{n}||(\textbf{p}_i-(\textbf{R}\textbf{p}'_i+\textbf{t}))||_2^2minR,tJ=21∑i=1n∣∣(pi−(Rpi′+t))∣∣22 。

7.4.1. SVD方法

计算两组点的质心位置 p,p′\textbf{p},\textbf{p}'p,p′ ，然后计算每个点的去质心坐标：
qi=pi−p,qi′=pi′−p′\textbf{q}_i=\textbf{p}_i-\textbf{p},\ \ \textbf{q}_i'=\textbf{p}'_i-\textbf{p}'qi=pi−p, qi′=pi′−p′
根据 R∗=argmin⁡R12∑i=1n∣∣qi−Rqi′∣∣2=argmin⁡R12∑i=1nqiTqi+qi′Tqi′−2qiTRqi′=argmin⁡R−tr(R∑i=1nqi′qiT)\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)R∗=arg Rmin21i=1∑n∣∣qi−Rqi′∣∣2=arg Rmin21i=1∑nqiTqi+qi′Tqi′−2qiTRqi′=arg Rmin−tr(Ri=1∑nqi′qiT) 计算旋转矩阵。
根据旋转矩阵计算 t\textbf{t}t 。

7.4.2. BA方法

求导式子为：
∂e∂δξ=−[I−(exp(ξΛ)pi′)Λ00]\frac{\partial\textbf{e}}{\partial\delta\xi}=-\begin{bmatrix}\textbf{I} & -(exp(\xi^\Lambda)\textbf{p}'_i)^\Lambda\\\textbf{0}&\textbf{0}\end{bmatrix} ∂δξ∂e=−[I0−(exp(ξΛ)pi′)Λ0]

7.5. 直接法

7.5.1. 引出

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

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

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

7.5.2. Lucas-Kanade 光流

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

设
Ix=∂I∂x,Iy=∂I∂y,u=dxdt,v=dydt,It=∂I∂t\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}Ix=∂x∂I, Iy=∂y∂I, u=dtdx, v=dtdy, It=∂t∂I

根据灰度不变假设，我们有:
Ixu+Iyv=−Itor[IxIy][uv]=−It\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 Ixu+Iyv=−It or [IxIy][uv]=−It

令：
A=[[IxIy]1...[IxIy]k],b=[It1...It2]\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} A=⎣⎡[IxIy]1...[IxIy]k⎦⎤,b=⎣⎡It1...It2⎦⎤

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

7.5.3. 直接法

光度误差 记为：
e=I1(p1)−I2(p2)e=\textbf{I}_1(\textbf{p}_1)-\textbf{I}_2(\textbf{p}_2) e=I1(p1)−I2(p2)

对于多个空间点 PiP_iPi ，相机位姿估计问题变为：
min⁡ξJ(ξ)=∑i=1NeiTei,ei=I1(p1,i)−I2(p2,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}) ξminJ(ξ)=i=1∑NeiTei, ei=I1(p1,i)−I2(p2,i)

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

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=TPTP+FPPrecision=\frac{TP}{TP+FP}Precision=TP+FPTP

召回率： Recall=TPTP+FNRecall=\frac{TP}{TP+FN}Recall=TP+FNTP

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

9.2. 词袋模型和字典

K-means 的步骤：

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

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

在根节点,用 K-means 把所有样本聚成 kkk 类(实际中为保证聚类均匀性会使用k-means++)。这样得到了第一层。
对第一层的每个节点,把属于该节点的样本再聚成 kkk 类,得到下一层。
依此类推,最后得到叶子层。叶子层即为所谓的 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 featurescout<<"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）：

我们统计某个叶子节点 wiw_iwi 中的特征数量相对于所有特征数量的比例,作为 IDF 部分。假设所有特征数量为 nnn，wiw_iwi 数量为 nin_ini ，那么该单词的 IDF 为：
IDFi=log⁡(nni)IDF_i=\log(\frac{n}{n_i})IDFi=log(nin)

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

于是 wiw_iwi 的权重等于 TF 乘 IDF 之积：
ηi=TFi×IDFi\eta_i=TF_i\times IDF_iηi=TFi×IDFi

于是对于图像 A 我们便可以这样描述：
A={(w1,η1),...,(wN,ηN)}=vAA=\{(w_1,\eta_1),...,(w_N,\eta_N)\}=\textbf{v}_AA={(w1,η1),...,(wN,ηN)}=vA

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

*这部分代码就不详述了

关键帧的处理：

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

10. 第十三讲：建图

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

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

SLAM 入门之《SLAM 14讲》笔记相关推荐

马哥linux运维1～14讲笔记＋自我知识储备补充
1-14主要是linux基础命令(略).根文件系统.文件管理命令.用户及权限.用户管理命令 1.bash特性讲解定义:在计算机科学中,Shell俗称壳(用来区别于核),是指"为使用者提供操 ...
视觉SLAM总结——视觉SLAM十四讲笔记整理
视觉SLAM总结--视觉SLAM十四讲笔记整理说明基础知识点 1. 特征提取.特征匹配 (1)Harris (2)SIFT (3)SUFT (4)ORB (5)特征匹配 2. 2D-2D:对极约束 ...
视觉SLAM十四讲笔记-1
视觉SLAM十四讲笔记-1 文章目录视觉SLAM十四讲笔记-1 第一讲:预备知识 1.1 本书讲什么 1.2 如何使用本书参考链接: link link 高翔,张涛,等. 视觉 SLAM 十四讲: ...
激光SLAM入门学习笔记
激光SLAM入门学习笔记激光SLAM入门学习笔记一.推荐阅读书籍二.推荐公众号.知乎.博客 1.公众号 2.知乎 3.博客三.推荐阅读论文&代码(参考泡泡机器人) 2D激光SLAM 3 ...
SLAM十四讲笔记1
文章目录 ch02 初识SLAM ch02-01 经典视觉SLAM框架 ch02-02 SLAM问题的数学表述 ch03 三维空间刚体运动 ch03.01 旋转矩阵:点和向量,坐标系 01 向量a在线 ...
视觉SLAM十四讲笔记-7-2
视觉SLAM十四讲笔记-7-2 文章目录视觉SLAM十四讲笔记-7-2 估计相机运动 7.3 2D-2D:对极几何 7.3.1 对极约束 7.3.2 本质矩阵 7.3.3 单应矩阵 7.4 实践:对 ...
SLAM 14讲中cere拟合曲线代码报错：undefined reference to symbol ‘omp_get_num_threads@@OMP_1.0‘
视觉SLAM 14讲中cere拟合曲线代码报错: /usr/bin/x86_64-linux-gnu-ld: /usr/local/lib/libceres.a(coordinate_descent_ ...
从零入门激光SLAM（一）——什么是SLAM
大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码.随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家 ...
如何选择ROS机器人平台进行SLAM导航入门：SLAM与ROS的关系
1.SLAM与ROS的关系 1.1.关于SLAM 在了解SLAM之前,需要先对机器人有一个整体的认识.机器人是一个复杂的装置,涉及到执行机构.感知.决策等主要环节.机器人上的配备的常用执行机构有轮式运 ...

SLAM 入门之《SLAM 14讲》笔记