【视觉SLAM入门】一些关于视觉SLAM十四讲的重要基础概念

高翔. 视觉SLAM十四讲 从理论到实践. 电子工业出版社, 2017. Print.

第二章：初识SLAM

1. 视觉SLAM模块：

   

   • 传感器信息读取：相机、惯性传感器等信息的读取和同步。
   • 前端视觉里程计 Visual Odometry, VO：估算相邻图像之间相机的运动，以及局部地图的样子。计算机视觉，图像的特征提取和匹配。
   • 后端（非线性）优化 Optimization：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。滤波和非线性优化算法。
   • 回环检测 Loop Closure Detection：判断机器人是否到达过先前的位置，判断图像间的相似性。
   • 建图 Mapping

2. 最大后验概率估计 Maximum-a-Posterior，MAP：从带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大。

3. SLAM问题数学描述：

   • 运动方程 $x_k=f(x_{k-1},u_k,w_k)$，参数分别为位置、运动和过程噪声。
   • 观测方程 $z_{k,j}=h(y_i,x_k,v_{k,j})$，$z_{k,j}$为观测值，$v_{k,j}$为观测噪声 z
   • 状态估计问题：当知道运动测量的读数u以及传感器的读数z时，如何求解定位问题（估计x）和建图（估计y）？如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量

第三章：三维空间刚体运动

1. 坐标：空间中的每一个向量等于该空间的一组基内积一组坐标

2. 反对称矩阵 Skew-symmetric Matrix：

   $a\times b=a$^$b$

3. 坐标系的欧式变换 Euclidean Transform：对于同一个向量p，它在世界坐标系下的坐标$p_w$和在相机坐标系下的坐标$p_c$是不同的，变换关系由变换矩阵$T$描述。

   旋转矩阵 Rotation Matrix / 方向余弦矩阵 Direction Cosine Matrix：两组基之间的内积组成。是一个行列式为1的正交矩阵。描述了一个旋转动作，其逆矩阵或转置矩阵描述了一个相反的旋转。

4. 特殊正交群 Special Orthogonal Group, SO：这个集合由n维空间的旋转矩阵组成。

   $SO(n)=\{R\in R^{n\times n}|R{R}^T=I,det(R)=1\}$

5. 坐标变换：$a^{\prime }=Ra+t$

   变换矩阵 Transform Matrix：引入齐次坐标，把旋转和平移写在一个矩阵中，使整个关系变成线性关系。正交矩阵且行列式为1。$(a,1{)}^T$称齐次坐标。下标规定，$T_{to|from}$
   

   该矩阵的逆表示一个反向变换

   $T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$

6. 特殊欧式群 Special Euclidean Group, SE：变换矩阵

   $SE(3)=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$

7. 旋转向量 Axis-Angle：任意旋转都可以用一个旋转轴和一个旋转角来刻画。旋转向量方向与旋转轴一致，长度等于旋转角。因此只需一个三维向量即可描述旋转。对于变换矩阵，我们是用一个旋转向量和一个平移向量即可表达一次变换。

8. 从一个旋转矩阵R到旋转向量n的转换：设旋转轴为一个单位长度的向量n，角度为$\theta$，则$\theta n$既可以描述这个旋转

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

   $\theta =arccos\frac{tr(R)-1}{2}$

   事实上，旋转轴上的向量在旋转后不发生改变$Rn=n$，即转轴n是矩阵R特征值1对应的特征向量。

9. 欧拉角：用三个分离的转角rpy——三次绕不同轴的旋转来描述旋转。但是会产生奇异性问题导致自由度缺失。

10. 四元数 Quarternion：扩展复数，既是紧凑的也没有奇异性。拥有一个实部和三个虚部。单位四元数对应一个以向量v为轴旋转$\theta$角度的旋转操作，任意单位四元数都可表示为一个3D旋转

    $q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v{]}^T,s=q_0\in R,v=[q_1,q_2,q_3{]}^T\in R^3$，设$q_a=[s_a,v_a{]}^T,q_b=[s_b,v_b{]}^T$

    运算

    • 加法：$q_a+q_b=[s_a+s_b,v_a+v_b{]}^T$
    • 乘法：$q_a{q}_b=s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b+(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i+(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j+(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b)k$
    • 模长：$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$， $||q_a{q}_b||=||q_a||||q_b||$
    • 共轭：$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$
    • 求逆：$q^{-1}=q^{\ast }/||q|{|}^2$
    • 数乘：$kq=[ks,kv{]}^T$

    用四元数表示旋转：设有一空间三维点$p=[x,y,z]\in R^3$，以及一个单位四元数q指定的旋转。

    • 用矩阵描述：$p^{\prime }=Rp$
    • 用四元数描述：虚四元数$p=[0,x,y,z{]}^T=[0,v{]}^T$ 则旋转后的点$p^{\prime }=qp{q}^{-1}$，$p^{\prime }$的虚部即旋转之后点的坐标。

11. 四元数与欧拉角之间的转换

    • Quaternion2Euler 四元数转欧拉角：

      $\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}cos(R/2)cos(P/2)cos(Y/2)+sin(R/2)sin(P/2)sin(Y/2)\\ cos(R/2)sin(P/2)cos(Y/2)+sin(R/2)cos(P/2)sin(Y/2)\\ cos(R/2)cos(P/2)sin(Y/2)-sin(R/2)sin(P/2)cos(Y/2)\\ sin(R/2)cos(P/2)cos(Y/2)-cos(R/2)sin(P/2)sin(Y/2)\end{array}\right]$

    

第四章：李群与李代数

1. 总结：除了对旋转的表示，我们还要对旋转进行估计和优化。因为SLAM中的位姿是未知的，我们需要解决什么样的相机位姿最符合当前的观测数据。一种典型的方式就是把它构建成一个优化问题，求解最优的R和t，使得误差最小化。

   旋转矩阵自身带有约束。旋转矩阵作为优化变量时，会引入额外的约束（正交且行列式为1），使优化变的困难。通过李群—李代数间的转换关系e2，我们希望把位姿估计变成无约束优化问题，简化求解方式。

2. 群 Group：一种集合加上一种运算的代数结构。集合记作A，运算记作·，群记作$G=(A,\cdot )$

   李群：具有连续（光滑）性质的群。$SO(n),SE(n)$

   李代数：描述了李群的局部性质，是单位元附近的正切空间,每个李群都有与之对应的李代数。$so(3),se(3)$

   李代数$so(3)$：是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。它和$SO(3)$之间的关系由指数映射给定$R=exp(\varphi$^)。

李代数$se(3)$：是一个六维向量组成的集合。前三维为平移记为$\rho$，后三维为旋转记作$\varphi$。

指数映射 Exponoential Map：矩阵的指数

第五章：相机与图像

1. 相机的内参数 Camera Intrinsics：相机内参数是与相机自身特性相关的参数,比如相机的焦距、像素大小等。单目棋盘格张正友标记法。

   相机外参数 Camera Extrinsics：是在世界坐标系中的参数, 比如相机的位姿R,t。SLAM中待估计的目标，代表着机器人的轨迹。

   深度信息在投影过程中被丢失了。

畸变 Distortion：摄像机的透镜使真实环境中的一条直线在图片中变成了曲线，越靠近图像的边缘，这种现象越明显。

径向畸变：坐标点沿着长度方向发生了变化，即距离远点的长度发生了变化。

切向畸变：坐标点沿着切线方向发生了变化，即水平夹角发生了变化。

畸变的多项式关系描述：
$$\begin{gathered} x_{distorted}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{distorted}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+p_1(r^2+2y^2)+2p_{2}xy \end{gathered}$$
去畸变 Undistort：

• 将三维空间点投影到归一化图像平面
• 对归一化平面上的点计算径向畸变和切向畸变
• 将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

2. 单目相机成像过程：

   • 世界坐标系下的世界坐标$P_w$
   • 由于相机在运动，他的运动由R,t或变换矩阵$T\in SE(3)$描述。P的相机坐标为$\widetilde{P_c}=R{P}_w+t$
   • 这时的$\widetilde{P_c}$的分量为X,Y,Z，投影到归一化平面Z=1上，得到P的归一化坐标。$P_c=[X/Z,Y/Z,1]$
   • 有畸变时，根据畸变参数计算$P_c$发生畸变后的坐标
   • P的归一化坐标经过内参后，对应到它的像素坐标$P_{uv}=K{P}_c$

3. 双目相机的原理：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。

视差：左右图的横坐标之差。$d=u_L-u_R$ 注意$u_R$为负数。视差越大，物体距离越近。

4. RGB-D相机模型：向探测目标发射光线（红外光）

   • 红外结构光 Structured Light：根据返回的结构光图案，计算物体与自身的距离。Kinect 1 代，Project Tango 1 代，Intel RealSense。
   • 飞行时间 Tome-of-Flight，TOF：相机向目标发生脉冲光，然后根据发送到返回之间的光束飞行时间确定物体与自身的距离。Kinect 2 代。

5. 图像：相机吧三维世界中的信息转换成了一张由像素组成的照片，储存在计算机中，以二维数组的形式表示。

   灰度图 Gray Scale Image：灰度图中，每个像素位置（x，y）对应一个灰度值$I$。用一个0~255的整数（即一个unsigned char，1个字节，8位整数）来表达图像的灰度值。x行y列灰度图对应数组image [y] [x]。

   

第六章：非线性优化

1. 经典SLAM模型：假设两个噪声项满足零均值的高斯分布，方差为协方差矩阵$w_k$ ~ $N(0,R_k)$，$v_k$ ~ $N(0,Q_{k,j})$
   $$\{\begin{array}{l}\begin{array}{rl}{x}_k& =f(x_{k-1},u_k)+w_k\\ z_{k,j}& =h(y_i,x_k)+v_{k,j}\end{array}\end{array}$$
   目标：我们希望通过带噪声的数据z和u推断位姿x和地图y以及他们的概率分布，这构成了一个状态估计问题。对机器人状态的估计，就是已知输入数据u和观测数据z的条件下，求状态x,y的条件概率分布：$P(x,y|z,u)$

   状态估计问题处理方法：

   • 增量式处理 incremental/滤波器：数据随时间逐渐到来，持有一个当前时刻的估计状态，然后用新的数据来更新它。
   • 批量处理 batch：数据攒起来一并处理。从图像中重建三维空间结构 SfM Structure from Motion方法。

   状态估计过程：直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化。即求解最大似然估计MLE $(x,y{)}_{MLE}^{\ast }=argmaxP(z,u|x,y)$，即理解为：在什么样的状态下，最可能产生现在观测到的数据。

   处理办法：最小二乘法 Least Square Problem。

   • 输入误差：$e_{u,k}=x_k-f(x_{k-1},u_k)$

   • 观测误差：$e_{z,j,k}=z(k,j)-h(x_k.y_j)$

   • 马哈拉诺比斯距离 Mahalanobis distance/马氏距离：最小化噪声项的二次型。

   • 最小化所有时刻估计值与真实读数之间的马氏距离即等价于求最大似然估计：
     $$minJ(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$

   在SLAM中的最小二乘问题具有特定的结构

   • 整个问题的目标函数由许多个误差的二次型组成，每个误差项都是简单的，仅与一两个状态变量有关。让整个问题有一种稀疏的形式。
   • 利用二次型度量误差，因此误差的分布将影响此项在整个问题中的权重。某次观测非常准确，那么协方差矩阵就会小，信息矩阵就会大。

2. 非线性最小二乘：$\min F(x)=\frac{1}{2}||f(x)|{|}_2^2$ 如果用求导来求解则需要我们知道关于目标函数的全局性质。对于不方便求解的最小二乘问题，可用迭代的方式，让求解导函数为零的问题变成了一个不断寻找下降增量$\Delta x_k$的问题。

梯度法：取增量为反向的梯度即可保证目标函数下降。

• 雅可比矩阵 Jacobian Matrix：F(x)关于x的一阶导数 $J_{i,j}=\frac{\partial f_i}{\partial x_j}$

• 黑森矩阵/海塞矩阵 Hessian Matrix：F(x)关于x的二阶导数 $H(f)(x{)}_{i,j}=\frac{{\partial }^{2}f}{\partial x_i\partial x_j}$

牛顿法：保留二阶梯度信息 $\Delta x^{\ast }=argmin(F(x)+J(x{)}^T\Delta x+\frac{1}{2}{\Delta }^{T}H\Delta x)$，求右侧等式关于$\Delta x$的导数并令其为零有

$J+H\Delta x=0\Rightarrow H\Delta x=-J$ 即得牛顿法的增量$\Delta =-\frac{J}{H}$

高斯牛顿法：将f(x)而不是目标函数F(x)进行一阶泰勒展开$f(x+\Delta x)\approx f(x)+J(x{)}^T\Delta x$，求使$||f(x+\Delta x)|{|}^2$达到最小的增量，即求解线性最小二乘问题$\Delta x^{\ast }=argmin\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2$，求目标函数的导数并令其为零即得到高斯牛顿方程 $J(x)J^T(x)\Delta x=-J(x)f(x)\Rightarrow H\Delta x=g$。其中用$J{J}^T$作为牛顿法中二阶Hessian矩阵的近似，省略了计算二阶导数H的过程。

信赖区域方法 Trust Region Method：定义了在什么范围二阶近似泰勒展开是有效的。

• 根据模型和实际函数的差异来确定：$\rho =\frac{f(x+\Delta x)-f(x)}{J(x{)}^T\Delta x}$，$\rho $趋近于1则近似是好的。

  

第七章：视觉里程计——特征点法

1. 视觉里程计 Visual Odometry：根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初值。

2. 特征点匹配

   • 特征 Feature：特征是图像信息 的另一种数字表达形式。

   • 特征点：图像里的特殊的地方，如反复出现的角点、边缘和区块。由关键点 Key-point 和描述子 Descriptor组成

   

   • 描述子：外观相似的特征应该有相似的描述子。只要两个特征点的描述子在向量空间上的距离相近就可以认为是同样的特征点。

   SIFT，Scale-Invariant Feature Transform 尺度不变特征变换：提取SIFT关键点，并计算SIFT描述子。

   ORB Oriented FAST and Rotated BRIEF：特征提取主流方案。

   • FAST，Features from Accelerated Segment Test 角点提取：FAST是一种角点，主要检测局部像素灰度变化明显的地方。如果一个像素与邻域的像素差别较大（过亮或过暗），那么它更可能是角点。
   • BRIEF描述子：二进制描述，描述向量由许多01组成，编码了关键点附近两个随机像素的大小关系，p比q大取1，反之取0。

   特征匹配：解决数据关联问题 data association，即确定当前看到的路标与之前看到的路标之间的对应关系。

   • 暴力匹配 Brute-Force Matcher：计算每一个特征点与其他每一个点的测量描述子距离，排序取最近点作为特征匹配点。描述子距离表示了两个特征之间的相似程度。**浮点类型描述子用欧式距离，二进制描述子用汉明距离 Hamming distance即不同位数的个数。**运算量极大。
   • 快速近邻最近邻 Fast Library for Approximate Nearest Neighbors，FLANN：适合匹配点数量极多的情况。

3. 计算相机运动：根据已经匹配好的点对估计相机的运动。

   • 单目相机：根据两组2D点估计运动，应用对极几何
   • 双目、RGB-D相机：根据两组3D点估计运动，应用ICP
   • 一组2D，一组3D：根据一些3D点和他们在相机的投影位置。应用PnP

4. 2D-2D 对极几何：设两像素点位置$p_1,p_2$，K为相机内参数矩阵，R,t为两坐标系的相机运动。则两匹配点的空间位置关系约束为

   $p_2^T{K}^{-T}t$^$R{K}^{-1}{p}_1=0$，设本质矩阵 Essential Matrix$E=t$^$R$，基础矩阵 Fundamental Matrix$F=K^{-T}E{K}^{-1}$即得$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$

相机位姿估计问题：

• 根据匹配点像素位置求出E或F，八点法。
• 根据E或F求出R,t：SVD奇异值分解

本质矩阵 Essential Matrix：$E=t$^$R$

• E乘任意非零常数后，对极约束依然满足，即E在不同尺度下是等价的。
• E的奇异值必定是$[\sigma ,\sigma ,0{]}^T$的形式。
• E有五个自由度。

八点法 Eight-point-algorithm：八对匹配点组成的矩阵满足只为8，就可以得到E。

单应矩阵 Homography：描述两个平面之间的映射关系，通常描述处于共同平面上的一些点在两张图像之间的变换关系。

5. 3D-3D PnP Perspective -n-point：求解3D到2D点对运动的方法。

   • 直接线性变换 Direct Linear Transform：已知一组3D点的位置，以及它们在某个相机中的投影位置，求该相机位姿R,t。

   • P3P：三对匹配点，利用相似原理求解。

     

6. Bundle Adjustment，BA：把相机和三维点放在一起进行最小化。

   重投影误差：3D点投影位置和观测位置作差。

7. 3D-3D 迭代最近点 Iterative Closest Point，ICP：用一组配对好的3D点求解位姿变换R,t。

   • SVD方法：
     1. 计算两组点的质心位置p,p’，计算没给点的去质心坐标：$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$
     2. 最优化$R^{\ast }=\mathrm{argmin}\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2$以求解旋转矩阵R
     3. 求平移t $t^{\ast }=p-R{p}^{\prime }$

第八章：视觉里程计——直接法

1. 直接法：根据像素的亮度信息估计相机的运动，不用计算关键点和描述子，既避免了特征的计算时间，也避免了特征缺失的情况。

2. 光流 Optical Flow：描述像素随时间在图像之间运动的方法。同一个像素会在图像中运动，我们希望追踪他的运动过程。

   • 稀疏光流：计算部分像素运动
   • 稠密光流：计算所有像素

3. Lucas-Kanade光流：认为来自相机的图像是随时间变化的。图像可看做时间的函数$I(x,y,t)$，通过灰度不变性求解像素运动速度u,v。

   • 假设：
     1. 灰度不变假设$I(x+dx,y+dy,t+dt)=I(x,y,t)$
     2. 某一个窗口内的像素具有相同的运动

第九、十章：后端

1. 状态估计的概率解释：希望整个运动轨迹在较长时间内都能保持最优。站在"久远的状态"的角度上，用未来的信息告诉它“你应该在哪里”。考虑一段时间内的状态估计问题，不仅使用过去的信息更新自己的状态，也会用未来的信息来更新。

   后端优化问题：当我们拥有某些运动数据u和观测数据z时，如何确定状态量x,y的分布？如果得到了新时刻的数据，它们的分布将发生怎样的变化？

   问题转换：假设状态量和噪声项服从高斯分布——在程序中只要存储他们的均值和协方差矩阵即可。均值看做对变量最优值的估计；协方差矩阵度量了它的不确定性。则后端优化问题——>当存在一些运动数据和观测数据时，如何估计状态量的高斯分布。

2. 卡尔曼滤波KF与扩展卡尔曼滤波EKF

   见《无人驾驶原理与实践》第四章：状态估计与传感器融合

3. Bundle Adjustment 与图优化

   • BA：指从视觉图像中提炼出最优的3D模型和相机参数（内参数和外参数）。考虑从任意特征点发射出来的几束光线**（bundles of light rays），他们在几个相机的成像平面上变成像素或是检测到的特征点，我们调整（Adjustment）**各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心。但是大量存在的特征点会严重降低计算效率，导致计算量巨大。

   投影模型：即观测方程的具体过程$z=h(x,y)$

   1. 把世界坐标转换到相机坐标，用到相机外参数（R,t）

      $P^{\prime }=Rp+t=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$

   2. 将$P^{\prime }$投影至归一化平面，得到归一化坐标

      $P_c=[u_c,v_c,1{]}^T=[X^{\prime }/Z^{\prime },Y^{\prime }/Z^{\prime },1{]}^T$

   3. 考虑归一化坐标的畸变，得到去畸变前的原始像素坐标。只考虑径向畸变。
      $$\{\begin{array}{l}\begin{array}{rl}{u}_c^{\prime }& =u_c(1+k_1{r}_C^2+k_2{r}_c^4\\ v_c^{\prime }& =v_c(1+k_1{r}_c^2+k_2{r}_c^4)\end{array}\end{array}$$

   4. 根据内参模型，计算像素坐标
      $$\{\begin{array}{l}\begin{array}{rl}{u}_s& =f_x{u}_c^{\prime }+c_x\\ v_s& =f_y{v}_c^{\prime }+c_y\end{array}\end{array}$$
      

   • Adjustment：

     • 观测误差：$e=z-h(T,p)$，$z=[u_s,v_s{]}^T$为像素坐标，T为外参数R,t的李群，p为路标y

     • 要进行优化的代价函数：设$z_{ij}为在位姿T_i处观察路标p_j产生的数据$，整体的代价函数为
       $$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||z_{ij}-h(T_i,p_j)|{|}^2$$

     • 方法：高斯牛顿法或信赖区域方法 $H\Delta x=g$，其中高斯牛顿法的$H=J^{T}J$

4. 稀疏性与边缘化

   $\star$ 发现了H矩阵的系数结构，因为误差项对应的雅可比矩阵只涉及部分相机位姿和路标点，因此有大量的0。

• 边缘化 Marginalization：对于具有稀疏结构的H，线性方程$H\Delta x=g$可利用H的稀疏性进行加速。Schur消元 Schur Elimination
• 核函数：保证每条边的误差不会大的没边而掩盖其他边。
• 鲁棒核函数 Robust Kernel：把原先误差的二范数度量替换成一个增长没那么快的函数，同时保持自己的光滑性质。Huber核、Cauchy核、Tukey核。

5. 控制BA的方法：

   • 滑动窗口法：仅保留离当前时刻最近的N个关键帧，去掉时间上更早的关键帧。即BA将固定在一个时间窗口内，离开这个窗口的则被丢弃。
   • 位姿图：只保留关键帧的轨迹。

第十一章：回环检测

长期误差的积累将导致我们无法构建全局一致的轨迹和地图。虽然后端能够估计最大后验误差，但是好模型架不住烂数据，只有相邻关键帧数据时，无法消除累积误差。

1. 回环检测：给出除了相邻帧的一些时间间隔更久远的约束，因为相机经过同一个地方可以采集到相似的数据。关键在如何有效地检测出相机经过同一个地方。相当于加入弹簧阻尼，提高系统稳定性。

   先估计哪里可能出现回环：

   • 基于里程计(Dodometry based)的几何关系：发现当前相机运动到了之前某个位置
   • 基于外观(Appearance basrd)的几何关系：仅根据两幅图像的相似性确定回环检测关系

   核心问题：如何计算图像间的相似性。

   • 相似性评分：$s(A,B)$，当评分大于一定量之后认为出现一个回环。

   感知偏差 Perceptual Aliasing：假阳性 False Psitive，FP

   感知变异 Perceptual Variability：假阴性 False Negative，FN

   

   准确率 Precision：算法提取的所有回环中确实是真实回环的概率。$Precision=\frac{TP}{TP+FP}$

   召回率 Recall：在所有真实回环中被正确检测出来的概率。$Recall=\frac{TP}{TP+FN}$

2. 词袋模型 Bags-of-Words，Bow：目的是用“图像上有哪几种特征”来描述一幅图像。如两个人一只狗。

   （1）定义单词 Word，组成字典 Dictionary：确定“人、狗”的概念

   （2）用单词出现的情况描述整幅图像，把一幅图像转换成一个向量描述。描述向量说的是是否出现，而不管它们在哪里出现，因此在相机发生少量运动时，只要物体仍在视野中出现，就仍然可以保持描述向量不变。

   （3）比较相似性

   例：记$w_1-人,w_2-车,w_3-狗$，则对某一张图片A根据所含有的单词可记为$A=2w_1+1w_2+0w_3$，即用向量$[2,1,0{]}^T$描述。

3. 字典 Dictionary：类似于聚类问题 Clustering，无监督机器学习，让机器自行寻找数据中的规律。

   Bow的字典生成问题：假设已对大量图像提取了N个特征点，想找一个有k个单词的字典，每个单词可看做局部相邻特征点的集合。

   • K均值 K-means方法：

     

   • k叉树：有N个特征点，希望构建一个深度为d，每次分叉为k的树。

   

4. 频率-逆文档频率 Term Frequency-Inverse Document Frequency, TF-IDF：希望对单词的区分性或重要性进行评估，给他们以不同的权值以起到更好的效果。假设多有特征数量n, $w_i$数量为$n_i$

   • TF：某单词在一幅图像中经常出现，他的区分度就高。即某个特征在单幅图像中出现的频率，$T{F}_i=\frac{n_i}{n}$
   • IDF：某单词在字典中出现的频率越低，分类图像时的区分度越高，$ID{F}_i=log\frac{n}{n_i}$
   • 权重${\eta }_i=T{F}_i\times ID{F}_i$
   • 描述向量$v_A=\{(w_1,{\eta }_1),(w_2,{\eta }_2),..,(w_N,{\eta }_N)\}$

5. 相似性评分处理：取一个先验相似度$s(v_t,v_{t-\Delta t})$，表示某时刻关键帧图像与上一时刻的关键帧的相似性，然后对其他评分进行归一化。

   $s(v_t,v_{t_j}{)}^{\prime }=s(v_t,v_{t_j})/s(v_t,v_{t-\Delta t})$

   如果当前帧与之前某关键帧的相似度超过当前帧与上一个关键帧相似度的3倍，则认为有可能存在回环。

   在一段时间内持续检测到的回环，才是正确的回环。

第十二章：建图

1. 地图：所有路标点的集合。确定了路标点的位置，即完成了建图。

   地图的作用：

   • 定位
   • 导航：需要知道地图中那些地方不可通过，哪些地方可以通过。
   • 避障
   • 重建
   • 交互：语义地图。增强现实中的，在房间里放置虚拟物体。

2. 稀疏地图：只建模感兴趣的部分即特征点（路标点）

   稠密地图：建模所有看到的部分。

3. 深度获取

   • 立体视觉 Stereo Vision
     • 移动视角的立体视觉 Moving View Stereo, MVS：使用单目相机，估计相机运动，并且三角化计算像素的距离。
     • 使用双目相机，利用左右目的视差计算像素的距离。
   • 使用RGB-D相机直接获取像素距离。

4. 单目稠密地图重建:确定第一幅图的某像素出现在其他图里的位置。

   • 极线搜索：在特征点方法中，通过特征匹配找到了2号像素的位置，在极线上搜索和1号像素长得比较相似的点。即沿着第二幅图像的极线从一头走向另一头，逐个比较每个像素与1号像素的相似程度。

     

   • 块匹配：在1号像素周围取小块，然后在极线上也取很多同样大小的小块进行比较，以确定两幅图像的相似性。即从像素的灰度不变性变成了图像块的灰度不变性。

   • 块间差异的计算：

     • 差的绝对值之和 Sum of Absolute Difference, SAD：取两小块的差的绝对值之和，越小越相似。

       $S(A,B{)}_{SAD}=\sum _{i,j}|A(i,j)-B(i,j)|$

     • 平方和 Sum of Squared Distance，SSD：块间差的平方和，越小越相似。

       $S(A,B{)}_{SSD}=\sum _{i,j}(A(i,j)-B(i,j){)}^2$

     • 归一化互相关 Normalized Crossd Correlation，NCC：相关性接近0表示两幅图像不相似，接近1相似。

       $S(A,B{)}_{NCC}=\frac{\sum _{i,j}A(i,j)B(i,j)}{\sqrt{\sum _{i,j}A(i,j{)}^{2}B(i,j{)}^2}}$

   • 深度滤波器：假设用NCC，即会得到一个沿着极线的NCC分布。此时应该用概率分布描述深度值。
     

     假设为高斯分布，则某个像素点的深度d服从 $P(d)=N(\mu ,{\sigma }^2)$，此像素点的观测深度分布为$P(d_{obs}=N({\mu }_{obs},{\sigma }_{obs}^2))$。用观测的信息更新原先d的分布，这是一个信息融合问题。显然，两个高斯分布的乘积依然是一个高斯分布设融合后的分布为$dN({\mu }_{fuse},{\sigma }_{fuse}^2)$，则
     $$\begin{gathered} {\mu }_{fuse}=\frac{{\sigma }_{obs}^2\mu +{\sigma }^2{\mu }_{obs}}{{\sigma }^2+{\sigma }_{obs}^2} \\ {\sigma }_{fuse}^2=\frac{{\sigma }^2{\sigma }_{obs}^2}{{\sigma }^2+{\sigma }_{obs}^2} \end{gathered}$$

   • 估计稠密深度的完整过程：

5. 像素梯度问题：有明显梯度的小块将具有良好的区分度，不易引起误匹配。对于梯度不明显的像素，由于块匹配时没有区分性，将难以有效估计其深度。即具有对物体纹理的依赖性。

6. 逆深度 Inverse depth：深度的倒数。假设逆深度为高斯分布是更有效地手段。

7. 八叉树地图 Octo-map：把一个小方块的每个面平均切成两片，即将一个小方块分成同样大小的八个。整个从最大空间细分到最小空间的过程，就是一颗八叉树。当某个方块的所有子节点都被占据或者都不被占据是，就没必要展开这个节点。因此节省了大量的存储空间。