插值指在离散数据的基础上补插连续函数，使得连续曲线 通过全部给定的离散数据点。 插值的本质 —— 利用已知数据估计未知位置数值。插值和拟合的不同之处在于：对于给定的函数，插值 要求离散点“坐落在”函数曲线上从而满足约束；而 拟合 则希望离散点尽可能地 “逼近” 函数曲线。

双线性插值 Bilinear Interpolation

普通的线性插值我们都很熟悉。 双线性插值是有两个变量的插值函数的线性插值扩展，其核心思想是在两个方向分别进行一次线性插值。


看这个推导

双线性插值在三维空间的延伸是三线性插值。

双三次插值 Bicubic interpolation

二维空间中最常用的插值方法。在这种方法中，函数f在点(x , y)的值可以通过矩形网格中最近的十六个采样点的加权平均得到，在这里需要使用两个多项式插值三次函数，每个方向使用一个。

双三次插值通过下式进行计算

计算系数的过程依赖于插值数据的特性。如果已知插值函数的导数，常用的方法就是使用四个顶点的高度以及每个顶点的三个导数。一阶导数与表示x与y方向的表面斜率，二阶相互导数表示同时在x与y方向的斜率。这些值可以通过分别连续对x与y向量取微分得到。对于网格单元的每个顶点，将局部坐标(0,0), (1,0), (0,1)和(1,1)带入这些方程，再解这16个方程

choskey分解

Cholesky分解一个重要的应用就是解方程组 Ax = B，其中A是一个正定矩阵。因为A是一个正定矩阵，所以有A =LL^T，其中L是一个下三角矩阵。原方程组可以写成 LL^Tx = B。如果令 y = L^Tx ，则有Ly = B。注意到L是一个下三角矩阵，所以从下向上求解y是非常容易的. 求解出y之后，在按照类似的方法求解y = L^Tx 中的 x，而其中L^T是一个上三角矩阵，所以最终求出 x 也是非常容易的

cholesky分解又称为平方根法，是A为实对称正定矩阵时，LU分解的变形。

协方差矩阵是实对称半正定的，如果对角线元素全为正，则可进行cholesky分解， $\Sigma = LL^T$

计算样本中两个特征向量的距离，可以用马氏距离表示

直接对协方差求逆比较复杂，使用cholesky分解

LDLT

LDLT分解法实际上是Cholesky分解法的改进， 优先使用LDLT而不是LLT方法。 Cholesky分解法虽然不需要选主元，但其运算过程中涉及到开方问题，而LDLT分解法则避免了这一问题。 若对称矩阵A的各阶顺序主子式不为零时，则A可以唯一分解为 $A=LDL^T$ 。 其中 L 为下三角单位矩阵 (即主对角线元素皆为 1，下三角其他元素不为0)，D为对角矩阵，$L^T$ 为L的转置矩阵。

LDLT则可以应对半正定和负半定问题，精度较LLT更高

QR分解

对于任意的实数矩阵 $A\in C^{n\times n}$，存在 n阶正交矩阵 Q 和 n阶上三角矩阵 R，使得 $A=Q*R$。注意：矩阵A可以是非方阵。 正交矩阵： $Q^{-1}=Q^T$

如果A是非奇异的，且限定R的对角线元素为正，则这个分解是唯一的。

实际计算有Givens旋转、Householder变换，以及Gram-Schmidt正交化等。Eigen常用的是 Householder变换

用于求线性方程组

对于直接求解线性方程组的逆，用QR分解的方法求解会更具有数据的稳定性。 对于求解一个线性系统Ax = b, 这里A的维度是 m x n。

上三角矩阵R的逆矩阵仍然是上三角矩阵，可以用分块矩阵迭代的方法很容易地求出来

下三角矩阵的逆矩阵也是类似求法

SVD分解 - 奇异值分解

特征值分解仅针对方阵，而不是方阵的矩阵就有了SVD分解： $A=U\Sigma V^T$

其中A为m x n的矩阵， 正交矩阵 U(m x m阶) 和 V(n x n阶)。

其中 $\sigma_i$ 是矩阵 $\Sigma_{1}$ 的特征值

矩阵U的列称为左奇异向量，是正交的。 矩阵V的列向量(也称为右奇异向量)也是正交的.

此时的A如果是方阵，那么逆矩阵也很容易求出: $A^{-1} = V\Sigma^{-1}U^T$

奇异值分解同时包含了旋转、缩放($\Sigma$)和投影三种作用。特征值分解只有缩放的效果。

最常用：

左边为分子布局，右边为分母布局，不推荐固定用哪个

不管啥layout归根结底一句话…保证雅可比矩阵乘起来的时候维度对应即可。如果有方阵，那肯定某两层中间变量的维度相同的，直接强行让两者的维度记号不一样即可。

叉乘的导数：

举例： $\frac{1}{2} ||Ax- \overrightarrow {b} ||^2$ 对 x 求导，最后结果是 $A^T(Ax - b)$

利文伯格法是对高斯牛顿法的改进，是把梯度下降法与高斯牛顿法进行线性组合，属于信赖区域法，认为近似只在区域内可靠，控制每一步迭代的步长以及方向。 高斯牛顿法属于线搜索，即先找方向，再确定步长。

一直到 $J(x_k)$ 满足Lipschitz连续且非奇异，算法二次收敛。利普希茨连续的定义为：对于函数 f(x)，若其任意定义域中的 $x_1$, $x_2$，都存在 $L>0$，使得

阻尼因子μ作用如下：

• μ非常大，说明此时高斯牛顿的一次泰勒展开近似效果不好，更新方式偏向近似最速下降法。

• μ比较小，说明此时高斯牛顿的一次泰勒展开近似效果挺好的，更新方式偏向近似高斯牛顿法。

狗腿法就是引入信赖区域代替LM算法中的阻尼项

利文伯格法的特点：

• μ大于0 保证了系数矩阵的正定，解决了高斯牛顿法的缺陷，迭代朝着下降方向进行。

• 即使初值距离(局部)最优解非常远，利文伯格法仍可以求解成功

• 利文伯格法的收敛速度要稍微低于高斯牛顿法，但更鲁棒

• 可以在一定程度避免系数矩阵的奇异和病态问题

对于以上几种方法， 不会直接求系数矩阵的逆，而是用矩阵分解，例如QR, Cholesky分解，这个矩阵往往有稀疏形式，为SLAM的实时求解提供了可能。SLAM中，Pose Graph的结构会越来越大，H矩阵如果不是稀疏的，维数会达到几千，求逆矩阵会极其耗时，不可能实时求解。

• 线搜索方法： 首先找到一个下降的方向，目标函数将沿其下降。然后再确定步长，从而决定沿该方向到底移动多远。 下降方向可以通过各种方法计算，如梯度下降法、牛顿法和拟牛顿法。步长可以精确或不精确地确定。

• 置信域法Trust Region: 在搜索空间的子集内应用模型函数（通常是二次方程）来近似目标函数，这个空间被称为置信域。如果模型函数成功地使真正的目标函数最小化，则扩大置信域。否则收缩置信域并且再次尝试求解模型函数的优化问题。

正定矩阵

定义: A是n阶方阵，如果对任何非零向量x，都有 $x^TAx$ >0，其中 $x^T$ 表示x的转置，就称A正定矩阵。

性质:

• 行列式恒为正
• 正定矩阵可逆。若矩阵A正定，则必有|A|（矩阵A的行列式）>0，所以矩阵A可逆
• 所有的特征值都是正的
• 实对称矩阵A正定， 当且仅当 A与单位矩阵合同
• 两个正定矩阵的和是正定矩阵， 乘积也是正定矩阵

判别实对称矩阵A的正定性有两种方法：

1. 求出A的所有特征值，若全为正数，则A是正定的； 若特征值全为负数，则A为负定的。

2. 若A的所有顺序主子式都大于零，则A是正定的； 若A的奇数阶顺序主子式为负，偶数阶为正，则A为负定的。

半正定矩阵

正定矩阵的几何意义

定义中的 $x^TAx>0$ 变为 $x^TY>0$ ($Y=Ax$)

那么有 $cos(\theta)=\frac{x^T Y}{\Vert x^T \Vert \Vert Y \Vert} > 0$

也就是说，一个向量 x 经过矩阵A变换后(左乘)，与其转置向量的夹角小于90°

反对称矩阵

也就是$A^T = -A$，或者说元素关系满足 $A_{ij} = -A_{ji}$ 主对角线元素皆为0

• 斜对称矩阵自身相乘的积是对称矩阵。
• 任意矩阵 $A$，$A^T - A$是斜对称矩阵。
• 若$A$是斜对称矩阵，$x$是向量，$x^T A x$ = 0

稀疏矩阵

在矩阵中，若数值为0的元素数目远远多于非0元素的数目，并且非0​元素分布没有规律时，则称该矩阵为稀疏矩阵

奇异矩阵(singular)

奇异和非奇异矩阵仅针对n阶方阵。 奇异矩阵就是对应的行列式等于0的矩阵。 若一个n阶矩阵是可逆的，则称它为非奇异矩阵。

奇异矩阵的判定方法: 行列式|A|是否等于0，若等于0，称矩阵A为奇异矩阵

非奇异矩阵的判定方法：

• 一个矩阵非奇异 当且仅当 它的行列式不为零
• 一个矩阵非奇异 当且仅当 它的秩为n
• 可逆矩阵就是非奇异矩阵，非奇异矩阵也是可逆矩阵

病态矩阵

求解方程组Ax=b时如果对数据进行较小的扰动，则得出的结果具有很大波动，这样的矩阵称为病态矩阵。

最根本的原因在于矩阵的列之间的相关性过大。

导数的定义是这样的

SLAM问题会构建一个优化问题，求解最优的R t，使误差最小化。这必然涉及求导，SLAM的运动过程是连续的，所以一定是可以求导的。在SLAM中最常见的变换是四维变换矩阵T以及三维旋转矩阵R。但是它们对加法并不封闭: 两个变换矩阵之和明显不是变换矩阵；而旋转矩阵是正交矩阵，但两个正交矩阵之和不是正交矩阵。需要把函数f换成旋转矩阵R，对自变量添加一个微小值来进行，但是前面我们说了，旋转矩阵做加减法运算之后就不再满足旋转矩阵的形式了，没有加减法就意味着不能求导了。

为什么会出现李群李代数的概念，原因就在于此。SLAM应当能求导，只是我们不该用这两种矩阵的形式，而是用李代数。

另外两个原因：

1. 旋转矩阵都是正交矩阵且行列式为1，它们作为优化变量时，会引入额外的约束，使优化更困难了。
2. 优化变量的迭代阶段： $x = x + \Delta x$ 不能直接用两种矩阵相加

引入李群和李代数，就可以解决旋转矩阵求导问题，也就是说李代数se(3)和so(3)对加法封闭，所以用李代数表示机器人的旋转和位移。求导结束后，再对数映射为李群，这个过程都是由优化器完成的，无需我们手动。

李群和李代数

具有连续（光滑）性质的群。整数群是离散的，没有连续性质。SO(3)和SE(3)都是连续的，空间中的机器人的姿态是连续变化的，而不是磕磕绊绊地运动，所以都是李群。

机器人的旋转是一个随时间连续变化的过程，函数是R(t)，根据旋转矩阵的性质

经过一系列推导，发现$\dot R(t)R(t)^T$ 是一个反对称矩阵，因此写做

这里的$\phi$ 是一个三维向量，对应到 SO(3) 的李代数 $\mathfrak{so}(3)$，李群和李代数直接是指数映射。

假设在很小的时间范围内， $\phi(t)$ 是一个恒定值，因为实际中机器人的姿态并不会突变。再经过一系列推导得到

李代数包括一个集合，一个数域和一个二元运算符，它们满足四条性质：封闭性、双线性、自反性、雅克比等价。

在不引起歧义的情况下，我们说李代数$\mathfrak{so}(3)$的元素是三维向量或3维反对称矩阵，不做区别。

对于李群SE(3)，对应的李代数 $\mathfrak{se}(3)$ 每个元素是一个6维向量，前三维是平移，后三维是旋转，实际就是 $\mathfrak{so}(3)$ 元素。 此时的 $\land$ 符号是把6维向量转换为四维的变换矩阵 T

对于李代数 $\mathfrak{so}(3)$的元素 $\phi$，由于它是3维向量，写成 $\phi=\theta\vec a$，$\vec a$ 是和 $\phi$ 方向相同的单位向量，$\theta$ 是 $\phi$的模长。

经过一连串推导，获得

这实际就是罗德里格斯公式 3.14.

旋转矩阵的李代数实际上就是旋转向量组成的空间。指数映射其实就是通过罗德里格斯公式变换的。

BCH公式

根据BCH公式，第2个公式右边还有一些余项

李代数的求导问题

对于公式 4.39，需要计算目标函数J关于变换矩阵T的导数。我们经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。但是旋转矩阵和变换矩阵，它们对加法都没有良好的定义，所以对姿态有关的函数求导，只能通过李代数进行。关于用李代数解决求导问题，有两种思路：

1. 用李代数表示姿态，然后根据李代数加法对李代数进行求导
2. 对李群左乘或者右乘微小扰动，然后对该扰动求导。即左扰动和右扰动模型。

因为旋转矩阵没有加法，所以我们就对旋转矩阵的李代数就行求导，也就是李代数的局部坐标上添加扰动，由于李代数本身对应旋转向量，因此对旋转向量添加扰动相当于同时改变旋转轴和旋转角度。

对一个空间点$p$进行旋转，得到$Rp$，现在计算旋转后的点坐标相对旋转的导数，不严谨地记做 $\frac{∂(Rp)}{∂R}$

由于$SO(3)$没有加法，假设$R$对应的李代数为$\phi$，于是我们计算出

平时不用这种方法，而是用扰动模型法。

不管是李代数求导还是扰动模型，都是旋转矩阵对李代数的求导。

对R进行一次扰动 $\Delta R$，这个扰动可以乘在 $R$左边，也可以是右边。假设 $\Delta R$ 对应的李代数为 $\varphi$，然后对 $\varphi$ 求导

• 左扰动模型

• 右扰动模型

扰动模型，显然计算量上要少非常多。

旋转连乘的求导的推导

逆旋转点的求导

逆旋转函数的求导

推导过程

有些人可能产生疑问，为什么通过最小二乘而不是其他方式获得最优函数，比如说最小化误差的绝对值的和？这里的推导使用最大似然估计推导出 $X^* = argmin\Sigma e_i^T \Omega e_i$ 就提供了概率学基础

或者简单的推导

特点：

• 要求H正定，但实际只保证半正定。如果觉得太抽象，可以把 $J^TJ$ 理解为 $x^2$，把正定理解为正数，半正定理解为非负。
• 可能出现H为奇异矩阵或病态矩阵，此时增量的稳定性较差，算法会不收敛。
• 即使H非奇异非病态，如果步长 $\Delta x$ 太大，也会导致不收敛。
• 只能在展开点附近有好的近似效果

根据十四讲的推导过程，牛顿法是对 $\left\|f(x+\Delta x)\right\|_2^2$ 的二阶展开，高斯牛顿法是对 $\left\|f(x) + J\Delta x\right\|_2^2$ 的展开

参考:
graph slam tutorial ：从推导到应用２
牛顿法 高斯牛顿法
非线性优化整理-2.高斯牛顿法

非线性最小二乘

非线性优化问题就是针对一个非线性函数求最值的问题，但很难通过求导数的方式求解。
非线性最小二乘问题的形式如下：

自变量 $x\in R^n$，f 是任意的非线性函数，我们可以假设它有m维： $f(x)\in R^m$

我们一般用李代数表示机器人的平移和旋转，常常无法对f进行直接求导，所以使用迭代的方式求解，非线性优化的几个解法都是使用迭代

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

非线性最小二乘问题就转为寻找 $\Delta{x_k}$ 的问题，直到满足迭代次数或者目标函数变化非常小为止。此时认为算法收敛，目标函数达到极小值。

非线性优化中的牛顿法

普通的函数求极值就是对f’(x)=0进行求解，但是有时导数难以获得，所以把f(x)进行二阶泰勒展开

右侧对 $\Delta x$ 求导并令其为0，可以获得

也就是$x_{n+1}= x_{n}- \frac{f'(x)}{f''(x)}$

现在放到高维情况下，把最开始的式子在x附近二阶泰勒展开:

变量的增量就可以获得: $H\Delta x = -J^T$

J是关于x的一阶导数，雅克比矩阵；H是二阶导数，海森矩阵。

牛顿法的思路是将函数 f 在 x 处展开为多元二次函数，再通过求解二次函数最小值的方法得到本次迭代的下降方向。那么问题来了，多元二次函数在梯度为0的地方一定存在最小值么？直觉告诉我们是不一定的。以一元二次函数 g(x)=ax²+bx+c为例，我们知道当a>0时，g(x)可以取得最小值，否则g(x)不存在最小值。

推广到多元的情况，可以得出二次项矩阵(Hessian矩阵)必须是正定的，函数f(x)的最小值才存在。因此， 牛顿法首先需要计算海森矩阵并且判断其正定性，这在问题规模很大时非常困难，我们希望避免计算海森矩阵。

牛顿法的特点：

• 在极小值点附近，牛顿法的收敛速度比梯度下降法 快很多
• 需要计算海森矩阵，但问题规模大时，计算难度很大
• 牛顿法经常会因为海森矩阵不正定而发散，因此牛顿法并不是非常的稳定

梯度下降法

对上面公式(1)进行展开时，如果只保留一阶项，对应一阶梯度法，$f(x+\Delta x) = f(x)+J\Delta x$

我们希望迭代过程中的函数值逐步减小，也就是$f(x+\Delta x) \le f(x)$。 可以让$\Delta x = -J^T(x)$ 由于$JJ^T > 0$，所以实现了函数值下降

也就是沿着反向梯度的方向，还要取一个步长，这就是最速下降法。 特点: 最速下降法过于贪心，越靠近极小值速度越慢，容易走出锯齿路线，反而增加迭代次数。

牛顿法是二阶收敛，梯度下降是一阶收敛，所以牛顿法就更快。如果更通俗地说的话，比如你想找一条最短的路径走到一个盆地的最底部，梯度下降法每次只从你当前所处位置选一个坡度最大的方向走一步，牛顿法在选择方向时，不仅会考虑坡度是否够大，还会考虑你走了一步之后，坡度是否会变得更大。可以说牛顿法比梯度下降法看得更远一点，能更快地走到最底部。（牛顿法目光更加长远，所以少走弯路；相对而言，梯度下降法只考虑了局部的最优 ，没有全局思想。）

从几何上说，牛顿法就是用一个二次曲面去拟合你当前所处位置的局部曲面，而梯度下降法是用一个平面去拟合当前的局部曲面。通常情况下，二次曲面的拟合会比平面更好，所以牛顿法选择的下降路径会更符合真实的最优下降路径。

数值分析中的牛顿法

牛顿迭代法最常见的一个应用就是求方根

牛顿法是一种在实数域和复数域上近似求解方程的方法。方法使用函数f(x)的泰勒级数的前面几项来寻找方程f(x) = 0的根。牛顿法最大的特点就在于它的收敛速度很快。

首先，选择一个接近函数f(x)零点的 x0，计算相应的f(x0) 和切线斜率f’(x0)。然后我们计算穿过点(x0, f(x0) )并且斜率为f’(x0)的直线和 x 轴的交点的x坐标，也就是求如下方程的解：
f’(x₀)(x-x₀) + f(x₀) = 0

这样将新求得的点的x坐标命名为x₁，通常x₁会比x₀更接近方程f(x) = 0的解。因此可以利用x₁开始下一轮迭代，迭代公式如下所示:

也就是知道曲线在某个点x₀的切线l，用l和x轴的交点作为下一个迭代点x₁，如此迭代来逼近曲线和x轴的交点。

已经证明，如果f'是连续的，并且待求的零点x是孤立的，那么在零点x周围存在一个区域，只要初始值x₀位于这个邻近区域内，那么牛顿法必定收敛。下图为一个牛顿法执行过程的例子

参考:
牛顿法 高斯牛顿法
梯度下降法(gradient descent)与牛顿法

接上一篇，把激光雷达的扫描数据插入子图，是通过ActiveSubmaps2D所提供的插入器对象range_data_inserter_完成的，也就是ProbabilityGridRangeDataInserter2D::Insert函数，用到了激光扫描的RayCasting模型。这里就涉及到论文的内容了

三维激光SLAM形成的点云地图不需要自己手动实现点云的数据结构, PCL中有写好的数据类型, 直接调用就行. 视觉SLAM形成的点云地图也可以用PCL来实现。

但是二维激光SLAM的栅格地图需要自己手动实现, 目前所有的二维激光SLAM的栅格地图都是SLAM作者自己写的，没有通用的数据结构，最后会转成ROS的地图数据结构。cartogrpaher,gmapping,hector皆是如此

理论基础

首先明确几个概念：

• Probability： 占据概率，下面用 p 表示
• CorrespondenceCost： 空闲概率（浮点型），对应的Value为线性映射到 [1-32767] 整型范围
• Odds： $\frac{p}{1-p}$

hit_table和miss_table存储的是free概率，但设置和获取均为hit概率，为何中间多一个转换步骤，因为匹配操作时采用类似最小二乘的优化思想。

在占据栅格地图中，对于一个点，我们用 $p(s=0)$ 来表示它是Free状态的概率，用 $p(s=1)$ 来表示它是Occupied状态的概率，两者的和为1。

默认情况下，对于一个cell，无论hit或miss事件，$p(s=1)$ 和 $p(s=0)$ 都是0.5。 scan经过cell后，观测值z会更新cell的状态，这显然是个条件概率的问题，现在要考虑的不是 $p(s)$了，而是观测的条件概率： $p(s=1|z)$ 和 $p(s=0|z)$
所以一个栅格实际为 hit 的概率和 miss 的概率，因此可以用贝叶斯公式进行求取

我们希望一个栅格中仅有一个值，因此引入 $Odds$ 这一概念(两者的比值)来衡量cell的状态：

设置一个概率的最大最小值，防止分母出现0的情况，比如[0.1,0.9]。
初始情况下，一个cell的状态没有任何信息，$Odds(s) = 1$。

cell原来的Odd是 $Odd(s)$，现在就变成了 $Odd(s|z)$，再使用贝叶斯公式变换得到

也就是说

其中k-1为上一时刻的 $Odds$ (cell第一次被scan更新)，而k为经过一次观测后的 $Odds$ (cell再次被更新)，显然仅是相差了 $C(z)$这个系数。由于栅格表示是否为障碍物，观测采用激光测距是否障碍，地图中点的更新状态有三种：hit、miss、free(不更新)。

其中$p(z=1 \ |\ s=1)$表示的是当cell为障碍时，传感器认为是障碍的概率，这显然是传感器本身属性决定的，因此为常数。同样其他3种组合也均为常数，因此系数 $C(z)$ 是根据观测hit和miss决定的两个常数，即 $C_{miss}$ 和 $C_{hit}$ 两个常数。当传感器测到此cell为 hit 时，使用 $C_{hit}$ ，反之是 $C_{miss}$

论文中的栅格 hit 概率更新公式是

clamp是c++中的函数：将结果限制在[0, 1]。 odds运算是从占用概率计算odds，逆运算就是反过来。 我理解的是：先不看clamp，两边进行odds运算，公式就变成了这样

与上面的公式(3) 应该是一样的

注意： lua配置中的 hit_probability = 0.55 和 miss_probability = 0.49指的是 $C_{hit}$ 和 $C_{miss}$

其他大部分SLAM实现地图更新采用对数加减的方式：$lg\ odd_{new}(x) = lg\ odd_{old}(x)+lg\ C$

举例如下

同样地，ProbabilityGrid在将点云插入到栅格地图时, 即使这个栅格被之前的点云更新过, 当前帧的点云还是会对这个栅格进行更新

第二次观测会在第一次的基础上累计，其实这里只是个举例，还不太准确，对于图中圈出的栅格，第一次是从栅格中心射过，但第二次是从角落射过， 从对角穿过对这个栅格的空闲概率的提升显然作用更大， 而只穿过一个小角落显然贡献很小，所以第二次不应该是 -0.7， 0.9的栅格也是同样道理

如果栅格地图以 odds 的形式表示，那么更新过程就只是一个乘法运算，效率也还可以。但Cartographer还是想要进一步的提高效率，它以内存空间换取时间，构建了hit表和miss表。 实际上是两个 vector<uint16>， 保存的是空闲概率映射后的索引值。 如果发生了hit事件，就从hit表中查找更新后的数据。发生了miss事件则查找miss表。

ProbabilityGrid在将点云插入到栅格地图时, 新的栅格值是预先通过查找表计算好的, 直接查表就行了, 不用再重新计算.

ProbabilityGridRangeDataInserter2D 和 两个表的构建

类ProbabilityGridRangeDataInserter2D值得关注的成员函数只有Insert，只有三个成员变量：

1
2
3
4

const proto::ProbabilityGridRangeDataInserterOptions2D  options_;
// 用于更新栅格单元的占用概率的查找表
const std::vector<uint16>  hit_table_;
const std::vector<uint16>  miss_table_;

构造函数

1
2
3
4
5
6
7

ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
    : options_(options),
      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()) ) ),
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability())  ))  {}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

// 构建查找表, 处理某一个cell的 CorrespondenceCostValue 已知时  如何更新的情况
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(float odds)
{
  std::vector<uint16> result;
  result.reserve(kValueCount);   // 32768
    /*  三个函数从里到外依次为： 
  对odds转换成占据概率p; 
  求空闲概率 1-p;  
  转换为[1, 32767]的存储值*/
  
  // kUpdateMarker表示此栅格的更新
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds) )) +  kUpdateMarker);
  // 计算存储值从1到32767所对应的更新值并塞进result中
  for (int cell = 1; cell != kValueCount; ++cell)
  {
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]) )  ) )) +
        kUpdateMarker );
  }
  return result;
}

uint16的范围是[0, 65535]，定义的vector有 32768 个元素，先根据参数 odds插入了一个元素，然后在for循环里又插入了剩下的32767个元素。

对第一个元素，把参数odds(lua中配置的hit和miss概率)转为空闲概率对应的[1, 32767]的索引值，然后再加上32768，结果的范围是[32768+1, 65535]。 CorrespondenceCostToValue函数极为复杂，不用研究细节了。

对剩下的元素，所有odds均乘以更新系数（$C_{hit}$ 和 $C_{miss}$）并放入表格中

对于参数hit_probability=0.55, hit_table_第一个成员是47104。 对于参数miss_probability=0.49, miss_table_第一个成员是49562。 两个表大不相同。

变量kValueToCorrespondenceCost的定义：const std::vector<float>* const kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost().release();，其实是一系列的浮点数，查看定义会发现相应函数的参数全都写死了。我运行了一下，发现结果是0和0.1到0.9的32768个数，其中0.1到0.9之间的一大堆数，间隔大约是0.24。 其实是建立了如下的关系

代码里加上了kUpdateMarker，所以Grid2D的成员函数FinishUpdate里会将存储值都减去一个kUpdateMarker

kUpdateMarker 为已更新后的标志量，是为了防止miss和hit重复对地图更新，其值为32768，为uint16的最高位，其操作的方式是，更新时加上，更新完了后减去。

          补充 ，看函数GetProbability获取占用概率

1
2
3
4
5
6

float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const
{
  if (!limits().Contains(cell_index))   return kMinProbability;
  return    CorrespondenceCostToProbability(ValueToCorrespondenceCost(
      correspondence_cost_cells()[ToFlatIndex(cell_index)]) );
}

ValueToCorrespondenceCost是索引值转成空闲概率

参考：
查找表与占用栅格更新

1
2
3
4
5
6
7
8

struct InsertionResult
{
    // 插入的节点数据，包括：子图更新时，节点在局部地图中的位姿，以及有传感器原始数据转换之后的点云信息
    // 记录了更新子图的时刻和重力方向
    std::shared_ptr<const TrajectoryNode::Data>  constant_data;
    //已经建立起来的子图列表
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

保存插入Local Slam的一个节点的数据结构，也就是下面这个东西

在类LocalTrajectoryBuilder2D中，通过对象active_submaps_来维护子图，它是一个ActiveSubmaps2D类型的数据。根据最新版本的代码，
除了刚开始构建的时候，没有子图(Submap2D)，其他时候它都维护着两个子图对象。 一个子图用于进行扫描匹配，称为旧图。另一个子图用于插入扫描数据，作为储备，被称为新图。 当新图中插入一定数量的数据完成了初始化操作之后，它就会被当作旧图，用于扫描匹配。 对象active_submaps_将抛弃原来的旧图，并重新构建一个新图。下表中列出了所有成员变量

ActiveSubmaps2D的构造函数：

1
2
3
4
5

// std::unique_ptr<RangeDataInserterInterface>   range_data_inserter_;
// 构建了插入器对象，代码只是根据配置对 ProbabilityGridRangeDataInserter2D 类的赋值
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
    : options_(options), range_data_inserter_(CreateRangeDataInserter() )
    {}

1
2
3

return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
          options_.range_data_inserter_options()
              .probability_grid_range_data_inserter_options_2d());

从InsertIntoSubmap函数开始，就是向子图插入传感器数据的过程，总流程：

InsertIntoSubmap

1
2
3
4
5
6
7
8
9

struct RangeData {
  Eigen::Vector3f   origin;    // 当次扫描测量时激光雷达的位置
  // PointCloud就是 vector<Eigen::Vector3f>
  // 扫描到的hit点与miss点
  PointCloud   returns;
  PointCloud   misses;
};
// 对2D SLAM， 第三个元素为0
// typedef std::vector<Eigen::Vector3f> PointCloud;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

/*InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation()  );*/
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& range_data_in_local,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment) 
{
  if (motion_filter_.IsSimilar(time, pose_estimate)  )
    return nullptr;
  
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

  return absl::make_unique<InsertionResult>(  InsertionResult{
      std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},
          {},
          {},
          pose_estimate}),
      std::move(insertion_submaps)}  );
}

MotionFilter

motion_filter部分的IsSimilar是这个类唯一值得关注的函数，内容很简单

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

bool MotionFilter::IsSimilar(const common::Time time,
                  const transform::Rigid3d& pose)
{
  LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500)
      << "Motion filter reduced the number of nodes to "
      << 100. * num_different_ / num_total_ << "%.";
  ++num_total_;
  if (num_total_ > 1 &&
      time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) &&
      (pose.translation() - last_pose_.translation()).norm() <=
          options_.max_distance_meters() &&
      transform::GetAngle(pose.inverse() * last_pose_) <=
          options_.max_angle_radians()  )
  {
    return true;
  }
  last_time_ = time;
  last_pose_ = pose;
  ++num_different_;
  return false;
}

ActiveSubmaps2D::InsertRangeData

这里是 插入子图的核心函数，有子图插入点云数据和终结子图两条线

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

std::vector<std::shared_ptr<const Submap2D> > 
ActiveSubmaps2D::InsertRangeData(const sensor::RangeData&  range_data)
{
   // vector<std::shared_ptr<Submap2D> >  submaps_;
  // 如果当前一个子图也没有，或最后一个子图的num_range_data达到配置值(抛弃旧图)
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data() )
  // 删除旧子图，加入新子图。 参数是局部坐标系中的位姿 x y  
  // 新子图的初始位置，为起始range的激光坐标
  // 也就是说num_range_data个scan，只取第1个scan的初始位置
  AddSubmap(range_data.origin.head<2>());
  
  for (auto& submap : submaps_ )
      submap->InsertRangeData(range_data, range_data_inserter_.get() );
  
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data())
      submaps_.front()->Finish();
  return submaps();
}

先看AddSubmap

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin)
{
  if (submaps_.size() >= 2) 
  {
    // 在插入新子图之前，crop the finished Submap
    // 以减少内存的占用。 保证只有两个子图
    CHECK(submaps_.front()->insertion_finished());
    submaps_.erase(submaps_.begin()  );  // 删除旧子图，但对象没有销毁
  }
  submaps_.push_back( absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release()) ),
      &conversion_tables_)  );
}

也就是逻辑如下：

在拥有两个子图后，头部的子图总比尾部多N个scan。 在ScanMatch里会看到，头部的子图(旧图)用于 real_time_correlative_scan_matcher

Submap2D 和 Grid2D

它是ActiveSubmaps2D中维护的子图数据类型， 该数据类型继承自Submap。父类Submap定义了2D和3D子图的一些共有的属性，该类主要描述了子图在local坐标系下的相对位姿，记录插入的数据数量以及是否构建完成等基本信息。

Submap的成员变量如下：

1
2
3

const  transform::Rigid3d   local_pose_;  // 子图在local坐标系的位姿 
int  num_range_data_  = 0;   // 子图中插入的数据数量
bool  insertion_finished_ = false;   // 标志着子图是否已经构建完成

value_to_correspondence_cost_table_: 将[0, 1~32767] 映射到 [0, 0.1~0.9] 的转换表
update_indices_记录已经更新过的索引

函数CreateGrid用于为子图创建栅格信息存储结构。在获取了子图尺寸和分辨率信息之后，就构建了一个ProbabilityGrid类型的栅格存储。(一般不用TSDF格式)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
    const Eigen::Vector2f& origin)
{
  constexpr int kInitialSubmapSize = 100;
  float resolution = options_.grid_options_2d().resolution();
  // 省略TSDF格式
  // 构建了一个ProbabilityGrid 类型的栅格存储
  return absl::make_unique<ProbabilityGrid>(
      MapLimits(resolution,
          origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
          Eigen::Vector2d::Ones(),
          // CellLimits函数只有赋值给成员 num_x_cells和num_y_cells
          // 只保证num_x_cells和num_y_cells大于0
          CellLimits(kInitialSubmapSize, kInitialSubmapSize) ),
      &conversion_tables_  );
}

Submap2D::InsertRangeData

回头还看InsertRangeData，发现出现了一个Submap2D::InsertRangeData

1
2
3
4
5
6
7
8
9

void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) 
{
  CHECK(grid_);
  CHECK(!insertion_finished());
  range_data_inserter->Insert(range_data, grid_.get());
  set_num_range_data(num_range_data() + 1);
}