If you wish for peace, prepare for war

双线性插值双三次插值 2021-07-16|数学基础

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

双线性插值 Bilinear Interpolation

一次线性插值.png
普通的线性插值我们都很熟悉。双线性插值是有两个变量的插值函数的线性插值扩展，其核心思想是在两个方向分别进行一次线性插值。
示意图.png
二次线性插值的公式.png
看这个推导

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

双三次插值 Bicubic interpolation

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

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

$\mathop{\Sigma}\limits_{i=0}\limits^{3} \mathop{\Sigma}\limits_{j=0}\limits^{3} a_{ij}x^iy^j$

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

矩阵的分解 2021-07-15|数学基础

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_{mn} = \left[ \begin{matrix} \Sigma_1 & 0 \\ 0 & 0 \\ \end{matrix} \right]$ $\Sigma_1=diag(\sigma_1....\sigma_r)$

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

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

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

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

矩阵求导 2021-07-15|数学基础

最常用：

$\frac{\partial Ax}{\partial x} = A^T$ $\frac{\partial x^TA}{\partial x} = A$

矩阵求导 1.jpg

$\frac{dTr(AB)}{dA} = B^T$ $\frac{d(ABA^T)}{dA} = 2AB$

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

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

叉乘的导数：

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

利文伯格法 2021-07-15|算法推导

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

推导.png

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

$\left| \ f(x_1)-f(x_2) \right| \leq L\left| \ x_1-x_2 \right|$

阻尼因子μ作用如下：

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

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

利文伯格法的特点：

μ大于0 保证了系数矩阵的正定，解决了高斯牛顿法的缺陷，迭代朝着下降方向进行。
即使初值距离(局部)最优解非常远，利文伯格法仍可以求解成功
利文伯格法的收敛速度要稍微低于高斯牛顿法，但更鲁棒
可以在一定程度避免系数矩阵的奇异和病态问题

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

矩阵之所以是稀疏，根本原因是约束只和它周边少数的节点有关。

线搜索方法：首先找到一个下降的方向，目标函数将沿其下降。然后再确定步长，从而决定沿该方向到底移动多远。下降方向可以通过各种方法计算，如梯度下降法、牛顿法和拟牛顿法。步长可以精确或不精确地确定。
置信域法Trust Region: 在搜索空间的子集内应用模型函数（通常是二次方程）来近似目标函数，这个空间被称为置信域。如果模型函数成功地使真正的目标函数最小化，则扩大置信域。否则收缩置信域并且再次尝试求解模型函数的优化问题。

正定矩阵对称矩阵反对称矩阵奇异矩阵病态矩阵 2021-07-14|数学基础

正定矩阵

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

性质:

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

实数域上所说的正定矩阵都是对称矩阵，在复数域上是厄米特矩阵（共轭对称）

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

求出A的所有特征值，若全为正数，则A是正定的；若特征值全为负数，则A为负定的。
若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时如果对数据进行较小的扰动，则得出的结果具有很大波动，这样的矩阵称为病态矩阵。

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

李群李代数 2021-07-14|算法推导

导数的定义是这样的

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

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

另外两个原因：

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

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

李群和李代数

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

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

$R(t)R(t)^T = I$

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

$\dot R(t)R(t)^T = \phi(t)^\land$

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

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

$R = exp(\phi^\land)$

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

在不引起歧义的情况下，我们说李代数 $\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的导数。我们经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。但是旋转矩阵和变换矩阵，它们对加法都没有良好的定义，所以对姿态有关的函数求导，只能通过李代数进行。关于用李代数解决求导问题，有两种思路：

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

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

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

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

$\frac{∂(exp(\phi^\land)p)}{∂\phi} =−(Rp)^\land J_l$

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

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

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

左扰动模型

$\frac{∂(Rp)}{∂\varphi} =−(Rp)^\land$

右扰动模型

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

旋转连乘的求导的推导

逆旋转点的求导

逆旋转函数的求导

高斯牛顿法 2021-07-13|算法推导

推导过程

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

或者简单的推导
推导 1.png
推导 2.png

特点：

要求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.高斯牛顿法

牛顿法梯度下降法 2021-07-13|算法推导

非线性最小二乘

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

$\min_{x} \frac{1}{2} {\mid \mid f(x) \mid \mid}^2_{2}$

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

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

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

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

非线性优化中的牛顿法

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

$f(x+\Delta x) \approx f(x) + f'(x)\Delta x + \frac{1}{2}f''(x)\Delta x^2$

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

$\Delta x = - \frac{f'(x)}{f''(x)}$

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

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

$\left\|f(x+\Delta x)\right\|_2^2 \approx \left\|f(x)\right\|_2^2 + J(x)\Delta x + \frac{1}{2}\Delta x^TH\Delta 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)与牛顿法

机器人到障碍物的距离 calculateDistance 2021-06-28|路径规划TEB算法

在TEB中的footprint模型中提到calculateDistance函数计算机器人到障碍物的 Euclidean 距离，用于图优化环节。

以最简单的Point类型为例

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

Obstacle是个抽象类，它的派生类有

PointObstacle
CircularObstacle
PillObstacle
LineObstacle
PolygonObstacle

显然不同的障碍类型对函数有不同实现，这里又涉及到costmap_converter和Homotopy，这就更复杂了

createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision

TEB中的footprint模型 2021-06-17|路径规划TEB算法

TEB算法中的footprint模型是单独构建的，而没有直接调用通用代价地图中的 footprint，二者footprint模型不同，但在TEB中二者都有使用。

TEB的footprint用于图优化，costmap的 footprint用于 `feasibility check`环节

LayeredCostmap类的成员函数： double getInscribedRadius() { return inscribed_radius_; }

对于polygon轮廓，两种footprint模型的内接半径相同， TEB的模型没有返回外接半径的函数

在TebLocalPlannerROS::initialize中定义: RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);，然后进行模型验证validateFootprints

在planner初始化时，也就是在TebOptimalPlanner::initialize中赋值给robot_model_.

RobotFootprintModelPtr 其实就是 BaseRobotFootprintModel类型的 boost::shared_ptr

很显然BaseRobotFootprintModel是个抽象类，定义了机器人footprint/contour models的接口，根据yaml设置的类型而定义相应的类，这个类只用于optimization，代价地图的footprint只用于检查路径的可行性

Point 和 circular模型的footprints要求算力最少。
two-circles模型要求的是两次 distance calculations
line模型增加了一些 simple case checking, 但是算力在大多数情况仍要求不高。
polygon模型对每一个edge都要求 distance check，所以计算时间主要取决于 edges和vertices 的数量

也就是说这几种模型的算力要求依次增大。

RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh)
{
  std::string model_name; 
  if (!nh.getParam("footprint_model/type", model_name))
  {
	// 如果未指定类型，则使用点类型
    return boost::make_shared<PointRobotFootprint>();
  }
  // point  
  if (model_name.compare("point") == 0)
  {
      ROS_INFO("Footprint model 'point' loaded for trajectory optimization.");
      return  boost::make_shared<PointRobotFootprint>();
  }
  // circular   省略
  if (model_name.compare("circular") == 0)
  // line  省略
  if (model_name.compare("line") == 0)
  // two circles 省略
  if (model_name.compare("two_circles") == 0)

  // polygon  只对 Point2dContainer vertices_; 赋值
  if (model_name.compare("polygon") == 0)
  {
    // check parameters
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) )
    {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for 
      	trajectory optimization, since param '" << nh.getNamespace() << 
      	"/footprint_model/vertices' does not exist. Using point-model instead");
        return boost::make_shared<PointRobotFootprint>();
    }
    // get vertices
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      try
      {
        Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, 
        	"/footprint_model/vertices");
        ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization.");
        return boost::make_shared<PolygonRobotFootprint>(polygon);
      } 
      catch(const std::exception& ex)
      {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: "
        	<< ex.what() << ". Using point-model instead.");
        return boost::make_shared<PointRobotFootprint>();
      }
    }
    else
    {
      ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, 
      	since param '" << nh.getNamespace() << "/footprint_model/vertices' does not 
      	define an array of coordinates. Using point-model instead.");
      return boost::make_shared<PointRobotFootprint>();
    }
  }
  return boost::make_shared<PointRobotFootprint>();
}

validateFootprints

// TebLocalPlannerROS::initialize中的调用
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, 
    double costmap_inscribed_radius, double min_obst_dist)
{}

函数就一句ROS_WARN_COND，要求 TEB优化所用的机器人内接半径 + 参数 min_obstacle_dist > costmap中的内接半径(包括footprint_padding) ，否则Infeasible optimziation results会频繁出现

这里就是设置为点类型的缺陷之处，第1参数个会是0，但是如果min_obstacle_dist超过costmap中的内接半径，会无法走窄通道。

BaseRobotFootprintModel的派生类实现了calculateDistance函数，计算机器人到障碍物的 Euclidean距离，用于图优化环节。以最简单的Point类型为例

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

至于细节，需要单独一篇文章分析

变量robot_model_在之后用于图优化时的一系列函数：
AddEdgesObstacles
AddEdgesObstaclesLegacy
AddEdgesDynamicObstacles
AddEdgesVelocityObstacleRatio，也就是所有和障碍相关的约束

以及可视化中的publishInfeasibleRobotPose