概述 这种机器人的自主运动过程中，有两大基本问题——我在什么地方？周围环境是什么样子的？也就是定位和建图的问题，而想完成定位与建图问题，其中定位侧重于对自身的了解，建图侧重于对外在环境的了解，二者相互关联，而这，就需要依靠传感器完成对外界环境的感知，也就是说，在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己的运动 传感器主要是有两种： (1) 携带于机器人本体上的传感器，例：机器人的轮式编码器、相机、激光传感器、惯性测量单元(IMU)等 (2) 安装于环境之中的传感器，例：导轨、二维码标志等 但是环境传感器是有限制的，比如说GPS需要在能接收到卫星信号的环境下才可以工作 而本体上传感器具有以下优点：激光、相机等携带式传感器测量的通常都是一些间接的物理量而不是直接的位置数据，所以更加自由使用携带式传感器来完成SLAM也是我们重点关注的问题 slam介绍 slam是什么呢？slam就是同时建图与定位，我们先不用学术的说法去介绍它，而是先用实际生活中的例子来介绍。 大家知道，如果你在一个地方生活久了，就会很熟悉这个地方，可能随便把你扔在其中某一个地点，你就可以知道你在什么位置，你应该怎么走回家，但是如果你出现在一个陌生的没去过的地方，你可能就一时半会不知道自己的位置了。 这个过程其实就是一个slam的过程，你在熟悉的环境下走路，相当于已经有一个地图在你大脑里面了，你需要根据眼睛看到的信息（相当于传感器感知环境）来判断自己在这个地图的什么地方，然后就可以进行自主导航了。 视觉传感器 视觉SLAM是本书的主题，所以我们非常关心小萝卜的眼睛能够做些什么事。即如何用相机解决定位和建图的问题。 - SLAM中使用的相机更加简单，以一定速率采集图像、形成视频,相机的特点就是以二维投影形式记录了三维世界的信息 - 但是该过程丢掉了一个维度：距离(或深度) 相机分类 - 单目相机 Monocular：只使用一个摄像头的相机，通过相机的运动形成视差，可以测量物体相对深度 - 优点：结构简单，成本低，便于标定和识别 - 缺点：在单张图片里，无法确定一个物体的真实大小。它可能是一个很大但很远的物体，也可能是一个很近很小的物体，即单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子也就是尺度(scale)，单凭图像无法确定这个真实尺度，所以称尺度不确定性。 - 双目相机(立体相机)Stereo：由两个单目相机组成，通过基线来估计每个像素的空间位置(类似于人眼) - 优点:基线距离越大，能够测量的距离就越远;并且可以运用到室内和室外。 - 缺点:配置与标定较为复杂，深度量程和精度受到双目基线与分辨率限制，计算非常消耗计算资源，需要GPU(图形处理器)/FPGA设备(现场可编程门阵列)加速用两部相机来定位。 - 深度相机 RGB-D：通过红外结构光或ToF(time of fly)的物理方法测量物体深度信息 - 优点:相比于双目相机可节省大量的计算资源。 - 缺点:是测量范围窄，噪声大，视野小，易受日光干扰，无法测量透射材质等问题，主要用在室内，室外很难应用。 深度相机主要用来三维成像，和距离的测量。 - 其他相机：全景相机、Event Camera（事件相机） SLAM流程 1、传感器信息读取:在视觉SLAM中主要为相机图像信息的读取和预处理。 2、前端视觉里程计(Visual Odometry，VO)视觉里程计的任务是估计相邻图像间相机的运动，以及局部地图的样子。(VO又称为前端) 3、回环检测(Loop Closure Detection)用于判断机器人是否到达过先前的位置。 4、后端(非线性)优化(optimization)。对不同时刻的视觉里程计测量的相机位姿及回环检测的信息进行优化，得到全局一致的轨迹和地图。 5、建图(Mapping)。根据估计的轨迹，建立任务要求对应的地图。 前端视觉里程计 视觉里程计通过相邻帧间的图像估计相机运动，并恢复场景的空间结构，但只计算相邻时刻的运动，不关心再往前的信息。但前端过程中必然存在误差，误差会不断累积，形成累积漂移(会发现原本直的走廊变成了斜的，而原本90°的直角变成一歪的)。为消除漂移，我们需要回环检测和后端优化。 后端优化 定义:如何处理前端所传噪声的数据，从带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——称为最大后验概率估计 通常来说，前端给后端提供待优化的数据，以及这些数据的初始值。后端负责整体的优化过程，它往往面对的就只有数据。其反映了SLAM问题的本质:对运动主体自身和周围环境空间不确定性的估计。(状态估计理论——估计状态的均值和不确定性) 回环检测 回环检测的作用:主要解决位置估计随时间漂移的问题(通俗的理解就是，假设机器人经过一段时间又回到了原点(事实)，但是我们的位置估计值没有回到原点，怎么解决) 回环检测要达到的目标:通过某种手段，让机器人知道“回到原点”这件事情，让机器人具有识别到过的场景的能力。再把位置估计值“拉”过去 相机检测手段:判断与之前位置的差异，计算图像间相似性。 回环检测后:可将所得的信息告诉后端优化算法，把轨迹和地图调整到符合回环检测结果的样子。 地图 地图大体上可分为以下两类: 1. 度量地图(强调精确的表示地图中的位置关系)，常用稀疏与稠密进行分类 稀疏地图:即由路标组成的地图 稠密地图:着重于建模所有看到的东西(可用于导航)(耗费大量的储存空间) 2. 拓扑地图(更加强调元素之间的关系)，是一个图:由节点和边组成，例:只关注A、B点是连通的，而不考虑如何从A点到达B点，不适用于表达较为复杂结构的地图 传感器对比 激光雷达与相机对比 - 激光雷达有效距离远，可以达到百米级别（可达300m），相机有效距离相对近 - 雷达贵，动辄上万，相机较为便宜，高速相机不过上千 - 激光雷达受天气影响较大，雨雪都会成为巨大干扰，因为雨雪也会反射激光 - 雷达重，激光较轻 - 在纹理不清楚或者光线过强过弱的地方，相机几乎无法运作 SLAM数学表述 但是只有对各个模块的组成和功能有一定的认知还是不够的，我们无法根据这种直观了解写出可以运行的程序，需要上升到数学层面进行描述和建模 首先，机器人会携带某种传感器在未知环境中运动，如何用数学语言描述这件事呢，我们知道，相机通常是在某些时刻采集数据的，所以我们也关心这些时刻的位置和地图，也就是一段连续时间的运动变成了离散时刻当中发生的事，在这些时刻，使用 $\(\boldsymbol x\)$ 表示机器人的位置，使用下标来区分不同时刻的位置，这些时刻的位置就构成了机器人的轨迹 假设地图是由许多个路标组成，每个时刻，传感器会测量到一部分路标点，得到他们的观测数据，设路标点有N个，用 $\(\boldsymbol{y}_1,\boldsymbol{y}_2,\cdots,\boldsymbol{y}_N\)$ 表示 这样，我们需要考虑以下两件事情，还真是，机器人带着传感器在环境中运动是由如下两件事描述的： 1. 什么是运动？从 $\(k-1\)$ 时刻到 $\(k\)$ 时刻，小萝卜的位置是如何变化的？ 2. 什么是观测？假设小萝卜在 $\(k\)$ 时刻位于 $\(\boldsymbol{x}_k\)$ 处观测到了某个路标 $\(\boldsymbol{y}_k\)$ ，我们如何用数学语言描述呢？ 首先，运动模型可以描述运动，资料机器人会携带一个测量自身运动的传感器，这个传感器可以测量有关运动的读数，但不一定直接就是位置之差，还可能是加速度、角速度这些信息。我们可以用一个抽象的数学模型来描述 $$\boldsymbol{x}k = f(\boldsymbol{x}}, \boldsymbol{uk, \boldsymbol{w}_k) $$ $\(\boldsymbol{u}_k\)$ 是运动传感器的读数，$$ \boldsymbol{w}_k $$ 是运动过程中加入的噪声（因为真实物理世界中的传感器都会带有噪声） 然后是观测模型，机器人在 $\(\boldsymbol{x}_k\)$ 位置上看到某个路标点 $\(\boldsymbol{y}_j\)$，产生了一个观测数据 $\(\boldsymbol{z}_{k,j}\)$，同样可以用一个抽象的数学模型描述： $$\boldsymbol{z}} = h(\boldsymbol{yj, \boldsymbol{x}_k, \boldsymbol{v}) \quad $$ $\(\boldsymbol{v}_{k,j}\)$ 是观测的噪声 实际上，这只是一个简化的方程，描述了一种形式，在真实世界中，根据机器人的真实运动和传感器的种类，存在着若干种参数化形式。而考虑视觉SLAM时，传感器是相机，则观测方程就是“对路标点拍摄后，得到图像中的像素”的过程，若考虑激光SLAM时，传感器是激光雷达，则观测方程就是对路标点（或环境表面）进行扫描后，得到该点在雷达坐标系下的三维坐标（或距离与方位角） 运动方程 这里举一个运动方程的例子，假设机器人在平面运动，那么位姿（位置+姿态）由两个位置的坐标和一个转角来描述 $\(\boldsymbol x_k = (x, y, \theta)^T\)$，同时，运动传感器能够测量到机器人在任意两个时间间隔位置和转角的变化量 $\(\boldsymbol u_k = (\Delta x, \Delta y, \Delta \theta)^T\)$，于是，此时运动方程就可以写成： $\(\begin{pmatrix} x \\ y \\ \theta \end{pmatrix}_{k} = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix}_{k-1} + \begin{pmatrix} \Delta x \\\Delta y \\\Delta \theta\end{pmatrix}_{k} + \boldsymbol w_k\)$ 学过现代控制理论、理论力学等课程的同学可能会对这种方程有印象，实际上运动方程本质上是通过系统运行的物理规律构建出的方程，而且上面的方程是一种很简单的线性方程，但是并不是所有的输入指令都会如此简单，诸如油门和操纵杆的输入就是速度或者加速度量，并且也有其他的更加复杂的运动方程形式 观测方程 关于观测方程，比如机器人携带着一个二维激光传感器（当激光传感器观测一个2D路标点时，可以测到路标点与机器人之间的距离 $\(r\)$ 和夹角 $\(\phi\)$，记路标点 $\(\boldsymbol y_j = [y_1, y_2]^T_j\)$，位姿为 $\(\boldsymbol x_k = [x_1, x_2]^T_k\)$，观测数据为 $\(\boldsymbol z_{k,j} = [r_{k,j}, \phi_{k,j}]^T\)$，那么观测方程可以写成 $\(\begin{bmatrix} r_{k,j} \\\phi_{k,j} \end{bmatrix}_{k} = \begin{bmatrix} \sqrt{(y_{1,j} - x_{1,k})^2 + (y_{2,j} - x_{2,k})^2} \\\arctan \left( \frac{y_{2,j} - x_{2,k}}{y_{1,j} - x_{1,k}} \right) \end{bmatrix}_{k} +\boldsymbol v\)$ SLAM基本方程 针对不同的传感器，两个方程有不同的参数化形式，如果出于通用性考虑，那么就可以对其进行抽象，并且总结为两个基本方程 $\(\begin{cases} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1},\boldsymbol{u}_{k},\boldsymbol{w}_{k}\right), & k=1,\cdots,K \\ \boldsymbol{z}_{k,j}=h\left(\boldsymbol{y}_{j},\boldsymbol{x}_{k},\boldsymbol{v}_{k,j}\right), & (k,j)\in O \end{cases}\)$ O 表示观测集合，即哪些时刻观测到了哪些路标 三维空间刚体运动 想描述三维空间中的物体运动，就必须先确定坐标系的概念，因为运动都是相对的，我们无法描述一个物体的绝对运动情况，只能描述一个物体的相对运动情况 点与坐标系 三维空间由3个轴组成，则一个空间点的位置可以由3个坐标指定，对于一个刚体（在运动和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体，与软体相对），不光有位置，还有自身的姿态，如相机可以看成三维空间中的刚体，则位置就是说相机在空间中的哪个地方，姿态是指相机的朝向（相机处于空间(0，0，0)处，朝向正前方），这些情况都需要使用数学语言进行描述 - 点：没有长度，没有体积，但是点和点可以组成向量 - 向量是带指向性的箭头(有方向性)，可以进行加法、减法等运算 - 坐标：当我们指定坐标系后，才可以谈论该向量在此坐标系下的坐标 - 坐标系：实际上是构成线性空间的一组基，分为左手系和右手系，在机器人领域，一般使用右手系，机器人的运动也都是在右手系里面进行讨论 [图片] 对于向量的运算，如内积、外积，这里不再赘述，但是对于外积，这里引入一个特殊的记法，如下 $\(\boldsymbol a \times \boldsymbol b = \begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{bmatrix} = \begin{bmatrix} a_2 b_3 - a_3 b_2 \\ a_3 b_1 - a_1 b_3 \\ a_1 b_2 - a_2 b_1 \end{bmatrix} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \boldsymbol b \triangleq \boldsymbol a^\wedge \boldsymbol b\)$ 也就是将叉乘写成矩阵相乘的方式，并且对应的叉乘矩阵 $\(\boldsymbol a^\wedge\)$ 实际上是一个反对称矩阵，也就是满足 $\((\boldsymbol a^\wedge)^{-1}=(\boldsymbol a^\wedge)^T\)$ 并且很容易得知，反对称符号实际上是一种一一映射，如此记法可以简化数学推理 坐标变换与旋转矩阵 首先提出一个问题：如果我们在相机或者雷达坐标系下观测到一个对象，那么这个对象在世界坐标系下或者机器人坐标系下的位置是如何表示的呢？这里就需要用数学公式来表述了，也就是坐标变换的问题：如何计算同一个向量在不同坐标系里的坐标？ 实际上，两个坐标系之间的关系，只有旋转和平移两种，或者说，两个坐标系之间的运动就是一个旋转加上一个平移，这种运动称为刚体运动 刚体运动中的旋转实际上可以通过一个旋转矩阵进行定义，至于旋转矩阵的具体来龙去脉请深入学习机器人学课程，这里我们可以知道，旋转矩阵是由两组基之间的内积组成的，刻画了旋转前后同一个向量的坐标变换关系，也就是只要旋转是一样的，旋转矩阵就是一样的 旋转矩阵有一些很特殊的性质，比如说其行列式为1且为正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵，所以可以将 n 维旋转矩阵的集合定义如下形式 $\(SO(n) = \{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^T = \boldsymbol{I}, \det(\boldsymbol{R}) = 1\}\)$ 其中 $\(SO(n)\)$ 是特殊正交群 由于旋转矩阵为正交矩阵，所以旋转矩阵的逆阵就是其本身的转置，也就是描述了相反方向的旋转 如果考虑坐标系之间的旋转与平移，那么可以定义坐标系1和坐标系2，那么向量 $\(\boldsymbol a\)$ 在两个坐标系下的坐标是 $\(\boldsymbol a_1\)$ 和 $\(\boldsymbol a_2\)$，则存在如下的关系 $\(\boldsymbol{a}_1 = \boldsymbol{R}_{12} \boldsymbol{a}_2 + \boldsymbol{t}\)$ 也存在行列式为-1的旋转矩阵，但是这种矩阵表示的是瑕旋转，即一次旋转加一次反射 但是上面的变换方程并不是一个线性方程，如果进行了两次或多次变换会难以描述和推导，因此引入了齐次坐标和对应的变换矩阵 $\(\begin{bmatrix} \boldsymbol{a}' \\ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^T & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{a} \\ 1 \end{bmatrix} \triangleq \boldsymbol{T} \begin{bmatrix} \boldsymbol{a} \\ 1 \end{bmatrix}\)$ 这是一个数学技巧，其中的四维向量称为齐次坐标，记为 $\(\tilde{\boldsymbol{a}}\)$，并且可以使用一个变换矩阵同时描述旋转和平移，我们将其定义为 $\(\boldsymbol T\)$，这种矩阵又称为特殊欧式群 $\(SE(3) = \left\{ \boldsymbol{T} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\\boldsymbol{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in SO(3), \boldsymbol{t} \in \mathbb{R}^3 \right\}\)$ 并且可以定义反向的变换 $\(\boldsymbol{T}^{-1} = \begin{bmatrix} \boldsymbol{R}^T & -\boldsymbol{R}^T \boldsymbol{t} \\\boldsymbol{0}^T & 1 \end{bmatrix}\)$ 旋转矩阵的左乘/右乘与主动被动变换 实际上前面的旋转是一个“多义词”，比如说谁旋转了、以什么为准旋转，当你跟朋友出去玩，你在原地不动，但是朋友动了，那两个时刻下你在以朋友为准的坐标系下的位置就发生了变换，若你动但是朋友不动，则又是另外的情况，因此需要具体讨论 首先介绍主动变换 (Active Transformation) 与 被动变换 (Passive Transformation) 的概念 - 主动变换：顾名思义，向量主动变化，但是坐标系不变，如机械臂抓取的场景中，桌子为坐标系是不动的，机械臂需要旋转到新的位置来抓取，也就是物体在动 - 被动变换：向量不动，但是坐标系变化，如在相机观测场景中，相机在两个位置对物体进行观测，尽管物体不动，但是观测到的位置也会不同，也就是观测者在动 此外，旋转矩阵的左乘和右乘在数学和物理上有不同的意义，也就是旋转矩阵在向量的哪一侧相乘，这种矩阵乘法的顺序直接决定了旋转是相对于谁发生的，也就是你绕朋友旋转和原地旋转是截然不同的结果 - 左乘：矩阵在左，绕固定的全局坐标系旋转，一般用于地图矫正 - 右乘：矩阵在右，绕局部或者说自身坐标系旋转，一般用于 IMU 积分，因为 IMU 测量的是自身的角速度 因此可以进行列表，其中使用 $\(\boldsymbol R_{curr}\)$ 表示当前姿态，使用 $\(\Delta \boldsymbol R\)$ 表示旋转增量 乘法顺序 变换方式 解读/用途 公式 左乘 绕固定系转动 主动变换

$$$$

被动变换

$\(\boldsymbol R_{new}=\Delta\boldsymbol R \cdot\boldsymbol R_{curr}\)$ 右乘 绕自身系转动 主动变换

$$$$

被动变换

$$$$ 但是这种变换矩阵的方法是有缺陷的： 1. SO(3)的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度的变换。那么，是否有更紧凑的表示呢? 2. 旋转矩阵自身带有约束：它必须是个正交矩阵,且行列式为 1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵,变换矩阵时，这些约束会使得求解变得更困难。 旋转向量与欧拉角 旋转向量——除了旋转矩阵之外的旋转表示 旋转矩阵表示旋转是冗杂的(旋转矩阵有9个量，但一次旋转只有3个自由度并且旋转矩阵自身带有约束)，我们希望有一个紧凑和无约束的形式表示旋转和平移，所以有了新的表示方法——旋转向量，不过要注意一下，旋转向量与旋转矩阵只是表达方式不同，但是表达的内容是相同的 事实上很容易理解，任意一个旋转都可以使用一个旋转轴和旋转角描述，也就是绕该轴旋转了多少角度，因此可以定义一个旋转向量，其方向与旋转轴一致，长度等于旋转角度，也就是我们可以使用一个三维向量就可以表示旋转 那么对于同一个旋转，旋转矩阵形式和旋转向量形式之间有什么联系呢？实际上这种联系就是罗德里格斯公式 $\(\boldsymbol{R} = \cos \theta \boldsymbol{I} + (1 - \cos \theta) \boldsymbol{n} \boldsymbol{n}^T + \sin \theta \boldsymbol{n}^\wedge\)$ 其中的 $\(\theta\)$ 是旋转角度，$\(\boldsymbol n\)$ 是旋转轴方向的单位向量 其中转轴是矩阵 R 特征值1对应的特征向量 实际上的计算推导过程可以看下列视频，大概从22分钟开始讲解 https://www.bilibili.com/video/BV1Wa411L71b?spm_id_from=333.788.player.switch&vd_source=eea47a16439992e41b232bc5d5684e27 当然也可以逆向，通过旋转矩阵计算旋转向量，对于转角 ，可以对两侧取迹 $\(\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &= \cos\theta\operatorname{tr}(\boldsymbol{I}) + (1 - \cos\theta)\operatorname{tr}\left(\boldsymbol{n}\boldsymbol{n}^T\right) + \sin\theta\operatorname{tr}(\boldsymbol{n}^\wedge)\\ &= 3\cos\theta + (1 - \cos\theta)\\ &= 1 + 2\cos\theta \end{aligned}\)$ 因此可以获取旋转角的表达式 $\(\theta = \arccos\frac{\operatorname{tr}(\boldsymbol{R}) - 1}{2}\)$ 关于转轴，易知旋转轴上的向量在旋转后不发生改变，说明转轴是矩阵 R 特征值1对应的特征向量 $\(\boldsymbol R \boldsymbol n = \boldsymbol n\)$ 解此方程并且归一化就得到了旋转轴 欧拉角 无论是旋转矩阵、旋转向量，虽然它们能描述旋转，但对我们人类是非常不直观的。当我们看到一个旋转矩阵或旋转向量时很难想象出来这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是向哪个方向在转动。 而欧拉角提供了一种非常直观的方式，欧拉角将旋转分解为三次不同轴上的转动，以便理解 - 例如按照Z-Y-X转动 - 轴顺序亦可不同，因此存在许多种定义方式不同的欧拉角 - 其中ZYX 顺序（航向-俯仰-滚转）是常用的一种，顺序上首先围绕 z 轴旋转，接着围绕新的 y 轴旋转，最后围绕新的 x 轴旋转。广泛应用于航空航天和机器人学中，用来描述航向（yaw）、俯仰（pitch）和滚转（roll）。 实际上，欧拉角的定义方式比较多（XYZ三轴不同的先后顺序），而且会存在奇异性问题（万向锁这种），所以一般不会直接使用，在SLAM中也很少使用欧拉角表示姿态，因为欧拉角存在不连续和奇异点问题 而万向锁就是欧拉角奇异性问题的一种体现，即旋转角在特定值的时候，会有两个旋转轴重合，这种情况下旋转自由度减一 由于万向锁问题，欧拉角不适合插值和迭代，往往用于人机交互中，并且可以证明，用三个实数来表达三维旋转时，会不可避免地碰到奇异性问题。所以SLAM程序中很少直接用欧拉角表示姿态，或对于某些二维平面运动的场景中，也可以将旋转分解为三个欧拉角，然后将其中一个（尤其是是偏航角）拿出作为定位信息使用 正常情况下 [图片] 奇异情况下 [图片] 在这种时候，两轴重合 但是，如果使用四个数来表达旋转，则不会出现这种情况，这也就是四元数的用处 四元数 这是一种节省空间（紧凑）而且没有奇异性的表达形式，可以用来描述旋转 2D 情况下，可用单位复数表达旋转，三维情况下，四元数就是复数的扩充 四元数（Quaternion）有一个实部和三个虚部，形式如下 $\(\boldsymbol{q} = q_0 + q_1 i + q_2 j + q_3 k = [q_0, \boldsymbol{v}]^T\)$ 并且三个虚部满足以下关系 $\(\begin{cases} \begin{aligned} i^2 &= j^2 = k^2 = ijk = -1 \\ ij &= k, \quad ji = -k \\ jk &= i, \quad kj = -i \\ ki &= j, \quad ik = -j \end{aligned} \end{cases}\)$ 如何使用四元数描述旋转呢？实际上是通过单位四元数实现的，单位四元数表示三维空间中的任意一个旋转，但是我们可以先了解一下四元数的一些运算，定义两个四元数 $\(\begin{aligned} \boldsymbol q_a&=[s_a,\boldsymbol v_a]^T=s_a+x_a i+y_a j+z_a k\\ \boldsymbol q_b&=[s_b,\boldsymbol v_b]^T=s_b+x_b i+y_b j+z_b k\\ \end{aligned}\)$ 1. 加减法：四元数的加减法很简单，对应位置的元素直接加减即可 2. 乘法：乘法是第一个四元数的每一项与第二个四元数的每一项相乘 1. 可以给出直接形式：$\(\begin{aligned} \boldsymbol{q}_a \otimes \boldsymbol{q}_b = &\ (q_{a0}q_{b0} - q_{a1}q_{b1} - q_{a2}q_{b2} - q_{a3}q_{b3}) \\ & + (q_{a0}q_{b1} + q_{a1}q_{b0} + q_{a2}q_{b3} - q_{a3}q_{b2}) i \\ & + (q_{a0}q_{b2} - q_{a1}q_{b3} + q_{a2}q_{b0} + q_{a3}q_{b1}) j \\ & + (q_{a0}q_{b3} + q_{a1}q_{b2} - q_{a2}q_{b1} + q_{a3}q_{b0}) k \end{aligned}\)$ 2. 也可以给出简洁形式：$\(\boldsymbol{q}_a \otimes \boldsymbol{q}_b = \begin{bmatrix} s_a s_b - \boldsymbol{v}_a \cdot \boldsymbol{v}_b \\ s_a \boldsymbol{v}_b + s_b \boldsymbol{v}_a + \boldsymbol{v}_a \times \boldsymbol{v}_b \end{bmatrix}\)$ 3. 乘法具有非交换性，满足结合律和分配律 3. 模长 1. 定义模长为：$\(\|\boldsymbol{q}\| = \sqrt{s^2 + x^2 + y^2 + z^2}\)$ 2. 对于两个四元数的模长有：$\(\|\boldsymbol{q}_a \otimes \boldsymbol{q}_b\| = \|\boldsymbol{q}_a\| \cdot \|\boldsymbol{q}_b\|\)$ 4. 共轭：四元数的共轭就是把虚部取成相反数 1. 定义：$\(\boldsymbol{q}^* = s - x i - y j - z k = [s, -\boldsymbol{v}]^T\)$ 2. 相乘：$\(\boldsymbol{q} \otimes \boldsymbol{q}^* = \boldsymbol{q}^* \otimes \boldsymbol{q}=[s_a^2+\boldsymbol v^T\boldsymbol v,0]^T\)$ 5. 逆 1. 定义：$\(\boldsymbol{q}^{-1} = \frac{\boldsymbol{q}^*}{\|\boldsymbol{q}\|^2}\)$ 2. 四元数和自己的逆的乘积为实四元数 1 那么如何使用四元数描述旋转呢？首先我们有一个旋转向量，那么就可以根据这个旋转向量计算出一个四元数，实际上这与之前的罗德里格斯公式有异曲同工之处 $\(\boldsymbol{q}=\left[ \cos \frac \theta 2,\boldsymbol{n} \sin \frac \theta 2 \right]\)$ 那么这个四元数如何实现旋转计算呢？首先我们有一个三维空间中的点 $\(\boldsymbol p=[x,y,z]^T\in \mathbb R^3\)$，然后定义其旋转之后的点为 $\(\boldsymbol p^\prime\)$，那么我们先使用一个虚四元数描述该点 $\(\boldsymbol p=[0,x,y,z]^T=[0, \boldsymbol v]^T\)$ 相对于把四元数中的三个虚部与空间中的三个轴对应，那么旋转之后的点就可以表示为 $\(\boldsymbol p^\prime= \boldsymbol q \boldsymbol p \boldsymbol q^{-1}\)$ 注意一下，上式实际上是一个四元数乘法，使用结果也是四元数，需要最后将虚部取出，然后才可以得到旋转之后的点坐标 程序设计 使用动态矩阵的时候，运算会比较慢 李群和李代数 背景 在SLAM中，除了表示之外，还要对它们进行估计和优化，因为SLAM整个过程就是在不断地估计机器人的位姿与地图，该位姿是由旋转矩阵或变换矩阵描述的。为了优化位姿，需要对变换矩阵进行插值、求导、迭代等操作，比如说当我们去估计相机位姿的时候，当估计不准确的时候，要对旋转和平移进行微调。 设某个时刻机器人的位姿为 $\(\boldsymbol{T}_{cw}\)$，它观察到了一个世界坐标位于 $\(\boldsymbol{P}_w\)$ 的点，产生了一个观测数据 $\(\boldsymbol{Z}_c\)$，根据坐标变换有 $$ \boldsymbol{Z}c = \boldsymbol{T}$$ 那我们实际要做的事情是求一个欧氏变换 $} \boldsymbol{P}_{w} + \boldsymbol{w\(\boldsymbol{T}_{cw}\)$，使得 $\(\boldsymbol{T}_{cw}\)$ 满足上式。 然而，由于观测噪声 $\(\boldsymbol{w}\)$ 的存在，$\(\boldsymbol{z}\)$ 往往不可能精确地满足 $\(\boldsymbol{z} = \boldsymbol{T}\boldsymbol{p}\)$ 的关系。所以，我们通常会计算理想的观测与实际数据的误差：$\(\boldsymbol{e} = \boldsymbol{z} - \boldsymbol{T}\boldsymbol{p}\)$ 假设一共有 $\(N\)$ 个这样的路标点和观测，则就有 $\(N\)$ 个上式，则对于机器人的位姿估计，相当于寻找一个最优的 $\(\boldsymbol{T}\)$，使得整体误差最小化： $\(\min_{\boldsymbol{T}} J(\boldsymbol{T}) = \sum_{i=1}^{N} \|\boldsymbol{z}_i - \boldsymbol{T}\boldsymbol{p}_i\|_2^2\)$ 计算最优就需要求导，求导就需要进行加减，但是由于其性质，我们无法完成这个求导操作，自然无法完成优化，所以我们需要用一种新理论去完成这个操作 代数基础 之前的章节介绍了旋转矩阵和变换矩阵的定义。当时，我们说三维旋转矩阵构成了特殊正交群 $\(SO(3)\)$，而变换矩阵构成了特殊欧氏群 $\(SE(3)\)$。它们写起来像这样： $$SO(3) = { \boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I}, \det(\boldsymbol{R}) = 1 }\

SE(3) = \left{ \boldsymbol{T} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\boldsymbol{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in SO(3), \boldsymbol{t} \in \mathbb{R}^3 \right}$$ 不过，当时我们并未详细解释群的含义。细心的读者应该会注意到，旋转矩阵也好，变换矩阵也好，它们对加法是不封闭的。换句话说，对于任意两个旋转矩阵 $\(\boldsymbol{R}_1\)\(，\)\(\boldsymbol{R}_2\)$，按照矩阵加法的定义，和不再是一个旋转矩阵： $\(\boldsymbol{R}_1 + \boldsymbol{R}_2 \notin SO(3), \quad \boldsymbol{T}_1 + \boldsymbol{T}_2 \notin SE(3)\)$ 你也可以说两种矩阵并没有良好定义的加法，或者通常矩阵加法对这两个集合不封闭。相对地，它们只有一种较好的运算：乘法。$\(SO(3)\)$ 和 $\(SE(3)\)$ 关于乘法是封闭的： $\(\boldsymbol{R}_1 \boldsymbol{R}_2 \in SO(3), \quad \boldsymbol{T}_1 \boldsymbol{T}_2 \in SE(3)\)$ 同时我们也可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。我们知道，乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好的）运算的集合，我们称之为群 那么如何理解这种概念呢？回想一下线性代数中的线性空间或者向量空间的概念，线性空间的定义就是满足若干公理的向量的集合，如加法、数乘、交换律、封闭性等，其定义了一个平整光滑的空间，不能弯曲、闭合和存在边界，就如同一张无限大的纸 那么如果砍去其中的一些性质，如砍掉数乘，但是仍然满足交换律等，那么就构成了一个阿贝尔群，也就是其中的公理只涉及向量集合内部的元素，不涉及外部的标量；如果继续砍去一些性质要求，就构成了李群，可以理解为李群是弱约束下的线性空间，线性空间是强约束下的李群 那么为什么要如此定义呢？因为线性空间必须可以数乘，因此必须平直，但是李群并没有那么多要求，只需要满足互操作即可，空间就可以是弯曲和封闭（比如说首尾相连），如旋转群 $\(SO(3)\)$ 就像一个球体表面。你在球面上走（旋转），没法定义“把这个旋转放大 2.5 倍”而不离开球面（数乘失效），但你可以定义“先转 A 再转 B”（群乘法有效） 群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 $\(A\)$，运算记作 $\(\cdot\)$，那么群可以记作 $\(G = (A, \cdot)\)\(。群要求这个运算满足以下几个条件或者说公理： 1. 封闭性：\)\(\forall a_1, a_2 \in A, \quad a_1 \cdot a_2 \in A\)$ 2. 结合律：$\(\forall a_1, a_2, a_3 \in A, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3)\)$ 3. 幺元（也是单位元）：$\(\exists a_0 \in A, \quad \text{s.t.} \quad \forall a \in A, \quad a_0 \cdot a = a \cdot a_0 = a\)$ 4. 逆：$\(\forall a \in A, \quad \exists a^{-1} \in A, \quad \text{s.t.} \quad a \cdot a^{-1} = a_0\)$ 群结构保证了在群上的运算具有良好的性质 对于旋转矩阵和变换矩阵群，上面的性质都很容易证明与理解： 1. 旋转矩阵与旋转矩阵的乘积仍然是旋转矩阵 2. 旋转矩阵的连续乘法满足结合律 3. 单位矩阵即为幺元，也就是旋转角度为零 4. 旋转矩阵存在逆阵，表示反向旋转 上述性质对于变换矩阵同样适用 李群概念 几何理解 李群是具有连续性质的群，或者说这个群是光滑可微的（可以想象成没有尖刺和棱角的封闭几何体的表面），所以既是群也是流形（Manifold），直观上看，一个刚体能够连续地在空间中运动，也就有连续的位姿，相应的旋转矩阵和变换矩阵也是连续的，故 $\(SO(3)\)$ 和 $\(SE(3)\)$ 都是李群 所有李群都是流形，但并非所有流形都是李群。李理论的基本现象是，人们可以以一种自然的方式将李群 $\(\mathcal G\)$ 与李代数 $\(\mathfrak g\)$ 联系起来。李代数 $\(\mathfrak g\)$ 首先是一个向量空间，其次被赋予了一个双线性非结合乘积，称为李方括号 $\([\cdot,\cdot]\)$。令人惊讶的是，群 $\(\mathcal G\)$ 几乎完全由李代数 $\(\mathfrak g\)$ 和它的李括号决定。因此，处于许多目的，我们可以用李代数 $\(\mathfrak g\)$ 代替李群 $\(\mathcal G\)$ 。由于李群 $\(\mathcal G\)$ 是一个复杂的非线性对象，而 $\(\mathfrak g\)$ 只是一个向量空间，所以使用 $\(\mathfrak g\)$ 和李括号通常要简单得多，这是李理论力量的来源之一 具体来说，流形可以被定义为一个空间（可以想成一个曲面），因为流形是光滑的，所以它在每个点处都有且只有一个“切空间（切线或者切平面）”，切空间是一个局部欧几里得空间或者说线性向量空间，其维度等于流形的自由度，它可以用欧几里得几何的方法来描述，然后我们可以使用切空间的一些性质来近似表示局部曲面的性质（类似于函数可以使用若干阶导数的多项式近似表示，甚至二者之间可以形成一个双射关系)，这种性质可以用来解决位姿求导和状态估计的问题 1. 概率分布的定义：高斯分布（Gaussian Distribution）定义在向量空间上。我们无法在球面上直接定义标准高斯分布，但可以在切平面上定义，这代表了围绕某一名义状态的不确定性。这一点可用于预积分、里程计等 2. 微积分的运算：导数和积分本质上是线性的极限操作，它们在弯曲空间难以直接定义，但在切空间中却轻而易举。 下图展示了李群和李代数之间的关系，李群流形 $\(\mathcal{M}\)$ 是三维空间中的蓝色球面，李代数 $\(T_{\mathcal{X}}\mathcal{M}\)$ 是红色平面所表示的切空间，切点位于 $\(\mathcal{E}\)$，通过指数映射，经过李代数切空间原点的每条直线 $\(\boldsymbol vt\)$ 产生了一条围绕流形的路径 $\(\exp(\boldsymbol vt)\)$ ，它沿着各自的测地线（geodesic）进行移动。相反地，群中的每个元素在李代数中都有一个等价的元素。这个关系是如此深刻，以至于（几乎）群中的所有操作，它是弯曲的和非线性的，在李代数中有一个精确的等价性，它是一个线性的向量空间。虽然三维空间中的球体不是一个李群（我们只是用它作为一个可以在纸上绘制的表示），但四维欧式空间中的球体是一个李群，一个单位四元数的群 [图片] 三维球面为二维的流形，因为可由一群二维的平面图形来叠加（广义加法）表示 [图片] [图片]

如图所示的地球球面就是一个2维流形。因此，对于球面上的一个曲面三角形，可以摊开展成（即流动变形成）一个2维欧几里得空间上的平面三角形。此外，因为地球实在太大，我们往往把地球上的一块足够小的（曲面）局部区域当作平面来丈量，而不用担心会引起大的误差。比如，你要丈量学校操场的面积，根本不用把它认为是地球上的一块曲面，而直接看作一块平面即可。所以，光滑流形其足够小的结构是“硬”的（如可以固定丈量），而整体结构则是“柔软”的（可流动变形）。也就是我们可以使用一个平面来表示局部的曲面，并且带来计算上的方便，因为很多计算在曲面上是难以实现或者完全无法实现的。 也就是这种方法可以在局部欧式空间中的使用常规方法，因此不需要考虑复杂的全局拓扑结构，也能够精确估计出高维空间中物体的运动状态。 流形（Manifold）可看作是很多曲面片的叠加（这些曲面在大的尺度上即为平面，叠加即为广义加法）。这些平面可以看成位姿增量，将平面指数映射成曲面，而曲面的叠加即构成李群，也就是地球的球面一个三维空间中的二维的流形。 [图片] 我们所能观察到的数据（r）实际上是由一个低维流形映射到高维空间上的，即这些数据所在的空间是“嵌入在高维空间的低维流形。这个 r 是迭代卡尔曼每次迭代出的位姿增量，即李代数，也是欧几里得空间中的平面，只有李代数才满足广义加法。从整体观察：流形即为李群，从局部观察：流形近似为欧式空间。 直观样例 单位复数群 第一个李群的例子是复乘法下的单位复数群，这是最容易可视化的。单位复数的形式为：$\(\boldsymbol z=\cos \theta +i\sin\theta\)$ 1. Action 动作：向量 $\(\boldsymbol x\)$ 在平面中旋转角度 $\(\theta\)\(，通过复数乘法，\)\(\boldsymbol x '=\boldsymbol z \boldsymbol x\)$ 2. Group facts 群的事实：单位复数的乘积是一个单位复数，幺元为1，且逆为共轭 $\(\boldsymbol z^*\)$ 。 3. Manifold facts 流形的事实：单位范数约束定义了在复平面内的单位圆（它可以看作是1维球 1-sphere，命名为 $\(\boldsymbol S^1\)$，如下图中的蓝色圆形所示），这是一条在2维空间中自由度为1的曲线，也是一个流形。单位复数在这个圆上随时间演化。群（圆）局部调整线性空间（切线），而不是全局。 [图片] 流形 $\(\boldsymbol S^1\)$ 是复平面 $\(\mathbb C\)$ 中的单位圆（蓝色），其中单位复数始终满足 $\(\mathbf{z^*z}=1\)$。李代数 $$\mathfrak s^1=T_{\mathcal E} S^1 $$ 是虚部 $\(i\mathbb R\)$（红色）的线条，且任意切空间 $\(TS^1\)$ 是与（红色）线 $\(\mathbb R\)$ 同构的（isomorphic）。切向量（深红色片段）缠绕贴合到流形上得到圆弧（蓝色弧线）。两种映射 exp 和 log（黑色箭头）将虚部 $\(i\mathbb R\)$ 的元素 缠绕wrap 或 掰直unwrap为流形 $\(\boldsymbol S^1\)$ 中的元素（蓝色弧线）。单位复数之间的增量（increment）通过合成和指数映射在切线空间中表示（为此，我们将定义特殊的运算符 $\(\oplus\)$ ） 单位四元数群 李群的第二个例子是在四元数乘法组合运算背景下单位四元数群，它也是相当容易可视化理解的。单位四元数的形式为：$$\mathbf q=\cos(\theta/2)+\mathbf u\sin(\theta/2) $$ ，其中 $\(\mathbf u=iu_x+ju_y+ku_z\)$ 是一个单位旋转轴，$\(\theta\)$ 是旋转角度。 - Action 动作：向量 x=ix+jy+kz\mathbf x=ix+jy+kz 在三维空间中通过两次四元数乘法 x′=qxq∗\mathbf {x'=qxq^} 绕单位轴 u\mathbf u 旋转 θ\theta 角。 - Group facts 群的事实：单位四元数的乘积是仍是一个单位四元数，幺元为1，逆位共轭四元数q∗\mathbf q^。 - Manifold facts 流形的事实：单位范数约束定义了一个三维球体 S3S^3 ，四维空间中的一个球形三维曲面或者三维流形。单位四元数在这个曲面上随着时间变化。群（球体）局部重构了线性空间（切超平面 R3⊂R4\mathbb R^3\subset\mathbb R^4 ），但不是全局的。 [图片] 如下图4所示。 S3S^3 流形是在四元数的四维空间中的一个单位三维球体（unit 3-sphere）（蓝色），其中始终保持着 q∗q=1\mathbf{q^* q}=1 。李代数是纯虚四元数 ix+jy+kz∈Hix+jy+kz\in\mathbb H 所在的空间，同构于超平面 R3\mathbb R^3 （红色网格），任何其它切线空间 TS3TS^3 也与 R3\mathbb R^3 同构。切向量 （深红色线段）贴着优弧（great arc）或者测地线（geodesic）（蓝色虚线）缠绕（wrap）到流形上。中间和右边两图显示了经过这条测地线的侧视图（注意看它如何类似于图3中的流形 S1S^1 ）。带箭头的黑线表示的两种映射运算 exp\exp 和 log\log 将 Hp\mathbb H_p 中的元素映射 到/自 S3S^3 中的元素（深蓝色弧线）。四元数之间的增量通过运算 ⊕,⊖\oplus,\ominus 在切空间中进行表示。 李代数概念 李代数是与李群对应的一种结构，位于向量空间，对应李群的正切空间，描述了李群局部的导数，记作 $\(\mathfrak{so}(3)\)$ 和 $\(\mathfrak{se}(3)\)$ 从旋转矩阵可以引出李代数，我们考虑任意旋转矩阵 $\(\boldsymbol{R}\)$，满足 $\(\boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I}\)$ 在连续运动过程中，显然 $\(\boldsymbol{R}\)$ 是连续时间的函数，我们记为 $\(\boldsymbol{R}(t)\boldsymbol{R}(t)^T = \boldsymbol{I}\)$ 两侧对时间求导 $\(\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T + \boldsymbol{R}(t)\dot{\boldsymbol{R}}(t)^T = 0 \\\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T = -(\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T)^T\)$ 如果我们将 $\(\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T\)$ 看做一个整体，我们就发现其为一个反对称矩阵，三维的反对称矩阵与三维向量是一一对应的，因此可以引入反对称符号来表示 $\(\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T = \boldsymbol{\phi}(t)^\wedge\)$ 两边右乘 $\(\boldsymbol{R}(t)\)$ 可得 $\(\dot{\boldsymbol{R}}(t)\boldsymbol{R}(t)^T \boldsymbol{R}(t) = \boldsymbol{\phi}(t)^\wedge \boldsymbol{R}(t)\)$ 其中 $\(\boldsymbol{R}(t)^T \boldsymbol{R}(t) = \boldsymbol{I}\)$，消去后得到 $\(\dot{\boldsymbol{R}}(t) = \boldsymbol{\phi}(t)^\wedge \boldsymbol{R}(t)\)$ 可以看成求导之后，左侧多出一个 $\(\boldsymbol{\phi}(t)^\wedge\)$，或者说，每对旋转矩阵求导一次，只需要左乘一个此矩阵（当然此矩阵不是一个常数），这类似乎指数函数的操作，变量的导数等于其本身乘以一个系数 $\(y = e^{kx} \rightarrow y' = ke^{kx} \rightarrow y' = ky\)$ 从简单情况考虑，当 $\(t_0 = 0\)$, $\(\boldsymbol{R}(0) = \boldsymbol{I}\)$ 的时候 $\(\begin{aligned} \boldsymbol{R}(t) &\approx \boldsymbol{R}(t_0) + \dot{\boldsymbol{R}}(t_0)(t - t_0) \\ &= \boldsymbol{I} + \boldsymbol{\phi}(t_0)^\wedge(t) \end{aligned}\)$ 在这里，$\(\phi^{\wedge}\)$ 为 $\(R(t)\)$ 的李代数，是李群在单位元 $\(t_0\)$ 处的正切空间 在 $\(t_0\)$ 附近，设 $\(\phi\)$ 保持为常数向量 $\(\phi(t_0) = \phi_0\)$，则有微分方程 $\(\dot{R}(t) = \phi(t_0)^{\wedge} R(t) = \phi_0^{\wedge} R(t)\)$ 已知初始情况，解得 $\(R(t) = \exp(\phi_0^{\wedge} t)\)$ $\(R(t)\)$ 与 $\(\phi\)$ 之间的关系称为指数映射，这里的 $\(\phi\)$ 称为 $\(SO(3)\)$ 对应的李代数：$\(\mathfrak{so}(3)\)$ 但是新的问题来了，$\(\mathfrak{so}(3)\)$ 的定义和性质是什么呢？这个指数映射应该怎么求呢 实际上每个李群都有与之对应的李代数，李代数描述了李群单位元数的正切空间性质。 李代数由一个集合 $\(\mathbb V\)$，一个数域 $\(\mathbb{F}\)$ 和一个二元运算 $\([,]\)$ 组成。如果它们满足以下几条性质，称 $\((\mathbb V,\mathbb{F},[,])\)$ 为一个李代数，记作 $\(\mathfrak{g}\)$ 1. 封闭性：$\(\forall X,Y \in \mathbb V,[X,Y] \in \mathbb V\)$ 2. 双线性：$\(\forall X,Y,Z \in \mathbb V,a,b \in \mathbb{F}\)\(，有\)\([aX + bY,Z] = a[X,Z] + b[Y,Z], [Z,aX + bY] = a[Z,X] + b[Z,Y]\)$ 3. 自反性：$\(\forall X \in \mathbb V,[X,X] = 0\)$ 4. 雅可比等价：$$\forall X,Y,Z \in \mathbb V,[X,[Y,Z]] + [Y,[Z,X]] + [Z,[X,Y]] = 0 $$ 二元运算被称为李括号，例子：三维空间向量加叉积运算构成李代数，当然，实际上我们不需要去记忆这些性质 对于李群 $\(SO(3)\)$，有李代数 $\(\mathfrak{so}(3)\)$，实际上该李代数就是定义在三维空间上的向量或三维反对称矩阵，只不过向量形式更加自然，且可以用于表达旋转矩阵的导数 $\(\mathfrak{so}(3) = \{\phi \in \mathbb{R}^3, \Phi = \phi^{\wedge} \in \mathbb{R}^{3 \times 3}\}\\[0.5em] \Phi = \phi^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\\phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3}\)$ 在此定义下，两个向量的李括号为 $\([\phi_1, \phi_2] = (\Phi_1 \Phi_2 - \Phi_2 \Phi_1)^{\vee}\)$ 从物理角度理解，李代数就是旋转向量，李括号是两个角速度向量的叉积，它度量了两个无穷小旋转在交换顺序时产生的净旋转误差 / 额外角速度 / 耦合效应，而具体的推导会在后面展开。 对于 $\(SE(3)\)$，它也有对应的李代数 $\(\mathfrak{se}(3)\)$。为节省篇幅，这里就不介绍如何引出 $\(\mathfrak{se}(3)\)$ 了。与 $\(\mathfrak{so}(3)\)$ 相似，$\(\mathfrak{se}(3)\)$ 位于 $\(\mathbb{R}^6\)$ 空间中： $\(\mathfrak{se}(3) = \left\{ \boldsymbol\xi = \left[ \begin{array}{c} \boldsymbol\rho \\ \boldsymbol\phi\end{array} \right] \in \mathbb{R}^6, \quad \boldsymbol\rho \in \mathbb{R}^3, \boldsymbol\phi \in \mathfrak{so}(3), \quad \boldsymbol\xi^{\wedge} = \left[ \begin{array}{cc} \boldsymbol\phi^{\wedge} & \boldsymbol\rho \\ 0^T & 0 \end{array} \right] \in \mathbb{R}^{4 \times 4} \right\}\)$

我们把每个 $\(\mathfrak{se}(3)\)$ 元素记作 $\(\boldsymbol\xi\)$，它是一个六维向量。前三维为平移（但含义与变换矩阵中的平移不同，分析见后），记作 $\(\boldsymbol\rho\)$；后三维为旋转，记作 $\(\boldsymbol\phi\)$，实质上是 $\(\mathfrak{so}(3)\)$ 元素。同时，我们拓展了符号的含义。在 $\(\mathfrak{se}(3)\)$ 中，同样使用 $\(\wedge\)$ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称： $\(\boldsymbol\xi^{\wedge} = \left[ \begin{array}{cc} \boldsymbol\phi^{\wedge} & \boldsymbol\rho \\ 0^T & 0 \end{array} \right] \in \mathbb{R}^{4 \times 4}\)$ 我们仍使用 $\(\wedge\)$ 和 $\(\vee\)$ 符号来指代"从向量到矩阵"和"从矩阵到向量"的关系，以保持和 $\(\mathfrak{so}(3)\)$ 上的一致性。它们依旧是一一对应的。读者可以简单地把 $\(\mathfrak{se}(3)\)$ 理解成"由一个平移加上一个 $\(\mathfrak{so}(3)\)$ 元素构成的向量"（尽管这里的 $\(\boldsymbol\rho\)$ 还不直接是平移）。同样，李代数 $\(\mathfrak{se}(3)\)$ 亦有类似于 $\(\mathfrak{so}(3)\)$ 的李括号： $\([\boldsymbol\xi_1, \boldsymbol\xi_2] = (\boldsymbol\xi_1^{\wedge} \boldsymbol\xi_2^{\wedge} - \boldsymbol\xi_2^{\wedge} \boldsymbol\xi_1^{\wedge})^{\vee}\)$ 指数映射 指数映射反映了从李代数到李群的对应关系，并且任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵 $\(\exp(\boldsymbol A) = \sum_{n=0}^{\infty} \frac{1}{n!} \boldsymbol A^n\)$ 同样地，对 $\(\mathfrak{so}(3)\)$ 中任意元素 $\(\phi\)$，我们亦可按此方式定义它的指数映射 $\(\exp(\phi^{\wedge}) = \sum_{n=0}^{\infty} \frac{1}{n!} (\phi^{\wedge})^n\)$ 但这个定义没法直接计算，因为我们不想计算矩阵的无穷次幂。下面我们推导一种计算指数映射的简便方法。由于 $\(\phi\)$ 是三维向量，我们可以定义它的模长和它的方向，分别记作 $\(\theta\)$ 和 $\(\boldsymbol a\)$，于是有 $\(\phi = \theta \boldsymbol a\)$。这里 $\(\boldsymbol a\)$ 是一个长度为1的方向向量，即 $\(\|\boldsymbol a\| = 1\)\(。首先，对于\)\(\boldsymbol a^{\wedge}\)$，有以下两条性质： $\(\boldsymbol a^{\wedge} \boldsymbol a^{\wedge} = \begin{bmatrix} -a_2^2 - a_3^2 & a_1 a_2 & a_1 a_3 \\ a_1 a_2 & -a_1^2 - a_3^2 & a_2 a_3 \\ a_1 a_3 & a_2 a_3 & -a_1^2 - a_2^2 \end{bmatrix} = \boldsymbol a \boldsymbol a^{\top} - \boldsymbol I \\[0.5em] \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} = \boldsymbol{a}^{\wedge} (\boldsymbol{a} \boldsymbol{a}^{\top} - \boldsymbol{I}) = -\boldsymbol{a}^{\wedge}\)$ 这两个式子提供了处理 $\(\boldsymbol{a}^{\wedge}\)$ 高阶项的方法。我们可以把指数映射写成： $\(\begin{aligned}\exp (\boldsymbol{\phi}^{\wedge}) &= \exp (\theta \boldsymbol{a}^{\wedge}) = \sum_{n=0}^{\infty} \frac{1}{n!} (\theta \boldsymbol{a}^{\wedge})^n \\&= \boldsymbol{I} + \theta \boldsymbol{a}^{\wedge} + \frac{1}{2!} \theta^2 \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} + \frac{1}{3!} \theta^3 \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} + \frac{1}{4!} \theta^4 (\boldsymbol{a}^{\wedge})^4 + \cdots \\&= \boldsymbol{a} \boldsymbol{a}^{\top} - \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} + \theta \boldsymbol{a}^{\wedge} + \frac{1}{2!} \theta^2 \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} - \frac{1}{3!} \theta^3 \boldsymbol{a}^{\wedge} - \frac{1}{4!} \theta^4 (\boldsymbol{a}^{\wedge})^2 + \cdots \\&= \boldsymbol{a} \boldsymbol{a}^{\top} + \underbrace{\left( \theta - \frac{1}{3!} \theta^3 + \frac{1}{5!} \theta^5 - \cdots \right)}_{\sin\theta} \boldsymbol{a}^{\wedge} - \underbrace{\left( 1 - \frac{1}{2!} \theta^2 + \frac{1}{4!} \theta^4 - \cdots \right)}_{\cos\theta} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\&= \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} + \boldsymbol{I} + \sin \theta \boldsymbol{a}^{\wedge} - \cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\&= (1 - \cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} + \boldsymbol{I} + \sin \theta \boldsymbol{a}^{\wedge} \\&= \cos \theta \boldsymbol{I} + (1 - \cos \theta) \boldsymbol{a} \boldsymbol{a}^{\top} + \sin \theta \boldsymbol{a}^{\wedge}. \end{aligned}\)$ 实际上这是一个似曾相识的结果——罗德里格斯公式 1. $\(\mathfrak{so}(3)\)$ 的物理意义就是旋转向量，即 $\(\mathfrak{so}(3)\)$ 的李代数空间就是由旋转向量组成的线性空间。 2. 如果李群(旋转矩阵，$\(\boldsymbol R(t)\)$，类似一个函数)代表一个球面，那么球上所有点的切线(单位元处李群的切空间李代数，旋转向量)，也会组成一个球面，而且这个球面和原来的球面一样。 我们可以使用下图来可视化的理解李群和李代数的关系 [图片] 其中的指数映射就是李代数向量到旋转矩阵的映射，通过罗德里格斯公式完成旋转矩阵计算，对数映射就是从旋转矩阵到李代数的映射，通过逆向求解罗德里格斯公式，也就是通过求迹和解特征方程的方法解出，而不必专门计算泰勒展开，其中定义对数映射如下 $\(\boldsymbol{\phi} = \ln\left(\boldsymbol{R}\right)^\vee = \left(\sum_{n=0}^\infty \frac{(-1)^n}{n+1} (\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^\vee\)$ 现在，我们介绍了指数映射的计算方法，那么指数映射性质如何呢？是否对于任意的 $\(\boldsymbol{R}\)$ 都能找到一个唯一的 $\(\boldsymbol{\phi}\)$？很遗憾，指数映射只是一个满射，并不是单射。这意味着每个 $\(SO(3)\)$ 中的元素，都可以找到一个 $\(\mathfrak{so}(3)\)$ 元素与之对应；但是可能存在多个 $\(\mathfrak{so}(3)\)$ 中的元素，对应到同一个 $\(SO(3)\)$。至少对于旋转角 $\(\theta\)$，我们知道多转 $\(360^\circ\)$ 和没有转是一样的——它具有周期性。但是，如果我们把旋转角度固定在 $\(\pm\pi\)$ 之间，那么李群和李代数元素是一一对应的，矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。 李群与李代数 需要注意的是，我们经常会构建与位姿有关的函数然后讨论该函数对于位姿的导数，从而调整当前的估计值，但是基于旋转矩阵的方法是无法计算导数的，所以使用李群和李代数的方法进行位姿导数的计算 使用李代数解决求导问题的思路分为两种 1. 用李代数表示姿态，然后根据李代数加法来对李代数求导 2. 对李群左乘或右乘微小扰动，然后对该扰动求导 李代数上的求导与扰动模型 使用李代数的一大动机是进行优化，而在优化过程中导数是非常必要的信息。下面来考虑一个问题。虽然我们已经清楚了 $\(SO(3)\)$ 和 $\(SE(3)\)$ 上的李群与李代数关系，但是，当在 $\(SO(3)\)$ 中完成两个矩阵乘法时，李代数中 $\(\mathfrak{so}(3)\)$ 上发生了什么改变呢？反过来说，当 $\(\mathfrak{so}(3)\)$ 上做两个李代数的加法时，$\(SO(3)\)$ 上是否对应着两个矩阵的乘积？如果成立，相当于： $\(\exp(\boldsymbol{\phi}_1^{\wedge})\exp(\boldsymbol{\phi}_2^{\wedge})=\exp((\boldsymbol{\phi}_1+\boldsymbol{\phi}_2)^{\wedge})\)$ 如果 $\(\boldsymbol{\phi}_1,\boldsymbol{\phi}_2\)$ 为标量，那显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立： $\(\ln(\exp(\boldsymbol{A})\exp(\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}\)$ 很遗憾，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式（BCH 公式）给出。由于其完整形式较复杂，我们只给出其展开式的前几项： $\(\ln(\exp(\boldsymbol{A})\exp(\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}+\frac{1}{2}[\boldsymbol{A},\boldsymbol{B}]+\frac{1}{12}[\boldsymbol{A},[\boldsymbol{A},\boldsymbol{B}]]-\frac{1}{12}[\boldsymbol{B},[\boldsymbol{A},\boldsymbol{B}]]+\cdots\)$ 其中 $\([\ ,\ ]\)$ 为李括号。BCH 公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑 $\(SO(3)\)$ 上的李代数 $\(\ln(\exp(\boldsymbol{\phi}_1^{\wedge})\exp(\boldsymbol{\phi}_2^{\wedge}))^{\vee}\)$，当 $\(\boldsymbol{\phi}_1\)$ 或 $\(\boldsymbol{\phi}_2\)$ 为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达： $\(\ln(\exp(\boldsymbol{\phi}_1^{\wedge})\exp(\boldsymbol{\phi}_2^{\wedge}))^{\vee}\approx\begin{cases} \boldsymbol{J}_l(\boldsymbol{\phi}_2)^{-1}\boldsymbol{\phi}_1+\boldsymbol{\phi}_2 & \text{当 }\boldsymbol{\phi}_1\text{ 为小量}, \\\boldsymbol{J}_r(\boldsymbol{\phi}_1)^{-1}\boldsymbol{\phi}_2+\boldsymbol{\phi}_1 & \text{当 }\boldsymbol{\phi}_2\text{ 为小量}. \end{cases}\)$ 1. 对李群左乘或者右乘微小扰动，然后对这个扰动求导，即把增量的扰动直接添加在李群上，然后利用李代数表示此扰动。 2. 把增量直接定义在李群上需要注意：传统上我们通常用加法表示增量，而李群对加法不封闭。所以这里的增量不再用加法表示，而是乘法。 3. 乘法：增量指的是，在原来的基础上改变一点点。当对旋转矩阵做乘法，乘以的是一个趋近于单位矩阵，也就是差不多没旋转，那这样就是对其“加了一个小量 4. 单位矩阵的李代数为0。 5. 与导数模型相比，省去了一个雅可比的计算，更为实用。 应用 因为李代数是线性的，而李群则是非线性的，而二者之间还有一一对应的关系，所以我们可以使用李代数来进行插值等操作，比如说两个时刻我们获取了两个位姿或者说变换矩阵，那么如何求得两个时刻之间任意时刻的位姿呢？这里就可以基于前一时刻，以前一时刻的位置为原点，计算后一时刻相对的位姿变换矩阵，然后将此矩阵变为李代数，在得到线性的李代数之后，就可以计算出中间任意时刻的对应的李代数，进而求得中间任意时刻的李群也即位姿 Sophus库 Sophus库是一个专门处理李群和李代数的C++库，安装的时候需要注意一下 git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout a621ff cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local cmake --build build -j cmake --install build 编译安装完成之后，在CMakeLists中，可以使用如下命令导入Sophus库 find_package(Sophus REQUIRED) 然后就可以在CPP中使用此库了，下面是一段代码

include

include"sophus/so3.h"

include"sophus/se3.h"

//导入库，Eigen和Sophus Eigen::Matrix3d R=Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0.1.0)).toRotationMatrix();//沿Y轴转45度的旋转矩阵

//Sophus的构造方式 Sophus::SO3 rot_r(R);//从旋转矩阵构造 Sophus::SO3 SO3_v(0,M_PI/4,0);// 从旋转向量构造 Eigen::Quaterniond q(R);//从四元数构造 Sophus::SO3 SO3_q(q); //当使用cout输出的时候，就会以李代数形式输出，或者可以使用.matrix()方法输出矩阵 相机模型 观测，也就是机器人如何观测外部世界，如果使用激光雷达观测或者使用相机观测，也就构成了激光slam或者视觉slam 缺失距离维度的照片 照片记录了真实世界在成像平面上的投影，这个过程丢弃了“距离”维度上的信息，就比如说下面这个照片，实际上两个人是一样大小，但是照片中仿佛棕色衣服的是巨人一样 [图片] 小孔成像模型 这里我们需要稍微暂停一下，定义几个常用坐标系

• 世界坐标系：代表物体在真实世界的三维坐标 $\((X_w, Y_w, Z_w)\)$，实际上是一种全局坐标系
• 相机坐标系：以相机光学中心 $\(O\)$ 为原点的坐标系，Z轴与光轴重合 $\((X_c, Y_c, Z_c)\)$，正方向朝外
• 图像坐标系：代表相机拍摄的图像的坐标系，原点为相机光轴与成像平面的交点 $\((x, y)\)$
• 像素坐标系：在图像的平面上，基本单位是像素，原点一般在相片左上角 $\((u, v)\)$ [图片] 考虑到小孔成像本身为倒像，而实际我们实际拿到的相片都是正向的，因此通常采用等价形式，将小孔模型的成像平面前移 我们来用数学的方法描述一遍相机的成像过程，给定一个世界坐标点 $\((X_w, Y_w, Z_w)\)$，得到其最终像素平面的坐标 $\((u, v)\)$ 首先是世界坐标系到相机坐标系，通过旋转和平移矩阵 $\(R, T\)$ 将点进行变换 $\(\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = R \begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} + T \Rightarrow TP_w\)$ 也可以采用齐次坐标的形式 $\(\begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} \Rightarrow TP_w\)$ 齐次坐标（homogeneous coordinates）是射影几何常用的一种表示形式，简单来说其采用增加一个维度的方式来描述当前点，如常见的2D/3D 点最后维度补1，实际使用时保证该值为1（比如除以该值）。其可以非常方便的描述射影几何的一些特殊情况，如无穷远点（最后一位补0）等，有兴趣可以参考《多视图几何》。这里我们使用该方式以方便后续的矩阵运算，如从相机坐标变换至世界坐标等。 我们来用数学的方法描述一遍相机的成像过程，给定一个世界坐标点 $\((X_w, Y_w, Z_w)\)$，得到其最终像素平面的坐标 $\((u, v)\)$ 之后我们采用投影公式进行投影 $\(\begin{cases} x = \frac{f}{Z_c} X_c \\ y = \frac{f}{Z_c} Y_c \end{cases}\)$ 也可以使用矩阵的形式表示 $\(\begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{f}{Z_c} & 0 & 0 & 0 \\ 0 & \frac{f}{Z_c} & 0 & 0 \\ 0 & 0 & \frac{1}{Z_c} & 0 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix} \Rightarrow K'P_c\)$ 这里我们损失了距离信息。 再就是图像坐标系到像素坐标系，图像坐标系和像素坐标系存在一个比例关系，设图像x方向每毫米有α个像素，y方向每毫米有β个像素，也就是放缩和偏移，则有： $\(\begin{cases} u = c_x + x \cdot \alpha \\ v = c_y + y \cdot \beta \end{cases}\)$ 矩阵形式为 $\(\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} \alpha & 0 & c_x \\ 0 & \beta & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \Rightarrow K''Pxy\)$ 其中 $\(c_x, c_y\)$ 为成像中心在像素坐标中的位置。 将上述公式统一，有 $\(Puv = K''K'TP_w \Rightarrow sKTP_w\)$ 其中 $\(s = \frac{1}{Z_c}; K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\\[0.5em] f_x = \alpha f; f_y = \beta f\)$ 如果相机的成像是 早期的相机有可能会存在像素本身是平行四边形而非矩形的问题，因此增加一个参数来描述，则有 $\(K = \begin{bmatrix} f_x & skew & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)$ 个参数用来建模像素是平行四边形而不是矩形，这个参数同样可以认为是传感器的安置不严格与相机主光轴垂直造成的变形的近似，事实与像素坐标系的X、Y轴之间的夹角的正切值成反比，因此当$\(skew = 0\)$表示像素为矩形。 通常我们称K为相机内参矩阵，而包含旋转和平移关系的T为外参矩阵。至此，简单的针孔相机模型就完成了。 鱼眼相机 小孔成像模型中，投影的过程可以理解为是一个三角相似变换的过程，也就是可以使用下图的过程描述，物体点沿着穿过光心的射线投影到 [图片] 传感器标定 我们所处的世界是三维的，而相机拍摄的照片却是二维的，丢失了其中距离/深度的信息。从数学上可以简单理解为，相机本身类似一个映射函数，其将输入的场景，通过某种关系映射为一张RGB图片，而我们永远无法完全准确的描述这个过程，只能是尽可能接近 而相机标定，就是使用数学模型和数学方法来近似逼近这一复杂映射函数的过程。标定后的相机即具有了描述这一过程的能力，从而可以用于各种计算机视觉的任务，如深度恢复、三维重建等，本质上都是对丢失的距离信息的恢复。为了更好地恢复这种信息，需要通过相机标定来求得这种映射关系的参数。这些参数包括内参数（例如焦距、光学中心位置等）和外参数（即相机相对于某个坐标系的方向和位置），以及畸变参数。 标定的作用主要有两个方面：校正镜头畸变和重构三维场景。
• 校正镜头畸变：由于每个镜头的畸变程度各不相同，通过相机标定可以校正这种镜头畸变，生成矫正后的图像。现实中的直线，在未经校正的图片中可能会呈现弯曲的形态，而标定后可以纠正这种情况。
• 重构三维场景：标定的过程涉及到一系列的三维点与它们在图像上对应的二维点的数学变换，通过这个过程可以求出相机的内外参数。有了这些参数，就可以根据获得的图像来重构场景的三维模型。 需要标定的场景有如下几种：
• 单目视觉应用，如单帧测距、车载 ADAS 辅助、单目SLAM等，这类应用，我们通常需要相机自身的成像模型参数以及相机相对某个坐标系下的相对位姿。再如工业上比较常见的机器人控制，需要构建机器人坐标系和其视觉坐标系之间的相对位置关系（也就是手眼标定），或者单目深度恢复等
• 双目/多目/RGB-D组合：这类应用更加常见，我们需要获取相机自身信息，以及各个相机之间的相对位姿关系，有时也需要获取其和某固定坐标系之间的关系，如车载环视相机、相机阵列（用于构建三维人体位姿等）等 目前相机标定方法主要分为三类： 标定方法 简述 优点 缺点 自标定 使用一些几何约束，如消失点等进行标定 支持在线标定，不需要特殊的设备 精度和鲁棒性较差 主动视觉相机标定 使用特定设备来控制相机进行特定运动，根据已知运动和图像变化关系进行标定 不需要标定物，鲁棒性高 离线标定，成本高昂 传统相机标定 使用特殊标志物进行标定 标定简单，成本较低，鲁棒性高 离线标定 而传统相机标定中张正友标定法仅使用一个规格已知的平面标定板进行标定，制作成本较低，因此是目前主流的标定方案。 相机成像与畸变问题 小孔成像原理：小孔成像利用的是光的直线传播。物体表面反射的光通过小孔投到屏上，物体不同位置上的光投到屏上的不同位置，形成倒像。 但是在真实世界中，没有真正意义上的小孔，因为孔径越小透过的光线越少，曝光时间就越长，但是如果孔不是那么小（或者称为大孔），那么就会出现如下情况：当光通过一个较大的孔径时，光线会发生弯曲和交叠，导致成像变得模糊。这种现象被称为衍射模糊，情况如下图所示。而小孔由于其尺寸较小，光线的衍射效应较小，因此可以形成相对清晰的成像。 [图片] 针孔相机的针孔，就可以看做最早的镜头了，可惜这个「镜头」性能实在不能满足要求。针孔相机若要成像清晰，针孔的大小就必然不大，经由针孔进入相机的光线也是少的可怜；即使如此，在成像平面上的像也不是那么清晰。直到后来，人类发明了凸透镜，利用凸透镜成像，取代小孔，成为相机真正意义上的镜头。凸透镜做镜头有着显而易见的好处。凸透镜可以做得比小孔大的多，从而进入相机的光线也多的多，落在相机成像平面上的像自然也明亮的多了。此外凸透镜成像的清晰度可比小孔要高多了，而且不会明显受到透镜大小的影响。 由于相机本身存在一个透镜组，我们简单的针孔模型并不足以描述透镜组引入的一些光线扭曲，比如我们说的超广角镜头就会存在明显的畸变。 [图片] 畸变主要分为桶型畸变和枕型畸变两种常见畸变形态，如下图所示。 [图片] 桶形畸变（Barrel Distortion）又称桶形失真，是指光学系统引起的成像画面呈桶形膨胀状的失真现象。桶形畸变在摄影镜头成像尤其是广角镜头成像时较为常见。 枕形畸变（Pincushion Distortion）又称枕形失真，它是指光学系统引起的成像画面向中间“收缩”的现象。枕形畸变在长焦镜头成像时较为常见。 较短的焦距可以捕捉更宽广的画面，但可能会引起桶型畸变；而较长的焦距可以捕捉较为狭窄的画面，但可能会引起枕型畸变。 数学上，我们使用 Brown-Conrady 模型近似描述畸变(以下公式均在图像坐标系下)：
• 径向畸变：透镜的厚薄不一，折射率不同，使得直线在投影后变成曲线 $\(x_{distorted} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)\\ y_{distorted} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)\\ r_d = r(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)\)$

• 切向畸变：机械组装过程中，透镜和成像平面不可能完全平行，从而导致切向畸变。 $\(x_{distorted} = x + 2p_1 xy + p_2 (r^2 + 2x^2)\\ y_{distorted} = y + p_1 (r^2 + 2y^2) + 2p_2 xy\)$ 最终考虑到所有畸变，有 $\(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\)$ 则最终的像素坐标可以表示为 $\(u=f_x \cdot x_d+c_x\\ v=f_y \cdot y_d+c_y\\\)$ 当然，由于制造工艺的提升，目前相机畸变主要是径向畸变，具体的使用可以灵活选择，比如使用单独的 $\(k_1、k_2\)$ 当然，上面的去畸变公式也可以使用更低阶的形式或者更高阶的形式，没有十分的严格，参数量越多则近似效果越强，但是也会带来更高的计算量 鱼眼相机也可以使用类似的方式描述畸变，不过其调整的是 在工业上，我们也会使用畸变率来描述畸变情况，定义畸变率为 $\(d = (r_d - r)/r\\ d = r_d/r - 1\\ d + 1 = r_d/r\)$ 其在一定程度上表征了径向畸变的程度。和上述的径向畸变对比来看，我们可以简单的得到 $\(d = k_1r^2 + k_2r^4 + k_3r^6\)$ 此外，畸变模型不止上面两种，事实上，畸变会存在多种模型，如最新的除法模型（最新的opencv已经采用），精度会更高 $\(r_d = r\frac{1 + k_1r^2 + k_2r^4 + k_3r^6}{1 + k_4r^2 + k_5r^4 + k_6r^6}\)$ 或者另外的除法模型 $\(r_d = r\frac{1}{1 + k_1r^2 + k_2r^4 + k_3r^6}\)$ 考虑相机传感器本身的复杂性，而且每个相机的结构都有一定的差异性（哪怕是同一批相机，在参数上都会有微小的差异），而针孔相机模型仅仅是一种真实相机的成像过程的近似，甚至于我们可以说这是一种非常粗糙的近似，因此相机标定实际上只能说近似真值而无法获得真值，那么想获取更加近似更加完美的结果，就需要使用更加准确的模型，但是更加准确的模型一般都会有更多的参数量，但是要注意的是，如果引入了过于高阶的量，就容易导致过拟合和龙格现象 最后，标定很难定量评估，除非有更真的 ”真值”，用更精确的测量设备进行精确的角度和位移测量。那么在无法获取绝对真值的情况下如何处理呢？可以通过构建一个三维空间中的直线，然后对拍摄到的图像进行去畸变操作，然后在处理后的图片的直线上进行采样，以此拟合该直线，然后对其统计方差等，以此来判断去畸变的效果，如果足够好就认为其是好内参 去畸变问题 去畸变本质上是对畸变模型的一次反向计算，我们需要通过已知的模型和 $\(r_d\)$ 来计算出 $\(r\)$ 的关系。当然，这个多项式本身相对来说求解比较复杂，一般会考虑使用优化的方法来计算，opencv 提供了相应的函数 如何计算出畸变模型的参数是一个非常实际的问题。要解出 Brown-Conrady 模型中的参数，本质上是一个非线性优化问题。你无法像解二元一次方程组那样直接算出一个确定的解，而是需要通过“逼近”的方式来寻找最优解，这个过程实际上就是重投影误差的最小化计算，就是通过给定一些 3D 点和对应 2D 角点的坐标，使用给定的相机模型，通过最小化两者的重投影误差，来优化相机的参数，因此可以构建一个无约束优化问题 $\(F(x) = \frac{1}{2} \sum_{i=1}^{m} (f_i(x))^2 = \frac{1}{2} \| f(x) \|_2^2\)$ 我们希望能够通过最小化 $$ F(x) $$ 的方法，找到某个给定形式的函数 $$ f_i(x) $$ 的系数。 PnP问题 PnP (Perspective-n-Point) 问题是计算机视觉中的一个问题，目的是在已知一定数量的三维空间点及其在图像上对应的二维点时，估计相机的位姿，即求解世界坐标系到相机坐标系的旋转矩阵R和平移向量。

• 最少点数要求：PnP 问题的未知数是相机位姿（旋转 3 自由度 + 平移 3 自由度 = 6 DOF）。P3P 算法仅需 3 个点对即可求解（最多 4 组解），第 4 个点用于消歧。因此"至少 4 个点对"是获得唯一解的实用要求。每个点对提供 2 个方程，4 个点对提供 8 个方程，足以约束 6 个自由度。

• 求解方法：常用算法包括 DLT（直接线性变换，求解完整 3×4 投影矩阵，不要求**相机已标定）、P3P（最小化求解器）、EPnP（高效 PnP，适用于**已标定**相机，O(n) 复杂度）、以及带 RANSAC 的迭代方法。注意：ICP 是 3D-3D 配准方法，**不是 PnP 求解方法。

• 应用场景：PnP算法在许多领域都有广泛应用，比如自动驾驶、增强现实、机器人导航以及视觉SLAM（Simultaneous Localization and Mapping）等领域。在这些应用中，能够准确地从图像中恢复出相机的位姿对于理解环境和做出决策至关重要。

• 算法挑战：PnP问题的求解可能会受到多种因素的影响，如特征点的选择、噪声、遮挡等。此外，不同的算法在速度、准确性和鲁棒性方面也有所不同，因此在实际应用中需要根据具体情况选择合适的算法。 张正友标定法 张正友标定方案使用平面标志物，通常是规整的棋盘格或者点阵图，将标志物打印后，使用待标定相机拍摄不同角度多组标定图案，当然，通常在多相机情况下为了方便区分图片中棋盘格朝向，我们一般使用宽高不同的棋盘格，单目情况下正方形即可 这里有几点需要注意：

• 拍摄数量通常为15~20张
• 实际上，按照标定的理论，三张图片就可以完成标定，但是为了减小标定误差，拍摄图片会稍微多一些
• 标定图案需要保证平整，每张图片中标志物尽量占据画面1/4以上，不同标定板角度尽量存在一个相对明显的旋转和平移
• 拍摄时应尽量保证相机参数不变
• 这里主要是焦距不变，焦距变化会导致部分fov变化，从而影响几乎全部的标定参数。当然，大多数场景中相机本身焦距都是固定的，而消费级，如手机镜头由于其本身畸变不大，且焦距较短，因此影响会比较小。
• 保证拍摄图案清晰，无明显模糊，由于图点在模糊情况下不会改变其中心点位置，精度理论上会更好一些
• 拍摄图案的总和需要覆盖整个画面
• 我们知道标定本身可以理解为一个拟合各个像素位置成像的过程，如果覆盖不完全，就会导致某些区域像素处于无约束状态，从而出现标定错误。我们常见的图片去畸变后边缘扭曲大多是该原因导致的。
• 使用标定工具进行标定计算 张正友标定法的数学流程是什么样呢？根据相机模型并暂时忽略畸变，有 $\(s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = A [R \quad T] \begin{bmatrix} X_w \\ Y_W \\ Z_W \\ 1 \end{bmatrix} = A [r_1 \quad r_2 \quad r_3 \quad t] \begin{bmatrix} X_w \\ Y_W \\ Z_W \\ 1 \end{bmatrix}\)$ 其中 $\(s\)$ 是尺度因子，且有内参矩阵如下 $\(A = \begin{bmatrix} \alpha & \gamma & u_0 \\ 0 & \beta & v_0 \\ 0 & 0 & 1 \end{bmatrix}\)$ 考虑到我们标定时使用的是平面标定板，我们将 $\(XOY\)$ 平面设置为标定板平面上，$\(z\)$ 轴垂直向外，这样对于检测的所有特征点都有 $\(Z_W = 0\)$ 代入上式，有 $\(s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = A [R \quad T] \begin{bmatrix} X_w \\ Y_W \\ 0 \\ 1 \end{bmatrix} = A [r_1 \quad r_2 \quad t] \begin{bmatrix} X_w \\ Y_W \\ 1 \end{bmatrix}\)$ 这里 $\(r_i\)$ 代表旋转矩阵的第 $\(i\)$ 个列向量。令 $\(\overrightarrow{M} = [X \quad Y \quad 1]^T, \quad \overrightarrow{m} = [u \quad v \quad 1]^T\)$, 上式简写为 $\(s\widetilde{m} = H\widetilde{M}\)$ 我们称 $\(H\)$ 为单应矩阵，把矩阵展开，有 $\(\begin{cases} su &= h_{11}X + h_{12}Y + h_{13} \\ sv &= h_{21}X + h_{22}Y + h_{23} \\ s &= h_{31}X + h_{32}Y + h_{33} \end{cases}\)$ 从而有 $\(\begin{cases} uXh_{31} + uYh_{32} + h_{33}u &= h_{11}X + h_{12}Y + h_{13} \\ vXh_{31} + vYh_{32} + h_{33}v &= h_{21}X + h_{22}Y + h_{23} \end{cases}\)$ 可以看到，如果对两个式子都除以 $$ h_{33} $$，并不会对整体的形式产生影响，因此，我们一般令 $$ h_{33} = 1 $$。也就是说，对于单应矩阵，其自由度并不是 9，而是 8 定义 $\(h' = [h_{11} \quad h_{12} \quad h_{13} \quad h_{21} \quad h_{22} \quad h_{23} \quad h_{31} \quad h_{32}]\)$ 那么上述可以修改为矩阵形式 $\(\begin{bmatrix} X & Y & 1 & 0 & 0 & 0 &-uX & -uY & -u \\ 0 & 0 & 0 & X & Y & 1 & -vX & -vY & -v \end{bmatrix} h' = 0\)$ 上式是一个很经典的线性方程，我们将上式写为 $$ Sh' = 0 $$，那么矩阵 $$ S^T S $$ 的最小特征值就对应该方程的最小二乘解。至此就求解出了单应矩阵，我们希望使用单应矩阵对外参矩阵进行拆解 接下来就是求解外参矩阵，我们上面求得的单应矩阵 $$ H $$ 可能和真实的值存在一个尺度因子，我们增加一个尺度因子 $\(\lambda\)$，有 $\(\lambda [h_1 \quad h_2 \quad h_3] = A [r_1 \quad r_2 \quad t]\)$ 由上节我们提到的旋转矩阵的性质可得两个约束条件 $\(r_1^T r_1 = r_2^T r_2 = 1 \\ r_1^T r_2 = 0\)$ 调整上式，有 $\(\begin{cases} \lambda h_1^T A^{-T} A^{-1} h_2 = 0 \\ \lambda h_1^T A^{-T} A^{-1} h_1 = h_2^T A^{-T} A^{-1} h_2 = 1 \end{cases}\)$ 我们知道，$\(A\)$ 的逆矩阵为 $\(A^{-1} = \begin{bmatrix} \frac{1}{\alpha} & -\frac{\gamma}{\alpha \beta} & \frac{\gamma v_0 - \beta u_0}{\alpha \beta} \\[0.5em] 0 & \frac{1}{\beta} & -\frac{v_0}{\beta} \\[0.5em] 0 & 0 & 1 \end{bmatrix}\)$ IMU模型 概述 IMU也就是惯性测量单元，是测量物体三轴姿态角（或角速率）以及加速度的装置
• 6轴：三轴加速度计+三轴陀螺仪（角速度传感器）
• 9轴：6轴+三轴磁力计（角度） 那么为什么用IMU呢？主要有以下几个原因
• 直接输出加速度角速度
• 频率高（100-400HZ），相机和雷达的频率基本上只有几十赫兹
• 不受外界干扰（磁力计可能受干扰），相机容易受到光照干扰，雷达在雨雾天可能受干扰
• 价格不贵，基本上几十几百就可以买到（几千几万的也有，但是一般精度的几十几百即可） 但是受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容易漂移，并且高精度的IMU价格昂贵，所以IMU只适合计算短时间且快速的运动 测量原理 实际上分为三种测量，加速度计、陀螺仪和磁力计 [图片] [图片] 加速度计实际上就是运用了胡克定律，也就是在弹簧固定下的质量块在有加速度的情况下会有位移，根据牛顿第二定律就可以知道加速度大小，当然在IMU中是使用电容差计算位移进而计算加速度 磁力计的工作原理跟指南针类似，通过霍尔效应计算磁场强度 误差模型 IMU的误差可以分为确定误差（可以通过标定获取，是一个确定值）和随机误差（也就是随机噪声，无法提取获取，但是可以计算出协方差矩阵） [图片] 其中，偏置是会随着时间而变化的，所以还要在 SLAM 过程中不断标定或者说估计，很多紧耦合的框架都会在过程中不断估计 测量模型 世界坐标系为 G，IMU坐标系为 I，通常忽略 Scale [图片] 一般认为重力加速度不变，导数为零，偏置服从随机游走模型，其导数是高斯的 所以，在世界坐标系下的速度等状态量如下 [图片] 运动模型（连续+离散） 基于连续的数学运动模型考虑 那么在 G 也就是全局坐标系下，连续运动的导数为 [图片] 后面两个容易理解，速度求导为加速度，位置求导为速度 对于第一个公式，我们假设一个从原点出发的向量 [图片] 绕某一个方向上的单位轴 [图片] 旋转，那么角速度为 [图片] ，角速度大小为 [图片] ，那么根据刚体旋转动力学，任意点绕固定轴旋转的线速度由角速度向量和该点位置向量的叉乘确定 [图片] 那么，坐标系沿着单位轴旋转，其三个轴的导数 [图片] 但是，机器无法处理连续数值，所以只能进行离散化才可以进一步处理，也就是进行欧拉积分，当然实际上也可以使用两帧之间的平均值计算（也就是中值积分） [图片] 这里因为时间间隔很小，所以在实际代码中可以忽略二次项 然后把测量模型的数值带入离散模型，就可以得到如下的计算模型，然后循环执行计算即可，进而不断更新状态量 [图片] 使用方法 IMU 的用途主要是作为先验估计来优化激光点，在实际中，我们希望是所有点云在同一时刻的采样，但是激光雷达的采样频率相对较慢而且存在运动畸变，而 IMU 的采样频率则非常高，所以可以使用 IMU 来去除畸变，并且把所有的点云统一到同一个时刻进行处理，文中是在 tk 时刻。因此，我们根据 IMU 积分估计的位姿，把 $\(t_{k-1}\)$ 每个点转到 tk 时刻。即同一时刻与同一位姿，发射与接受激光束。 [图片]
• 从尾部开始遍历找 head 对应的imu数据
• head对应的IMU状态之前已经计算出来了，计算当前点到head的时间间隔
• 假设tail对应的IMU数据为 当前点对应的IMU数据，因为IMU频率比较高，短时间内变化不大
• 计算head到当前点的位姿变换：∆T，并广义加到head对应的位姿，得到当前点 it 对应的位姿：P_i、R_i
• 计算当前点到 end_lidar的位姿变换：T_ei、R_ei 在激光SLAM中，大多数的算法都是以启动系统的时候第一帧 IMU 为参考，但是在安装 IMU 的时候可能存在安装偏差，导致 IMU 坐标系相对地面系有一个微小旋转 ROS1代码 在 ROS1 中，使用 sensor_msgs/Imu 来作为消息类型传递 IMU 数据的，其中包括了四部分
• 头消息：std_msgs/Header header，其中包含时间戳 stamp，时间戳可以使用 toSec() 方法转换以便求时间差
• 四元数及协方差：geometry_msgs/Quaternion orientation，表示朝向，并且有协方差数组float64[9] orientation_covariance
• 角速度及协方差：geometry_msgs/Vector3 angular_velocity 和 float64[9] angular_velocity_covariance
• 加速度及协方差：geometry_msgs/Vector3 linear_acceleration 和 float64[9] linear_acceleration_covariance 点云处理 激光雷达模型 目前的激光雷达基本上是光学雷达，通过发射脉冲激光，基于飞行时间法测量距离（也有部分初级雷达使用三角测距法），并且多线雷达还会同时发射多组激光来感知环境 激光雷达主要是以下三种，其中机械式激光雷达较为常见，受限于机械式结构，寿命相对较短，难以实现车规级运用 目前相对好的是混合固态的，相对机械式寿命相对更长，其中有大疆的Livox系列 [图片] 点云运动畸变矫正 因为雷达与机器人是刚性连接，并且是不断进行采样而非一次性完成采样的，所以会发生一个情况，点云中的点不是同一时间完成采集的，并且在这个过程中机器人是在运动的，所以同一个物体会造成多个位置不同的采样点，如下图所示，五角星表示物体采样的点，在一段时间的起止时刻，因为机器人的位置或者说坐标系发生了变化，所以测量出来的位置就会不一样，或者说产生了错位，因为在起始时刻和结束时刻的点云是在两个不同的坐标系下采样的，但是统一在结束时刻的坐标系下进行处理，也就是产生了运动畸变 [图片] 那么如何进行矫正呢，实际上就是把所有的点归到同一个坐标系下，然后把误差补偿掉 具体步骤如下图所示，根据从里程计（比如说IMU或者轮速里程计）处获得的位姿信息，计算出来结束时刻相对起始时刻的位姿变化，然后插值计算出来每一次点云扫描时刻的位姿，基于这个插值出来的位姿变化矫正点云坐标，使其统一转化到起始时刻的坐标下或者结束时刻的坐标下 [图片] 解决畸变工程技巧 工程上的一些技巧
• 激光雷达安装方式，通常y轴朝向载体左右两侧
• 相对位姿变换可以通过GPS或者IMU或者轮式里程计获取 点云下采样 因为直接从激光雷达获取的点云中含有大量的点，所以需要进行下采样减少点的数量，其中体素滤波方法较为常见，但是要注意体素大小，太大的话会造成点云细节缺失 [图片] 特征点提取 特征点提取方法中的角点和面点提取的方法来源于LOAM论文，这是激光SLAM的开篇之作，实际上这种特征点是根据曲率来判断并且提取的 [图片] 具体的提取方法是这样的，对于同一个scan（也就是同一个射线上的点），判断其曲率，曲率公式如上所示，选取一个近邻点集进行曲率计算，然后根据阈值判断是否为角点或者面点，上图右下角的图片中，绿色圆形就是面点，橙色三角就是角点 区分角点和面点的原因是，在计算残差的时候，两种特征点的计算方式不一样 残差 将去除畸变后的特征点转换到全局坐标系下 两种点的残差计算方式如下，而我们要做的事情就是通过优化位姿，使得残差接近零 [图片] 其中角点的残差计算方式如图，其中 [图片] 表示第 [图片] 帧的第 [图片] 个点的坐标（波浪线表示已经乘以待优化位姿的坐标，也就是转到世界坐标系下的坐标），然后我们计算此点到近邻直线的距离 ROS1 代码 点云是激光雷达产生的，而不同的激光雷达因为工作原理等不同，所以给出的点云格式也不同，这需要不同的厂家给出相应的代码从而得到 sensor_msgs::PointCloud2 格式的 ROS点云消息，然后我们需要使用 PCL 进行下采样和滤波操作 pcl::PointCloud pl_orig;// shengc pcl::fromROSMsg(msg,pl_orig); std::cout<<"lidar point size: "<<pl_orig.points.size()<<std::endl;

pcl::VoxelGrid 激光SLAM前端 激光里程计实际上就是一种点云配准的方法，通过两帧点云之间的相对位姿变换来计算里程计，而想实现两帧点云的相对位姿变换求解，就要进行点云配准，其中经典方法有ICP和NDT，两种方法都可以在一些点云库中调用现成接口实现 ICP方法 这是一种经典方法，思想是两帧点云中距离最近的点是近邻点，下图中 [图片] 和 [图片] 是两帧点云， [图片] 和 [图片] 是两帧点云的求和 [图片] 然后先后分布求解 [图片] 和 [图片] [图片] NDT方法 NDT方法也就是正态分布变换方法，是另一种经典的方法，思想是，两帧点云的分布要尽可能接近，具体操作是对上一帧点云计算分布，然后下一帧点云要尽可能接近这个分布，对应的姿态就是要寻找的姿态或者说最准确的姿态 [图片] 如上图所示，对上一帧点云 [图片] 进行均值和协方差矩阵计算，得到了高斯形式的概率密度函数，然后使得概率乘积尽可能更大，然后使用极大似然估计方法进行求解 [图片] 作者提出的一种改进如上图左侧所示，原来的概率分布（红线）可能会出现概率为0或者无穷大的情况，所以要做一点改动，如上图左下角公式所示，可以避免异常情况，变成绿线所示 [图片] 这里，NDT更不容易受到噪声干扰，因为点云中的点是存在噪声的，在ICP中噪声也会参与计算，造成干扰，导致优化陷入局部最优，此外NDT是点云的概率分布函数进行计算，所以计算上更快 特征点配准方法 这是一种优化的配准方法，选取点云中一些特殊的点来进行配准，在减少了点云数量的同时，基于特征点配准也会降低噪声的干扰，在LOAM中使用的特征点是角点和面点，也就是曲率满足一定条件的点 [图片] LOAM的代码是这样操作的，代码如下 for(size_t i = startIdx，regionIdx = 0; i <= endIdx;i++，regionIdx++){ //遍历点云，求取第 i 个点的曲率，pointWeight是权重，这里是-10，要与采样点数量对应或者说为相反数，便于后面的计算 float diffX = pointWeight * _laserCloud[i].x; float diffY = pointWeight * _laserCloud[i].y; float diffZ = pointWeight * _laserCloud[i].z;

//_config.curvatureRegion是求曲率范围，这里是5，也就是选取半径为5，选择临近的10个点求曲率
    for(int j = 1;j <= _config.curvatureRegion; j++){
    //实际上就是计算差值，因为diffX等是负值，并且已经乘以权重了，遍历求和等于计算所有邻近点与此点的差值并且求和，相当于计算所有近邻点相对此点的偏移量之和
    diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
    diffY += _lasercloud[i + j].y + _laserCloud[i - j].y;
    diffZ += _lasercloud[i + j].z + _laserCloud[i - j].z;
}
_regionCurvature[regionIdx] = diffx* diffx + diffy * diffy + diffz * diffz;
//对三维上的偏移量求平方和，以此来判断曲率是否过大或者过小
_regionSortIndices[regionIdx]= i;

} PL-ICP方法 除去点到点的配准方法，还有一种是点到线的配准方法，也就是PL-ICP方法，也即Point-Line ICP，其精度相对点到点方法更高，这也是LOAM采用的配准方法 [图片] VSLAM前端 2D-2D 假设在相邻时刻，相机分别观测到两个图像，并且在其中匹配到了对应点，如何求两个图像对应位姿之间的位姿变换 假设对应点的齐次像素坐标为 $\(p_1\)$ 和 $\(p_2\)$，那么在两个时刻的相机坐标系下有下面的公式，其中相机内参矩阵已知 $\(p_1=KP_{C1}\\p_2=KP_{C2}\)$ 那么以第一个相机坐标系为基准（或者为世界坐标系的话），那么有 $\(P_{C2}=RP_{C1}+t\\K^{-1}p_2=P_{C2}=RP_{C1}+t=RK^{-1}p_{1}+t\\t^\wedge K^{-1}p_2=t^\wedge R K^{-1}p_{1}+t^\wedge t\\其中，向量与自身的叉乘为零\)$ 然后左边乘以 $\((K^{-1}p_2)^T\)$，其中左侧为 $\(t^\wedge K^{-1}p_2\)$，也就是 t 与向量的叉乘，方向上垂直于此向量，再进行与向量的点乘结果为0，故有 $$ (K^{-1}p_2)^Tt^\wedge R K^{-1}p_{1}=p_2^TK^{-T}t^\wedge R K^{-1}p_{1}=0 $$ VIO 融合方案介绍 [图片] [图片] 激光SLAM框架 LOAM框架 LOAM框架是14年提出的三维激光框架，非常经典的激光里程记和建图方案，也是其他 LOAM 方案的鼻祖，LOAM 只基于激光雷达（可选 IMU），通过把 SLAM 拆分成一个高频低精的前端以及一个低频高精的后端来实现 lidar 里程记的实时性。 其系统架构如下，前端就是激光配准和点云里程计，里程计输出会有一个漂移，所以后面跟着一个Mapping的环节来优化这种情况，并且LOAM没有回环检测功能 在里程计部分，通过提取特征点，加上优化点到线的距离和点到平面的距离并且采用L-M方法求最优解，得到两帧点云之间的位姿关系 [图片] 在后面的地图部分，还会进行一次地图匹配，重新计算一次位姿变换，并且对两次的数据进行融合 [图片] 特征点提取 在LOAM的代码中，特征点提取部分考虑了特征点的均匀性，按照角度把点云分为了四个部分或者说四个扇形，每个扇形里面提取出固定数量的角点和面点 同时，对于一些特殊情况（如存在遮挡等情况的点云，也就是坏点）进行了剔除操作 点云配准 为了找到特征点的对应关系，要进行一次运动畸变矫正，然后才可以更为准确的进行运动畸变矫正 [图片] LEGO-LOAM 这是IROS2018的文章，基于LOAM进行了改进，并且一个很大的改进就是加入了回环检测功能 [图片] Cartographer 这是一个非常精简的框架，基于图优化算法，其在算法上并没有非常先进，但是其优点在于代码工程化非常好，而且不需要依赖于PCL、g2o等庞大的代码库 其分前端和后端，并且分别维护局部坐标系和全局坐标系（也就是 local 和 global），局部坐标系是不会动的，建图之后就固定了，而全局坐标系是动态维护更新的 在前端部分，其会将一次 scan 来与子地图（也就是submap）进行匹配，并且使用非线性优化来进行估计 地图是一种概率栅格地图，其中表达的是此处有障碍或者被占用的概率是多少，而且将雷达点分为了两部分，如果点落在某一个格子上，则此格子称为 hit（意思是打中了障碍物），而击中点与雷达中心之间连线所经过的格子则称为miss（意思是这个路径是空的或者说通畅的） Branch-and-Bound scan matching 分支定界算法，最初来源于数学中的混合整数线性规划问题，在Cartographer中的核心思想是，将解空间划分为树状结构，每一个节点代表一个解，叶子节点代表真正的解，这个算法分为三部分：选择，分支，定界 选择算法是使用深度优先搜索也就是DFS算法 FAST-LIO框架 创新点有二：一是提出了一种紧耦合的迭代卡尔曼滤波方法，以此融合雷达特征点和 IMU，二是提出了新的卡尔曼滤波增益公式 FAST-LIO的主体框架如下图所示，输入部分是雷达和IMU，雷达的输入是100k-500kHz，实际上因为这是固态雷达，一个点就相当于一个采样，所以是一秒有100k-500k的点云输入，当然一个点是无法进行SLAM的，所以会积累几十或者上百k个单位的点之后才会作为一次输入送入SLAM（一般取决于里程计频率，比如说里程计频率为50Hz时，则在20ms内积累点云，成为一帧点云并处理一次，这20ms内的点云为一次scan），然后进行特征提取（方法与LOAM一致，就是面点和角点提取，或者说是平面点和边缘点）；而IMU的输入会送入前向传播模块（这里传播的是状态量，也就是待估计的位移、偏置等），前向传播的目的有 - 在两帧雷达数据之间有很多帧IMU数据，将这些IMU数据进行粗略积分，得到位姿变化的估计，用于卡尔曼滤波的预测值、反向传播的运动补偿 - 传递误差量，尤其是传递误差量的变化和协方差矩阵（在后面迭代更新的时候会用到） 然后就是反向传播和运动补偿，因为进行了前向传播，所以每一帧的IMU都有一个预估的位姿，通过这个位姿对激光雷达点云数据进行运动补偿，降低运动产生的失真 然后将补偿后的点云和预估的状态量进行一个残差的计算，然后通过误差状态卡尔曼滤波器进行迭代更新状态量，如果收敛的话就进行一个里程计输出，否则继续进行迭代 收敛并且输出里程计之后，就会根据里程计信息或者说位姿信息，把新的点云插入到全局地图中去，当然也会在全局地图中根据位姿信息进行一个下采样，得到一个子地图，根据这个子地图去寻找近邻点进行匹配 [图片] 在一次scan的过程中，记录起止时间为 [图片] 和 [图片] ，后者为当前时刻，可以认为点云特征是在时间轴上均匀分布并且首尾对齐的，但是IMU的数据并非首尾对齐的，所以需要统一投影到当前时刻下或者说最后一帧点云下，这里使用 [图片] 和 [图片] 来表示 IMU 和 LiDAR 的时刻 数学公式 这里定义了一种广义的加减法或者说操作，首先我们定义 [图片] 是流形，例如 [图片] [图片] 如果 [图片] 是李群，那么则有： [图片] 如果是向量的话，那么广义加减就定义为向量的加减 实际上就相当于使用了一个 C++ 中的运算重载，以此来方便运算 IMU离散模型 在模型中，当时间间隔为 [图片] 的时候，设状态量为 [图片] ，则离散模型中的数据可以表示为 [图片] 其中 [图片] 是状态量，即为所有的待估计变量，共18维，我们要实时估计的是一个18维的量，它包含旋转矩阵，位置，速度、角速度零偏、加速度零偏以及重力向量。因此它是一个紧耦合的框架。 输入是 [图片] （也即加速度测量值）, [图片] 为所有的噪声量，包含测量噪声（前两项）和bias随机游走噪声（后两项），每一项都是三维的 而论文中的离散模型如下所示 [图片] [图片] 是 IMU 测量的序号或者说索引，这个索引并不是整个过程中的，而是两帧 LiDAR 之间的索引，也就是每测量一帧的 LiDAR 都会执行一次上面的公式，然后重新计算 IMU 测量的索引 其中 [图片] 实际上广义加可以拆开来看，不同的位置单独执行广义计算 状态估计 这里使用纯字母表示真值，使用波浪线上标表示误差，使用横线表示最优估计，使用上三角表示估计值 前向传播 前向传播有两个内容，第一个就是基于IMU积分计算一个粗略的状态量，这个状态量用于后续的反向传播来补偿运动失真 $\(\hat{x}_{i+1}=\hat{x}_i\boxplus (\Delta tf(\hat{x}_i,u_i,0)),\hat{x}_0=\bar{x}_{k-1}\)$ 其中的时间差表示的是相邻两帧 IMU 的时间差，噪声量为0是因为不知道噪声的实际大小，因此在传播过程中设为0，但是会在后续的误差状态方程中考虑噪声 这个公式与离散模型公式一致，我们每接收一个 IMU 都会进行一次上述计算，直到计算到最后一个 IMU 帧为止。 另一个内容是传播误差量，并计算对应的协方差矩阵。这里的问题是我们不知道真值，怎么计算误差呢？实际上，我们计算的误差量，也是一个近似值，因此它才会有对应的协方差矩阵来评判置信度。和传统的卡尔曼滤波器不同的是，传统的卡尔曼滤波器直接估计状态量，它的运动方程和观测方程通常长这样： $\(x_k=f(x_{k-1},u_k)+w_k\\z_k=h(x_k)+v_k\)$ 而文中使用的误差状态卡尔曼滤波器(Error state Kalman flter，ESKF)，以误差量作为待估计量，也就是把上式的 x 用 $\(\widetilde{x}\)$ 代替 我们现在要估计的是误差量，而不是直接估计状态量。而有了误差量的估计，再直接加上状态量的估计就是我们求得的最优估计，其中误差量 $\(\widetilde{x}_{k-1}=x_{k-1}\boxminus \bar{x}_{k-1}\)$ 这将带来以下好处: - 在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统KF需要用到四元数或者更高维的表达（如九维旋转矩阵），或采用带有奇异性的表达方式(欧拉角) - ESKF 总是在原点附近，离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题 - ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替 - 误差状态的运动学也相比原状态变量要来得更小，因为我们可以把大量更新部分放到原状态变量中 卡尔曼滤波 现在以误差状态卡尔曼滤波为主 ESKF公式推导 在现代的大多数IMU系统中，人们往往使用误差状态卡尔曼滤波器（Error state Kalman filter, ESKF）而非原始状态的卡尔曼滤波器。大部分基于滤波器的LIO或VIO实现中，都使用ESKF作为状态估计方法。相比于传统KF，ESKF的优点可以总结如下： 1. 在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统KF需要用到四元数（4维）或者更高维的表达（旋转矩阵，9维），要不就得采用带有奇异性的表达方式（欧拉角）。 2. ESKF总是在原点附近，离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题。 3. ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替。 4. 误差状态的运动学也相比原状态变量要来得更小，因为我们可以把大量更新部分放到原状态变量中。 在ESKF中，我们通常把原状态变量称为名义状态变量（Nominal State），然后把ESKF里的状态变量称为误差状态变量（Error State）。 - 标称状态：承载了系统运动的主要分量（大信号），其动力学方程通常是非线性的。在滤波过程中，标称状态根据IMU数据进行积分预测，并在每次测量更新后加上误差状态的估计值进行修正。 - 误差状态：表示真值与标称值之间的微小偏差（小信号）。由于误差量通常在零附近微小波动，其动力学方程极其适合进行线性化处理，满足卡尔曼滤波对线性高斯系统的假设。 ESKF整体流程如下：当IMU测量数据到达时，我们把它积分后，放入名义状态变量中。由于这种做法没有考虑噪声，其结果自然会快速漂移，于是我们希望把误差部分作为误差变量，放在ESKF中。ESKF内部会考虑各种噪声和零偏的影响，并且给出误差状态的一个高斯分布描述。同时，ESKF本身作为一种卡尔曼滤波器，也具有预测过程和修正过程，其中修正过程需要依赖IMU以外的传感器观测。当然，在修正之后，ESKF可以给出后验的误差高斯分布，随后我们可以把这部分误差放入名义状态变量中，并把ESKF置零，这样就完成了一次循环 注意一下，标称状态并不是测量值的等价概念，其准确定义是基于运动模型和 IMU 测量值（作为控制输入），在不考虑噪声的情况下推演出的“理想”预测值，是需要进一步优化的状态，但是其实际上也是测量得出的——基于上一次优化后的状态然后结合测量值的积分计算得出 ESKF状态方程 然后可以认为系统的真实状态是标称状态加上一个误差状态的，因此可以定义真实状态变量、名义状态变量和误差状态变量为： $\(\begin{gathered} \boldsymbol x_t=[\boldsymbol{p}_t ,\boldsymbol{R}_t ,\boldsymbol{v}_t ,\boldsymbol{b}_{a,t},\boldsymbol{b}_{g,t},\boldsymbol{g}_t]^T \newline \boldsymbol x=[\boldsymbol{p} ,\boldsymbol{R} ,\boldsymbol{v} ,\boldsymbol{b}_a,\boldsymbol{b}_g,\boldsymbol{g}]^T \\ \delta \boldsymbol x=[\delta\boldsymbol{p} ,\delta\boldsymbol{R} ,\delta\boldsymbol{v} ,\delta\boldsymbol{b}_a,\delta\boldsymbol{b}_g,\delta\boldsymbol{g}]^T \end{gathered}\)$ 其中 $\(p\)\(为相对于世界坐标系的平移，\)\(R\)$ 为相对于世界坐标系的旋转，$\(v\)\(为相对于世界坐标系的速度，\)\(b_a\)\(为当前时刻的加速度计随机游走偏置，\)\(b_g\)\(为陀螺仪的随机游走偏置，\)\(g\)$为世界坐标系下的重量向量，每个状态量的自由度为 3 维，其中带下标 $\(t\)$的表示真值，并且认为各种状态都是时间的函数 然后根据相关理论，很容易推导出状态变量导数相对于观测量的关系式，其中，在连续时间上我们记录 IMU 读数为 $\(\tilde{\boldsymbol{\omega}}\)$ 与 $\(\tilde{\boldsymbol{a}}\)$： $\(\begin{gather} \dot{\boldsymbol{p}}_t = \boldsymbol{v}_t\\ \dot{\boldsymbol{v}}_t = \boldsymbol{R}_t (\tilde{\boldsymbol{a}} - \boldsymbol{b}_{a,t} - \boldsymbol{\eta}_a) + \boldsymbol{g}\\ \dot{\boldsymbol{R}}_t = \boldsymbol{R} _t(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t} - \boldsymbol{\eta}_g)^\wedge\\ \dot{\boldsymbol{b}}_{g,t} = \boldsymbol{\eta}_{b,g}\\ \dot{\boldsymbol{b}}_{a,t} = \boldsymbol{\eta}_{b,a}\\ \dot{\boldsymbol{g}} = \boldsymbol{0} \end{gather}\)$ 这里把重力考虑进来的主要理由是方便确定IMU的初始姿态。如果我们不在状态方程里写出重力变量，那么必须事先确定初始时刻的IMU朝向 $\(\boldsymbol R(0)\)$，才可以执行后续的计算。此时IMU的姿态就是相对于初始的水平面来描述的。而如果把重力写出来，就可以设IMU的初始姿态为单位矩阵，而把重力方向作为IMU当前姿态相比于水平面的一个度量。二种方法都是可行的，不过将重力方向单独表达出来会使得初始姿态表达更加简单，同时还可以增加一些线性性 六项公式很容易理解： 1. 对位置求导获取速度 2. 世界坐标系下的加速度变换，其中的其中的 $\(\boldsymbol{n}_a\)$是加速度计高斯白噪声 3. 旋转向量与旋转矩阵的导数之间的变换关系，其中的 $\(\hat{\boldsymbol \omega}\)$是陀螺仪的测量值， $\(\boldsymbol{n}_\omega\)$是陀螺仪高斯白噪声 4. 陀螺仪偏置求导，认为导数是高斯白噪声 5. 加速度计偏置求导，认为导数是高斯噪声 6. 重力认为是常量，导数为零 如果把观测量和噪声量整理成一个向量，我们也可以把上式整理成矩阵形式。不过这里的矩阵形式将含有很多的零项，相比上式并不会有明显简化，所以我们就先使用这种散开的公式。下面我们来推导误差状态方程。首先定义误差状态变量为： $\(\begin{gather} \boldsymbol{p}_t = \boldsymbol{p} + \delta\boldsymbol{p}\\ \boldsymbol{R}_t = \boldsymbol{R}\,\delta\boldsymbol{R}\\ \boldsymbol{v}_t = \boldsymbol{v} + \delta\boldsymbol{v}\\ \boldsymbol{b}_{a,t} = \boldsymbol{b}_a + \delta\boldsymbol{b}_a\\ \boldsymbol{b}_{g,t} = \boldsymbol{b}_g + \delta\boldsymbol{b}_g\\ \boldsymbol{g}_t = \boldsymbol{g} + \delta\boldsymbol{g} \end{gather}\)$ 这里其他的项都是线性的，因此直接叠加，但是旋转矩阵是不满足加法而满足乘法的，因此是相乘，并且误差旋转矩阵是相对于机身坐标系而不是世界坐标系的旋转误差，此外 IMU 一般是固定在机身上，因此是右乘 不带下标的就是名义状态变量，名义状态变量的运动学方程式与真值相同，只是不必考虑噪声（因为噪声在误差状态方程中考虑了）。其中旋转部分的 $\(\delta\boldsymbol{R}\)$ 可以用它的李代数 $\(\text{Exp}(\delta\boldsymbol{\theta})\)$ 来表示，此时旋转公式也需要改成用指数形式来表达。关于误差变量的平移、零偏和重力公式，都很容易得出对应的时间导数表达式，只需在等式两侧分别对时间求导即可 $\(\begin{gather} \delta \dot{\boldsymbol{p}}=\delta {\boldsymbol{v}} \\ \delta \dot{\boldsymbol{b}}_g=\boldsymbol \eta_g \\ \delta \dot{\boldsymbol{b}}_a =\boldsymbol \eta_a \\ \delta {\boldsymbol{g}}=0 \\ \end{gather}\)$ 其中因为速度和旋转两个方程与 $\(\delta \boldsymbol R\)$ 有关，需要单独推导，具体推导过程在下面两小节给出，这里先给出完整的误差变量的运动学状态方程 $\(\begin{split} \delta \dot{\boldsymbol{p}} &= \delta \boldsymbol{v} \\ \delta \dot{\boldsymbol{v}} &= -\boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a)^\wedge \delta \boldsymbol{\theta} - \boldsymbol{R} \delta \boldsymbol{b}_a - \boldsymbol{\eta}_a + \delta \boldsymbol{g} \\ \delta \dot{\boldsymbol{\theta}} &= -(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_g)^\wedge \delta \boldsymbol{\theta} - \delta \boldsymbol{b}_g - \boldsymbol{\eta}_g \\ \delta \dot{\boldsymbol{b}}_g &= \boldsymbol{\eta}_{bg} \\ \delta \dot{\boldsymbol{b}}_a &= \boldsymbol{\eta}_{ba} \\ \delta \dot{\boldsymbol{g}} &= \boldsymbol{0} \end{split}\)$ 误差状态的旋转项 将旋转误差方程两侧分别对时间求导可得： $\(\begin{gather} \begin{split} \dot{\boldsymbol{R}}_t &= \dot{\boldsymbol{R}}\delta \boldsymbol{R}+ \boldsymbol{R}\delta \dot{\boldsymbol{R}}\\ &=\dot{\boldsymbol{R}}\text{Exp}(\delta\boldsymbol{\theta}) + \boldsymbol{R}\dot{\text{Exp}(\delta\boldsymbol{\theta})} \\ &= \boldsymbol{R} _t(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t} - \boldsymbol{\eta}_g)^\wedge \end{split} \end{gather}\)$ 又有公式：$\(\dot{\text{Exp}(\delta\boldsymbol{\theta})}=\text{Exp}(\delta\boldsymbol{\theta})\delta\dot{\boldsymbol{\theta}}^\wedge\)$，可以将其中的对应项进行转换，将第二行化为如下形式，并且标称状态是不考虑噪声的理想值，因此噪声项为 0 $\(\begin{gather} \dot{\boldsymbol{R}}\text{Exp}(\delta\boldsymbol{\theta}) + \boldsymbol{R}\dot{\text{Exp}(\delta\boldsymbol{\theta})} = \boldsymbol{R} \left( \tilde{\boldsymbol \omega} - \boldsymbol{b}_g \right)^\wedge \text{Exp}(\delta\boldsymbol{\theta}) + \boldsymbol{R}\text{Exp}(\delta\boldsymbol{\theta})\delta\dot{\boldsymbol{\theta}}^\wedge \end{gather}\)$ 再将第三行的真值消去，有如下形式 $\(\begin{gather} \boldsymbol{R}_t (\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t} - \boldsymbol{\eta}_g)^\wedge = \boldsymbol{R}\text{Exp}(\delta\boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t} - \boldsymbol{\eta}_g)^\wedge \end{gather}\)$ 根据公式20可知，公式21、22是相等的，因此可以联立两式，将其中的 $\(\dot{\delta \boldsymbol{\theta}}^\wedge\)$ 移动到一侧，并且约掉左侧的旋转矩阵，并且整理类似项，可以有如下形式 $\(\begin{gather} \text{Exp}(\delta\boldsymbol{\theta})\dot{\delta \boldsymbol{\theta}}^\wedge = \text{Exp}(\delta\boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t} - \boldsymbol{n}_g)^\wedge - \left( \tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t}\right)^\wedge \text{Exp}(\delta\boldsymbol{\theta}) \end{gather}\)$ 注意 $\(\text{Exp}(\delta\boldsymbol{\theta})\)$ 本身是一个SO(3)矩阵，利用SO(3)上的伴随性质用来交换，且其中根据旋转矩阵的性质有：$\(\text{Exp}(\delta\boldsymbol{\theta})^T=\text{Exp}(-\delta\boldsymbol{\theta})\)$ 李群的伴随性质为：$\(\boldsymbol\phi^\wedge\boldsymbol R=\boldsymbol R(\boldsymbol R^T \boldsymbol \phi)^\wedge\)$ 然后李群的伴随性质，可以将上面公式至的最后一项 $\(\left( \tilde{\boldsymbol{\omega}} - \boldsymbol{b}_{g,t}\right)^\wedge \text{Exp}(\delta\boldsymbol{\theta})\)$项进行交换 $$\begin{gather} \begin{split} \text{Exp}(\delta\boldsymbol{\theta})\delta\dot{\boldsymbol{\theta}}^\wedge &= \text{Exp}(\delta\boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}} - \boldsymbol{b}{g,t} - \boldsymbol{\eta}_g)^\wedge-\text{Exp}(\delta\boldsymbol{\theta}) [\text{Exp}(-\delta\boldsymbol{\theta})(\tilde{\boldsymbol{\omega}} - \boldsymbol{b})]^\wedge\ &= \text{Exp}(\delta\boldsymbol{\theta}) [(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}{g,t} - \boldsymbol{\eta}_g)^\wedge-(\text{Exp}(\delta\boldsymbol{\theta})(\tilde{\boldsymbol{\omega}} - \boldsymbol{b} ))^\wedge]\ &\approx \text{Exp}(\delta\boldsymbol{\theta})[(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}{g,t} - \boldsymbol{\eta}_g)^\wedge-((\boldsymbol I-\delta\boldsymbol{\theta}^\wedge)(\tilde{\boldsymbol{\omega}} - \boldsymbol{b} ))^\wedge]\ &= \text{Exp}(\delta\boldsymbol{\theta})[\boldsymbol{b}{g}-\boldsymbol{b}}-\boldsymbol{\etag+\delta\boldsymbol{\theta}^\wedge \tilde{\boldsymbol{\omega}}-\delta\boldsymbol{\theta}^\wedge\boldsymbol{b}]^\wedge\ &= \text{Exp}(\delta\boldsymbol{\theta})[(-\tilde{\boldsymbol{\omega}}+\boldsymbol{b}_g)^\wedge \delta\boldsymbol{\theta}-\delta\boldsymbol{b}_g-\boldsymbol{\eta}_g]^\wedge

\end{split} \end{gather}$$ 然后约掉左侧的系数 $\(\text{Exp}(\delta\boldsymbol{\theta})\)$即可得到： $\(\begin{gather} \delta\dot{\boldsymbol{\theta}} \approx \left( -\tilde{\boldsymbol{\omega}} + \boldsymbol{b}_g \right)^\wedge \delta\boldsymbol{\theta} - \delta\boldsymbol{b}_g - \boldsymbol{\eta}_{g} \end{gather}\)$ 误差状态的速度项 接下来考虑速度方程的误差形式，获取误差状态速度项的表达式，对速度求导即为加速度，因此速度真值的导数等于加速度真值，根据状态方程有： $$\begin{gather} \begin{split} \dot{\boldsymbol{v}}t &= \boldsymbol{R}_t (\tilde{\boldsymbol{a}} - \boldsymbol{b}_t \ &= \boldsymbol{R} \text{Exp}(\delta\boldsymbol{\theta}) (\tilde{\boldsymbol{a}} - \boldsymbol{b}_a - \delta\boldsymbol{b}_a - \boldsymbol{\eta}_a) + \boldsymbol{g} + \delta\boldsymbol{g} \ &\approx \boldsymbol{R} (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) (\tilde{\boldsymbol{a}} - \boldsymbol{b}_a - \delta\boldsymbol{b}_a - \boldsymbol{\eta}_a) + \boldsymbol{g} + \delta\boldsymbol{g} \ &\approx \boldsymbol{R}\tilde{\boldsymbol{a}} - \boldsymbol{R}\boldsymbol{b}_a - \boldsymbol{R}\delta\boldsymbol{b}_a - \boldsymbol{R}\boldsymbol{\eta}_a + \boldsymbol{R}\delta\boldsymbol{\theta}^\wedge \boldsymbol{a} - \boldsymbol{R}\delta\boldsymbol{\theta}^\wedge \boldsymbol{b}_a + \boldsymbol{g} + \delta\boldsymbol{g} \ &= \boldsymbol{R}\tilde{\boldsymbol{a}} - \boldsymbol{R}\boldsymbol{b}_a - \boldsymbol{R}\delta\boldsymbol{b}_a - \boldsymbol{R}\boldsymbol{\eta}_a - \boldsymbol{R}\tilde{\boldsymbol{a}}^\wedge \delta\boldsymbol{\theta} + \boldsymbol{R}\boldsymbol{b}_a^\wedge \delta\boldsymbol{\theta} + \boldsymbol{g} + \delta\boldsymbol{g} \end{split}} - \boldsymbol{\eta}_a) + \boldsymbol{g

\end{gather}$$ 从第三行推向第四行时，需要忽略 $\(\delta\boldsymbol{\theta}^\wedge\)$ 与 $\(\boldsymbol{\eta}_a\)$以及 $\(\delta\boldsymbol{b}_a\)$ 相乘的二阶小量。从第四行推第五行则用到了叉乘符号交换顺序之后需加负号的性质。另一方面，等式右侧为 $\(\begin{gather} \dot{\boldsymbol{v}} + \delta\dot{\boldsymbol{v}} = \boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a) + \boldsymbol{g} + \delta\dot{\boldsymbol{v}} \end{gather}\)$ 因为上面两式是相等的，因此可以得到 $\(\begin{gather} \delta\dot{\boldsymbol{v}} = -\boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a)^\wedge \delta\boldsymbol{\theta} - \boldsymbol{R} \delta\boldsymbol{b}_a - \boldsymbol{R} \boldsymbol{\eta}_a + \delta\boldsymbol{g} \end{gather}\)$ 这样我们就得到了 $\(\delta{\boldsymbol{v}}\)$ 的运动学模型。需要补充一句，由于上式中 $\(\boldsymbol{\eta}_a\)$ 是一个零均值白噪声，它乘上任意旋转矩阵之后仍然是一个零均值白噪声，而且由于 $\(\boldsymbol{R}^T\boldsymbol{R}=\boldsymbol{I}\)$ ，其协方差矩阵也不变（留作习题）。所以，也可以把上式简化为： $\(\begin{gather} \delta\dot{\boldsymbol{v}} = -\boldsymbol{R}(\bar{\boldsymbol{a}} - \boldsymbol{b}_a)^\wedge \delta\boldsymbol{\theta} - \boldsymbol{R} \delta\boldsymbol{b}_a - \boldsymbol{\eta}_a + \delta\boldsymbol{g} \end{gather}\)$ 离散时间ESKF运动学方程 上面给出的是连续时间下的状态方程，但是计算机只能处理离散数据，而如果进行数值近似的话会导致计算量的暴增，因此需要转换为离散时间下的状态方程，很容易可以得出名义状态变量的离散时间方程： $\(\begin{split} \boldsymbol{p}(t + \Delta t) &= \boldsymbol{p}(t) + \boldsymbol{v}\Delta t + \frac{1}{2} (\boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a)) \Delta t^2 + \frac{1}{2} \boldsymbol{g} \Delta t^2 \\ \boldsymbol{v}(t + \Delta t) &= \boldsymbol{v}(t) + \boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a) \Delta t + \boldsymbol{g} \Delta t \\ \boldsymbol{R}(t + \Delta t) &= \boldsymbol{R}(t) \text{Exp}\left((\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_g)\Delta t\right) \\ \boldsymbol{b}_g(t + \Delta t) &= \boldsymbol{b}_g(t) \\ \boldsymbol{b}_a(t + \Delta t) &= \boldsymbol{b}_a(t) \\ \boldsymbol{g}(t + \Delta t) &= \boldsymbol{g}(t) \end{split}\)$ 该式只需在上面的基础上添加零偏项与重力项即可。而误差状态的离散形式则只需要处理连续形式中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为： $\(\begin{split} \delta\boldsymbol{p}(t + \Delta t) &= \delta\boldsymbol{p} + \delta\boldsymbol{v} \Delta t \\ \delta\boldsymbol{v}(t + \Delta t) &= \delta\boldsymbol{v} + \left( -\boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a)^\wedge \delta\boldsymbol{\theta} - \boldsymbol{R}\delta\boldsymbol{b}_a + \delta\boldsymbol{g} \right) \Delta t + \boldsymbol{\eta}_v \\ \delta\boldsymbol{\theta}(t + \Delta t) &= \text{Exp}\left( -(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_g)\Delta t \right) \delta\boldsymbol{\theta} - \delta\boldsymbol{b}_g \Delta t - \boldsymbol{\eta}_\theta \\ \delta\boldsymbol{b}_g(t + \Delta t) &= \delta\boldsymbol{b}_g + \boldsymbol{\eta}_g \\ \delta\boldsymbol{b}_a(t + \Delta t) &= \delta\boldsymbol{b}_a + \boldsymbol{\eta}_a \\ \delta\boldsymbol{g}(t + \Delta t) &= \delta\boldsymbol{g} \end{split}\)$ 注意： 1. 右侧部分我们省略了括号里的 $\(t\)$以简化公式； 2. 关于旋转部分的积分，我们可以将连续形式看成关于 $\(\delta \boldsymbol\theta\)$ 的微分方程然后求解。求解过程类似于对角速度进行积分。 3. 噪声项并不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下： $\(\sigma(\boldsymbol{\eta}_v) = \sqrt{\Delta t} \sigma_{a} \quad \sigma(\boldsymbol{\eta}_\theta) = \sqrt{\Delta t} \sigma_{g} \quad \sigma(\boldsymbol{\eta}_g) = \sqrt{\Delta t} \sigma_{bg} \quad \sigma(\boldsymbol{\eta}_a) = \sqrt{\Delta t} \sigma_{ba}\)$ 其中前两式的 $\(\Delta t\)$ 是由积分关系导致的。 至此，我们给出了如何在ESKF中进行IMU递推的过程，对应于卡尔曼滤波器中的状态方程。为了让滤波器收敛，我们通常需要外部的观测来对卡尔曼滤波器进行修正，也就是所谓的组合导航。当然，组合导航的方法有很多，从传统的EKF，到本节介绍的ESKF，以及后续章节将要介绍预积分和图优化技术，都可以应用于组合导航中。 运动过程 根据上述讨论，我们可以写出ESKF的运动过程。误差状态变量 $\(\delta \boldsymbol x\)$ 的离散时间运动方程已经在上式给出，我们可以整体地记为 $\(\delta\boldsymbol{x} = f(\delta\boldsymbol{x}) + \boldsymbol{w}, \quad \boldsymbol{w} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q})\)$ 其中 $\(\boldsymbol w\)$ 为噪声。按照前面的定义，$\(\boldsymbol Q\)$ 应该为： $\(\boldsymbol{Q} = \text{diag}(\boldsymbol{0}_3, \text{Cov}(\boldsymbol{\eta}_v), \text{Cov}(\boldsymbol{\eta}_\theta), \text{Cov}(\boldsymbol{\eta}_g), \text{Cov}(\boldsymbol{\eta}_a), \boldsymbol{0}_3)\)$ 两侧的零是由于第一个和最后一个方程本身没有噪声导致的。 为了保持与EKF的符号统一，我们计算运动方程的线性化形式： $\(\delta\boldsymbol{x} = \boldsymbol{F}\delta\boldsymbol{x} + \boldsymbol{w}\)$ 其中 $\(\boldsymbol F\)$ 为线性化后的雅可比矩阵。由于我们列写的运动方程已经是线性化的了，只需把它们的线性系统拿出来即可 $\(\boldsymbol{F} = \begin{bmatrix} \boldsymbol{I} & \boldsymbol{I}\Delta t & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} \\\boldsymbol{0} & \boldsymbol{I} & -\boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a)^\wedge \Delta t & -\boldsymbol{R}\Delta t & \boldsymbol{0} & \boldsymbol{I}\Delta t \\ \boldsymbol{0} & \boldsymbol{0} & \text{Exp}(-(\tilde{\boldsymbol{\omega}} - \boldsymbol{b}_g)\Delta t) & \boldsymbol{0} & -\boldsymbol{I}\Delta t & \boldsymbol{0} \\\boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} & \boldsymbol{0} & \boldsymbol{0} \\\boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} & \boldsymbol{0} \\\boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} \end{bmatrix}\)$ 在此基础上，我们执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU积分）以及对误差状态的预测： $\(\delta\boldsymbol{x}_{\text{pred}} = \boldsymbol{F}\delta\boldsymbol{x} \\\boldsymbol{P}_{\text{pred}} = \boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T + \boldsymbol{Q}\)$ 不过由于ESKF的误差状态在每次更新以后会被重置，因此运动方程的均值部分没有太大意义，而方差部分则可以指导整个误差估计的分布情况 ESKF的更新过程 前面介绍的是ESKF的运动过程，现在我们来考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的 $\(h\)$ ，那么可以写为： $\(\boldsymbol{z} = h(\boldsymbol{x}) + \boldsymbol{v}, \quad \boldsymbol{v} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{V})\)$ 其中 $\(\boldsymbol{z}\)$ 为观测数据， $\(\boldsymbol{v}\)$为观测噪声， $\(\boldsymbol{V}\)$为该噪声的协方差矩阵。由于状态变量里已经有 $\(\boldsymbol{R}\)$ 了，这里我们换个符号。 在传统EKF中，我们可以直观对观测方程线性化，求出观测方程相对于状态变量的雅可比矩阵，进而更新卡尔曼滤波器。而在ESKF中，我们当前拥有名义状态 $\(\boldsymbol{x}\)$ 的估计以及误差状态 $\(\delta\boldsymbol{x}\)$ 的估计，且希望更新的是误差状态，因此要计算观测方程相比于误差状态的雅可比矩阵： $$\boldsymbol{H} = \frac{\partial h}{\partial \delta \boldsymbol{x}} $$ 然后再计算卡尔曼增益，进而计算误差状态的更新过程： $$\boldsymbol{K} = \boldsymbol{P}{\text{pred}} \boldsymbol{H}^T (\boldsymbol{H} \boldsymbol{P} \}} \boldsymbol{H}^T + \boldsymbol{V})^{-1

\delta \boldsymbol{x} = \boldsymbol{K}(\boldsymbol{z} - h(\hat{\boldsymbol{x}})) \ \boldsymbol{P} = (\boldsymbol{I} - \boldsymbol{K} \boldsymbol{H}) \boldsymbol{P}_{\text{pred}} \

$$ 其中 $\(\boldsymbol{K}\)$ 为卡尔曼增益，$\(\boldsymbol{P}_{pred}\)$ 为预测的协方差矩阵，最后的 $\(\boldsymbol{P}\)$为修正后的协方差矩阵。这里的 $\(\boldsymbol{H}\)$的计算可以通过链式法则来生成： $\(\boldsymbol{H} = \frac{\partial h}{\partial \boldsymbol{x}} \frac{\partial \boldsymbol{x}}{\partial \delta \boldsymbol{x}}\)$ 其中第一项只需对观测方程进行线性化，第二项，根据我们之前对状态变量的定义，可以得到： $\(\frac{\partial \boldsymbol{x}}{\partial \delta \boldsymbol{x}} = \text{diag}\left(\boldsymbol{I}_3, \boldsymbol{I}_3, \frac{\partial \log(\boldsymbol{R} \exp(\delta\boldsymbol{\theta}))}{\partial \delta \boldsymbol{\theta}}, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3\right)\)$ 其他几种都是平凡的，只有旋转部分，因为 $\(\delta \boldsymbol \theta\)$ 定义为 $\(\boldsymbol R\)$ 的右乘，我们用右乘的BCH即可： $$ \frac{\partial \log(\boldsymbol{R} \exp(\delta\boldsymbol{\theta}))}{\partial \delta \boldsymbol{\theta}}\bigg|_{\delta\boldsymbol{\theta}=0} = \boldsymbol{J}_r^{-1}(\log(\boldsymbol{R}))$$ 最后，我们可以给每个变量加下标 k，表示在 k 时刻进行状态估计。 ESKF的误差状态后续处理 在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需把误差状态归入名义状态，然后重置ESKF即可。归入部分可以简单地写为： $\(\begin{split} \boldsymbol{p}_{k+1} &= \boldsymbol{p}_k + \delta\boldsymbol{p}_k \\ \boldsymbol{v}_{k+1} &= \boldsymbol{v}_k + \delta\boldsymbol{v}_k \\ \boldsymbol{R}_{k+1} &= \boldsymbol{R}_k \text{Exp}(\delta\boldsymbol{\theta}_k) \\ \boldsymbol{b}_{g,k+1} &= \boldsymbol{b}_{g,k} + \delta\boldsymbol{b}_{g,k} \\ \boldsymbol{b}_{a,k+1} &= \boldsymbol{b}_{a,k} + \delta\boldsymbol{b}_{a,k} \\ \boldsymbol{g}_{k+1} &= \boldsymbol{g}_k + \delta\boldsymbol{g}_k \end{split}\)$ 有些文献如FAST-LIO里也会定义为广义的状态变量加法 $\(\boldsymbol{x}_{k+1} = \boldsymbol{x}_k \oplus \delta\boldsymbol{x}_k\\ \boldsymbol{x}_{k+1} = \boldsymbol{x}_k \boxplus \delta\boldsymbol{x}_k\)$ 这种写法可以简化整体的表达式。不过，如果公式里出现太多的广义加减法，可能让人不好马上辨认它们的具体含义，所以本书还是倾向于将各状态分别写开，或者直接用加法而非广义加法符号。 ESKF的重置分为均值部分和协方差部分。均值部分可以简单地实现为： $\(\delta \boldsymbol x=0\)$ 由于均值被重置了，之前我们描述的是关于 $\(\boldsymbol x_k\)$ 切空间中的协方差，而现在描述的是 $\(\boldsymbol x_{k+1}\)$ 中的协方差。这次重置会带来一些微小的差异，主要影响旋转部分。事实上，在重置前，卡尔曼滤波器刻画了 $\(\boldsymbol x_{pred}\)$ 切空间处的一个高斯分布 $\(\mathcal{N}(\delta \boldsymbol x,\boldsymbol P)\)$，而重置之后，应该刻画 $\(\boldsymbol{x}_{pred} \boxplus \delta\boldsymbol{x}_k\)$ 处的一个 $\(\mathcal{N}(0,\boldsymbol P_{reset})\)$。 我们设重置前的名义旋转估计为 $\(\boldsymbol R_k\)$，误差状态为 $\(\delta \boldsymbol \theta\)$，卡尔曼滤波器的增量计算结果为 $\(\delta \boldsymbol \theta_k\)$，注意此处 $\(\delta \boldsymbol \theta_k\)$ 是已知的，而 $\(\delta \boldsymbol \theta\)$ 是一个随机变量。重置之后的名义旋转部分为 $\(\boldsymbol R_k \text{Exp}(\delta\boldsymbol{\theta}_k) = \boldsymbol{R}^+\)$，误差状态为 $\(\delta\boldsymbol{\theta}^+\)$。由于误差状态被重置了，显然此时 $\(\delta\boldsymbol{\theta}^+=0\)$。但我们关心的并不是它们直接的取值，而是 $\(\delta\boldsymbol{\theta}^+\)$ 与 $\(\delta\boldsymbol{\theta}\)$ 的线性化关系。把实际的重置过程写出来： $\(\boldsymbol{R}^+\text{Exp}(\delta\boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta\boldsymbol{\theta}_k) \text{Exp}(\delta\boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta\boldsymbol{\theta})\)$ 不难得到 $\(\text{Exp}(\delta\boldsymbol{\theta}^+) = \text{Exp}(-\delta\boldsymbol{\theta}_k) \text{Exp}(\delta\boldsymbol{\theta})\)$ 注意这里 $\(\delta \boldsymbol \theta\)$ 为小量，利用线性化后的BCH公式，可以得到： $\(\delta\boldsymbol{\theta}^+ = -\delta\boldsymbol{\theta}_k + \delta\boldsymbol{\theta} - \frac{1}{2} \delta\boldsymbol{\theta}_k^\wedge \delta\boldsymbol{\theta} + o((\delta\boldsymbol{\theta})^2)\)$ 于是有 $\(\frac{\partial \delta\boldsymbol{\theta}^+}{\partial \delta\boldsymbol{\theta}} \approx \boldsymbol{I} - \frac{1}{2} \delta\boldsymbol{\theta}_k^\wedge\)$ 该式表明重置前后的误差状态相差一个旋转方面的小雅可比矩阵，我们记作 $\(\boldsymbol{J}_\theta = \boldsymbol{I} - \frac{1}{2} \delta\boldsymbol{\theta}_k^\wedge\)$ 。把这个小雅可比阵放到整个状态变量维度下，并保持其他部分为单位矩阵，可以得到一个完整的雅可比阵： $\(\boldsymbol{J}_k = \text{diag}(\boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{J}_\theta, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3)\)$ 因此，在把误差状态的均值归零同时，它们的协方差矩阵也应该进行线性变换： $\(\boldsymbol{P}_{\text{reset}} = \boldsymbol{J}_k \boldsymbol{P} \boldsymbol{J}_k^T\)$ 不过，由于 $\(\delta \boldsymbol \theta_k\)$ 并不大，这里的 $\(\boldsymbol J_k\)$ 仍然十分接近于单位矩阵，所以大部分材料里并不处理这一项，而是直接把前面估计的 $\(\boldsymbol P\)$ 阵作为下一时刻的起点。但本书仍然要介绍这一点，并且会在后面第9章中继续讨论这个问题。该问题实际意义是做了切空间投影，即把一个切空间中的高斯分布投影到另一个切空间中。在ESKF中，两者没有明显差异，但后文的迭代卡尔曼滤波器还牵扯到多次切空间的变换，我们必须在此加以介绍。 非线性优化 在SLAM中，经常性会碰到各种优化问题，比如说给一个目标函数，求出使其最小化的解，并且这个目标函数往往是非线性的 当然，目前很多库，比如说Ceres、g2o还有gtsam等库都可以实现非线性优化具体求解操作，我们只需要把待求解函数输入即可 概述 在三维世界中，可以通过旋转向量、旋转矩阵、欧拉角和四元数等等方法来描述刚体的运动，并且可以通过李群李代数来进行优化，此外通过相机进行观测世界，但是回归最初的问题，也就是位姿方程和观测方程，其中位姿可以使用变换矩阵描述，然后使用李代数进行优化，观测方程由相机成像模型给出 [图片] 但是由于噪声的存在，上述等式无法准确成立，则在给定模型和具体观测的时候，需要进行优化，并且是非线性优化 首先可以考虑噪声模型，认为噪声服从正态分布，也即 [图片] 并且，其他的变量可以做以下定义： 位姿变量： [图片]

路标点： [图片] 在 [图片] 处对路标 [图片] 进行了一次观测，对应到图像上像素位置 [图片] [图片] 然后我们得到了带有噪声的观测数据和传感器数据，就可以用来估计位姿 [图片] 和地图路标点 [图片] ，具体方法可以使用滤波器（卡尔曼滤波）和非线性优化 从概率角度，所有待求解的量称为状态变量 [图片] 状态估计等同于求已知输入数据 [图片] 和观测数据 [图片] 的条件下，状态 [图片] 的条件概率分布 [图片] 先考虑特殊情况，也就是没有运动测量的传感器，只有观测数据 [图片] 数学定义 对于一个函数 $$ f : F \subseteq R^n $$ - 连续：$\(f\)$ 在每一点都连续 - 连续可微：在每一点 $$ x \in F $$ 处，每一个偏导数 $$ \frac{\partial f(x)}{\partial x_i} $$ 存在且连续 - 二次连续可微：在每一点 $$ x \in F $$ 处，每一个偏导数 $$ \frac{\partial^2 f(x)}{\partial x_i \partial y_j} $$ 存在且连续 如果函数为一阶连续可微的，则 - 梯度(grad)：$$ \nabla f(x) = \left( \frac{\partial f(x)}{\partial x_1}, \frac{\partial f(x)}{\partial x_2}, \cdots, \frac{\partial f(x)}{\partial x_n} \right)^T $$ - 海森阵(Hessian)，其总为对称阵： $\(\nabla^2 f(x) = \begin{pmatrix} \frac{\partial^2 f(x)}{\partial x_1^2} & \frac{\partial f^2(x)}{\partial x_1 \partial x_2} & \cdots & \frac{\partial^2 f(x)}{\partial x_1 \partial x_n} \\[0.5em] \frac{\partial^2 f(x)}{\partial x_2 \partial x_1} & \frac{\partial f^2(x)}{\partial x_2 \partial x_2} & \cdots & \frac{\partial^2 f(x)}{\partial x_2 \partial x_n} \\[0.5em] \vdots & \vdots & \ddots & \vdots \\[0.5em] \frac{\partial^2 f(x)}{\partial x_n \partial x_1} & \frac{\partial f^2(x)}{\partial x_n \partial x_2} & \cdots & \frac{\partial^2 f(x)}{\partial x_n \partial x_n} \end{pmatrix}_{n \times n} = \left( \frac{\partial^2 f(x)}{\partial x_i \partial x_j} \right)_{n \times n}\)$

注：标量函数的 Hessian 矩阵始终是 \(n \times n\) 的方阵。

二次函数 $\(f(x) = \frac{1}{2}x^TAx + b^Tx + c\)$ ，其中 $$ A \in R^{n \times n} $\(（对称矩阵），\)$ b \in R^n $\(，\)$ c \in R$$（标量），则有 $\(\nabla f(x) = Ax + b\\[0.5em] \nabla^2 f(x) = A\\\)$ - 若 $$ F $$ 为多变量矩阵，则其一阶导雅可比阵(Jacobi)为 $\(F'(x) = \begin{pmatrix} \frac{\partial F_1(x)}{\partial x_1} & \frac{\partial F_1(x)}{\partial x_2} & \cdots & \frac{\partial F_1(x)}{\partial x_n} \\[0.5em] \frac{\partial F_2(x)}{\partial x_1} & \frac{\partial F_2(x)}{\partial x_2} & \cdots & \frac{\partial F_2(x)}{\partial x_n} \\[0.5em] \vdots & \vdots & \ddots & \vdots \\[0.5em] \frac{\partial F_n(x)}{\partial x_1} & \frac{\partial F_n(x)}{\partial x_2} & \cdots & \frac{\partial F_n(x)}{\partial x_n} \end{pmatrix}_{m \times n} = \left( \frac{\partial F_i(x)}{\partial x_j} \right)_{m \times n}\)$ 如多变量函数 $$ F(x) = Ax $$ 则其 Jacobi 为 $$ F'(x) = A $$ 先验、后验、似然 后验（知果求因） 假设，隔壁小哥要去15公里外的一个公园，他可以选择步行走路，骑自行车或者开车，然后通过其中一种方式花了一段时间到达公园。这件事中采用哪种交通方式是因，花了多长时间是果。 假设我们已经知道小哥花了1个小时到了公园，那么你猜他是怎么去的(走路or开车or自行车)，事实上我们不能百分百确定他的交通方式，我们正常人的思路是他很大可能是骑车过去的，当然也不排除开车过去却由于堵车严重花了很长时间。 这种预先已知结果(路上花的时间，在机器学习中就是观测到的X)，然后根据结果估计原因(交通方式)的概率分布即后验概率。 先验（由历史求因） 假设隔壁小哥还没去,我们根据他的个人历史习惯来推测他会以哪种方式出行。 假设我们比较了解小哥的个人习惯，小哥是个健身爱好者就喜欢跑步运动，这个时候我们可以猜测他更可能倾向于走路过去。当然我的隔壁小哥是个大死肥宅，这个时候我们猜测他更可能倾向于坐车，连骑自行车的可能性都不大。 这个情景中隔壁小哥的交通工具选择与花费时间不再相关。因为我们是在结果发生前就开始猜的，根据历史规律确定原因(交通方式)的概率分布，即先验概率。 似然估计（知因求果） 换个情景，先考虑小哥去公园的交通方式。 假设隔壁小哥步行走路去，一般情况下小哥大概要用2个小时;假设小哥决定开车，到公园半个小时是非常可能的。 这种先定下来原因，根据原因(出行方式)来估计结果的概率分布即似然估计， 根据原因来统计各种可能结果的概率即似然函数。 状态估计 [图片] 直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，是可行的。 最大后验估计（MAP）：求一个状态最优估计，使得在该状态下后验概率最大化 [图片] 最大似然估计（MLE）：在哪种状态下，最容易产生当前的观测 [图片] 最小二乘的引出 由观测方程和噪声服从高斯分布可知 [图片] 高维的高斯分布： [图片] 则概率密度为 [图片] 对其取负对数，则有 [图片] 其中，左边两项是固定的值，也就是说当最小化 [图片] 时，只与最右侧项有关 这样原问题的最大化，相当于负对数的最小化，进一步，最小化右侧二次型项，就得到了对状态的最大似然估计，也就是最小二乘的问题 [图片] 考虑实际中批量处理数据时，由于各个时刻的输入和观测相互独立，数学语言就是 [图片] 对数据的最小化估计误差也就是最大似然估计 [图片] 由SLAM的状态估计，得到了最小二乘问题：最优解等价于状态的最小二乘问题 [图片] ，其由许多个误差的平方和组成。虽然整体维度较高，但是每个项很简单，仅与一两个状态变量有关(比如运动误差只与 [图片] 有关,观测误差只与 [图片] 有关) 如果用李代数表达位姿，则是无约束优化问题，那么如何求解此类的非线性最小二乘问题？ 非线性最小二乘法 先考虑最简单的问题： [图片] 其中 [图片] 是任意的标量非线性函数， [图片] 可以是向量 当 [图片] 很简单的时候，令目标函数的导数为零，就可以求解最小值，从一系列极值中筛选 当 [图片] 很复杂的时候，就难以使用这种方法求解，所以需要一种迭代的方法求解，从一个初始值出发，不断优化当前的优化变量，使得目标函数下降，直到直到某个时刻增量非常小，无法使函数下降，则算法收敛，目标达到了个极小，也就完成寻找极小值的过程。但是迭代过程中的增量如何寻找呢？这就需要使用梯度法了。 [图片] [图片] [图片] [图片] [图片] [图片] [图片] [图片] 视觉里程计 特征点提取与匹配概述 现在开始就是SLAM系统的重要算法，其中视觉里程计方法就是对图像进行特征提取与匹配，计算出来两帧之间相机的位姿变换等，这里会涉及特征点法的使用，主要是有如下几点需要注意 - 理解图像特征点的意义，掌握在单幅图像中提取特征点及多幅图像中匹配特征点的方法 - 理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运动 - 理解如何通过三角化，获得二维图像上对应的三维结构 - 理解PNP问题，及利用已知三维结构与图像的对应关系，求解摄像机的三维运动 - 理解ICP问题，及利用点云的匹配关系，求解摄像机的三维运动 经典SLAM模型中以相机位姿-路标来描述SLAM过程，路标是三维空间中固定不变的点，可以在特定位姿下观测到，在视觉SLAM中，可利用图像特征点作为SLAM中的路标 特征点是图像当中具有代表性的部分，如轮点，较暗区域中的亮点较亮区域中的暗点等。特征点应该有如下特点，以便于快速准确的进行匹配 - 可重复性：相同的区域可以在不同的图像中找到 - 可区别性：不同的区域有不同的表达 - 高效率：同一图像中，特征点的数量应该小于像素的位置 - 本地性：特征仅与一小片图像区域有关 特征点的信息有关键点和描述子两部分，关键点指的是位置、大小、方向、评分等，描述子是特征点周围的图像信息，如：当谈论在一张图像中计算SIFT特征时，是指“提取SIFT关键点并计算SIFT描述子”。 ORB特征 ORB特征是一种更为高效的方法，其来源于著名的VSLAM框架ORB-SLAM，关键点是Oriented FAST(一种改进的FAST角点)，描述子是改进 BRIEF 关键点 FAST：主要检测局部像素灰度变化明显的地方(如果一个像素与领域的像素差别较大，则更可能是角点) 1. 在图像中选取像素 [图片] 1. ，假设它的亮度为 [图片] 1. 设置一个阈值 [图片] 1. （比如 [图片] 1. 的20%）。 2. 以像素 [图片] 1. 为中心,选取半径为3的圆上的16个像素点。 2. 假如选取的圆上，有连续的N个点的亮度大于 [图片] 1. 或小于 [图片] 1. ，那么像素 [图片] 1. 可以被认为是特征点(N通常取12，即为FAST-12。其它常用的N取值为9和11，他们分别被称为 FAST-9，FAST-11)。 2. 循环以上四步，对每一个像素执行相同的操作。 在FAST12中，提出一个高效的测试，来快速排除一大部分非特征点的点。该测试仅仅检查在位置1、5、9、13四个位置的像素，如果不满足至少三个角点亮度大于 [图片] 或小于 [图片] ，那么 [图片] 不可能是一个角点。 [图片] 当然FAST也有缺点，原始FAST角点经常出现扎堆的现象(分布不均匀)。所以在第一遍检测之后，还需要用非极大值抑制，在一定区域内仅保留响应极大值的角点，避免角点集中问题。 由干FAST角点不具有方向信息且存在尺度问题，ORB添加了尺度和旋转的描述：尺度不变性通过构建图像金字塔来实现，旋转是由灰度质心法来实现。 [图片] [图片] 描述子 然后描述子部分是BRIEF，一种二进制描述子，其描述向量由许多个0和1组成，这些描述了周围128个点的亮度值，或者说对比了周围两个像素点的亮度值 [图片] 实际上操作方法如下多种，只不过工程上经常性使用第二种方法： 1. 在图像块内平均采样 [图片] 1. 和 [图片] 1. 都符合 [图片] 1. 的高斯分布，或者也有这种情况： [图片] 1. 符合 [图片] 1. 的高斯分布，而 [图片] 1. 符合 [图片] 1. 的高斯分布 2. 在空间量化极坐标下的离散位置随机采样 3. 把 [图片] 1. 固定为 [图片] 1. ， [图片] 1. 在周围平均采样（实际上通过预先定义好的位置采用） 总结：描述子:BRIEF总结 1. 优点：BRIEF使用了随机选点的比较，速度比较快，而且由于使用了二进制表达，存储起来也十分方便。 2. 缺点：原始的BRIEF描述子不具有旋转不变性，在图像发生旋转时容易走失。而ORB在FAST特征点提取阶段计算了关键点的方向，计算了旋转之后的“BRIEF”特征使ORB的描述子具有较好的旋转不变性。 3. 注意: BRIEF是一种二进制描述，需要用汉明距离度量 ORB流程 ORB的流程如下图所示， 1. 根据改进特征点法计算出来特征关键点（这里是篮球的中心点）为圆心（几何质心），然后以 [图片] 1. 为半径作圆 2. 计算质心，得到特征点方向（绿色箭头） 3. 选取某一个模式下的pq点对，书上代码是128对，这里为了便于表示只选取2对 4. 根据特征点方向，将其旋转到与 [图片] 1. 轴平行，并且对所有的pq点对进行旋转（这里利用了特征点的方向性和旋转不变性），这样，这一帧的特征点就可以跟上一帧的特征点匹配上 2. 按照规则对比所有的pq点对的光照强度或者灰度值 3. 按照顺序排列描述子 [图片] 特征匹配 特征匹配解决了SLAM中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。 通过对图像与图像或者图像与地图之间的描述子进行准确匹配，可以为后续的姿态估计、优化等操作减轻大量负担；然而，由于图像特征的局部特性，误匹配的情况存在。 考虑两个时刻的图像： 1. 在图像 [图片] 1. 中提取到特征点 [图片] 1. ；在图像 [图片] 1. 中提取到特征点 [图片] 1. 暴力匹配：对每一个特征点 [图片] 1. 与所有的 [图片] 1. 测量描述子的距离，然后排序，取最近的一个作为匹配点 2. 当特征点数量很大时，暴力匹配的运算量会变得很大，选用一些改进算法，如：快速近似最近邻(FLANN)算法 在进行了特征点匹配方法之后，就可以进一步计算相机的运动了 计算相机运动 计算有三种情况，也就是在输入数据是二维图像还是三维点云的情况下，如何进行计算 - 如果只有两个单目图像，得到2D-2D间的关系(对极几何) - 得到一些3D点和2维图像投影，即得到3D-2D间的关系(PNP) - 当相机为双目、RGB-D，或者通过某种方法得到距离信息得到3D-3D间的关系(ICP) 单目图像——对极几何 已知的信息如下： - 三维空间点 [图片] - 在像素坐标平面的投影点为 [图片] - 和 [图片] - （像素坐标）； [图片] - 的一个特征点 [图片] - 在 [图片] - 中对应着特征点 [图片] - (匹配正确的情况下)；相机内参 想求：第一帧到第二帧的相机运动为 [图片]

[图片] 上图中，基线是 [图片] ，极点是基线和像平面的交点 [图片] ，极平面是包含基线的平面（或者说，是 [图片] 的平面，三个点确定一个平面），极线就是极平面与图像平面的交线 单张图像只能确定 [图片] 在某个直线上，无法确定深度信息，但是两张图像就可以在有约束的情况下恢复 Docker镜像 为方便开发，本人自己制作了一个Dockerfile文件用于构建slam_ros1项目，提供文件如下，其中部署了很多主流SLAM算法，目前已部署的有 - FAST-LIVO2 - R3Live - FAST-LIO（simple版本） - Point-LIO 暂时无法在飞书文档外展示此内容 同时本人基于云原生方法，在国内服务器上构建了Docker镜像，大家可以直接自行拉取并运行，不必再自行构建，同时拉取速度十分有保障，拉取方法如下，同时提供镜像仓库地址：slam_ros1镜像仓库，在该仓库的制品一栏中可以看到对应的镜像 docker pull docker.cnb.cool/gpf2025/slam_ros1:v1 拉取下来之后就可以运行，关于docker的一些具体使用方法在网上有大量教程，因此在这里不在赘述，只提供一个运行指令并附详细解释，也可以根据自己的具体情况对指令进行修改 docker run -it --rm \ --memory=16g \ --memory-swap=20g \ --cpus=8 \ -e "DISPLAY=\(DISPLAY" \ -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ -v "\)HOME/.Xauthority:/root/.Xauthority:rw" \ -v "\((pwd)/c013_graco:/root/c013_graco" \ --gpus all \ -e "QT_X11_NO_MITSHM=1" \ -e "NVIDIA_DRIVER_CAPABILITIES=all" \ --network=host \ docker.cnb.cool/gpf2025/slam_ros1:v1 - -e "DISPLAY=\)DISPLAY"可视化用 - -v "/tmp/.X11-unix:/tmp/.X11-unix:rw"可视化用 - -v "\(HOME/.Xauthority:/root/.Xauthority:rw"可视化用 - -v "\)(pwd)/outdoor.bag:/root/slam_ws/rosbag.bag"将本地数据包挂载到镜像中使用，以冒号:作为区分，冒号之前的是本地文件路径，冒号后的是容器中文件挂载路径 - --gpus all使用主机GPU，如果主机无GPU可忽略 - -e "QT_X11_NO_MITSHM=1"可视化用 - -e "NVIDIA_DRIVER_CAPABILITIES=all"映射计算库（Compute）和绘图（Graphics/Display）进容器，这对 Rviz 正常工作至关重要，没有这一项的话Rviz会卡顿 - docker.cnb.cool/gpf2025/slam_ros1:v1要运行的镜像名 Cloud_regi因为Gazebo仿真器需要GPU支持才可以流畅运行，所以建议主机配有英伟达显卡并装好驱动，如果没有则会导致运行该镜像的Gazebo时非常卡顿 如果是虚拟机中运行该镜像，则将--gpus all去掉，否则可能报错，因为虚拟机无法使用主机的显卡 另注意，在启动镜像之前要在主机终端中输入指令：xhost + localhost，这个指令可以让容器中的GUI界面显示出来，如果不输入这个指令，容器在运行Rviz的时候可能就会报错并且无法显示 如果本地下载了rosbag，但是想在镜像中使用的话，就需要设置挂载，将本地文件挂载到镜像中，使其可以读取和使用 然后需要安装NVIDIA Container Toolkit，否则Docker中无法使用GPU进行计算，或者说无法访问主机GPU

添加密钥和仓库

curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list

安装

sudo apt-get update sudo apt-get install -y nvidia-container-toolkit

更新Docker配置

生成配置文件

sudo nvidia-ctk runtime configure --runtime=docker

重启Docker服务

sudo systemctl restart docker

导航小车仿真（ROS1版本） 直接运行官方的镜像，选择的依然是ROS1 Noetic，注意一下官方镜像直接进去的话是某个目录而非根目录，因此需要先输入一个cd切换到正常根目录 docker run -it --rm \ -e "DISPLAY=\(DISPLAY" \ -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ -v "\)HOME/.Xauthority:/root/.Xauthority:rw" \ --gpus all \ -e "QT_X11_NO_MITSHM=1" \ -e "NVIDIA_DRIVER_CAPABILITIES=all" \ --network=host \ ros:noetic-perception-focal 并且要注意一下，刚刚进去的镜像中不存在Git等，因此需要单独安装 apt update && apt install git wget 之后就是编译安装一系列的依赖，但是方法上需要注意一下，Sophus的编译安装得稍加处理 git clone https://github.com/Livox-SDK/Livox-SDK.git cd Livox-SDK cd build && cmake .. make make install

git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout a621ff perl -0777 -pi -e 's/SO2::SO2()\s*{\s*unit_complex_.real()\s*=\s*1.\s*;\s*unit_complex_.imag()\s*=\s*0.\s*;\s*}/SO2::SO2()\n{\n unit_complex_.real(1.);\n unit_complex_.imag(0.);\n}/s' sophus/so2.cpp

方法一

cmake -S . -B build \ -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_INSTALL_PREFIX=/usr/local cmake --build build -j cmake --install build 当然，Sophus的安装方法似乎有点迷，因为我曾碰到过明明编译安装成功但是最终编译整个工作空间的时候发现找不到Sophus，因此我给出了两种编译安装的方法，在一套方法无法成功的时候可以切换另一套，一般都可以解决

方法二

mkdir build && cd build cmake .. && make make install 然后开始构建一系列的代码，主要是包含Gazebo的仿真项目、Mid360激光雷达的插件、FAST-LIO算法的运行等等 mkdir -p slam_ws/src && cd slam_ws/src git clone https://github.com/blackcoffeerobotics/bcr_bot.git cd bcr_bot && git switch ros1 && cd .. git clone https://github.com/zlwang7/S-FAST_LIO.git git clone https://github.com/Livox-SDK/livox_ros_driver.git git clone https://github.com/fratopa/Mid360_simulation_plugin.git PS：如果容器中无法正常下载，那么可以考虑在主机中下载然后传入进去

然后刷新环境变量之后就可以开始运行了，首先是第一个指令，可以验证环境是否正常 roslaunch bcr_bot gazebo.launch 如果出现以下报错，就说明Gazebo没有成功显示，这主要是因为容器中的可视化界面想显示在主机上需要权限的，但是主机中并没有加入这个权限 [gazebo_gui-3] process has died [pid 29062, exit code 134, cmd /opt/ros/noetic/lib/gazebo_ros/gzclient __name:=gazebo_gui __log:=/root/.ros/log/ea21918a-f5ae-11f0-bc4c-a0ad9fd115f2/gazebo_gui-3.log]. log file: /root/.ros/log/ea21918a-f5ae-11f0-bc4c-a0ad9fd115f2/gazebo_gui-3*.log 所以需要单独开启一个主机的终端，输入指令xhost + 如果一切正常，应该会显示如下的界面，这就是成功运行了仿真，下一步就是进行各种修改 [图片] 添加Mid360插件仿真 目前Mid360及其他激光雷达是非常主流的雷达，也得益于FAST-LIO等算法的推广，因此在这里我们使用Mid360进行导航等，但是原始的Gazebo中并没有Mid系列的插件，就需要单独进行处理，所幸Github上有一个仿真插件，并且可以用于Noetic上，也就是Mid360_simulation_plugin仓库，只需要将其下载到工作空间中即可（之前已经下载了） 然后查看BCR Bot，因为已经切换到了ROS1分支，因此可以看到其中的gazebo.launch文件就是开启仿真的接口，深入挖掘可以发现其分为三部分内容：参数声明、仿真环境搭建与创建机器人，因此主要的修改就是针对机器人部分，这一部分的文件在bcr_bot_spawn.launch中，而第一部分的参数声明可以看到，其中定义了很多的使能选项，根据命名可以很容易理解就是是否开启2D雷达、单目相机、是否发布轮式里程计等，为了防止后面对三维雷达的干扰，因此统一设置为false 在bcr_bot_spawn.launch中可以看到，解析的目标文件是urdf/bcr_bot.xacro，这是机器人的最终主文件，将其打开可以发现，里面定义了机器人主要功能（也就是各种传感器），至于机器人的机身等定义在其他文件中，因此我们选择在其中添加自定义的Mid360传感器，效仿其他传感器的声明方式作如下声明

<xacro:if value="$(arg mid360_enabled)">

<!-- MID-360 main body link -->

<link name="mid360">
    <collision>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <!-- 外形仅用于碰撞/可视化，可自行改尺寸 -->
    <geometry>
        <cylinder length="0.06" radius="0.055"/>
    </geometry>
    </collision>

    <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
        <cylinder length="0.06" radius="0.055"/>
    </geometry>
    <material name="aluminium"/>
    </visual>

    <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <mass value="0.1"/>
    <xacro:cylinder_inertia m="0.1" r="0.055" h="0.06"/>
    </inertial>
</link>

<joint name="mid360_joint" type="fixed">
    <parent link="base_link"/>
    <child link="mid360"/>
    <!-- 安装高度按你车体改 -->
    <origin xyz="0 0 0.2" rpy="0 0 0" />
</joint>

<!-- Gazebo: MID-360 lidar sensor + plugin -->

<gazebo reference="mid360">
    <material>Gazebo/White</material>

    <sensor type="ray" name="laser_livox">
    <!-- 传感器相对 mid360 link 的位姿 -->
    <pose>0 0 0.05 0 0 0</pose>
    <visualize>true</visualize>
    <always_on>true</always_on>
    <update_rate>10</update_rate>

    <plugin name="gazebo_ros_laser_controller" filename="liblivox_laser_simulation.so">
        <ray>
        <scan>
            <horizontal>
            <samples>100</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926535897931</min_angle>
            <max_angle> 3.1415926535897931</max_angle>
            </horizontal>
            <vertical>
            <samples>50</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926535897931</min_angle>
            <max_angle> 3.1415926535897931</max_angle>
            </vertical>
        </scan>

        <range>
            <min>0.1</min>
            <max>40</max>
            <resolution>1</resolution>
        </range>

        <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.03</stddev>
        </noise>
        </ray>

        <visualize>false</visualize>
        <samples>20000</samples>
        <downsample>1</downsample>

        <csv_file_name>mid360-real-centr.csv</csv_file_name>
        <!-- 2: sensor_msgs/PointCloud2 | 3: livox_ros_driver/CustomMsg -->
        <publish_pointcloud_type>2</publish_pointcloud_type>

        <!-- 这里沿用你之前的“绝对 topic”风格 -->
        <ros_topic>/livox/lidar</ros_topic>

        <!-- 建议用雷达自身 frame -->
        <frameName>mid360</frameName>
    </plugin>
    </sensor>
</gazebo>

<!-- Gazebo: IMU sensor + GazeboRosImuSensor plugin -->

<gazebo reference="mid360">
    <material>Gazebo/White</material>

    <sensor type="imu" name="mid360_imu">
    <always_on>true</always_on>
    <visualize>false</visualize>
    <update_rate>200</update_rate>

    <plugin name="mid360_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <!-- 设为空可得到根命名空间 “/”，从而发布到 /livox/imu -->
        <robotNamespace></robotNamespace>

        <!-- 注意：不要用前导 /，插件内部会拼接命名空间 -->
        <topicName>livox/imu</topicName>

        <!-- 必填：缺失会直接失败 -->
        <frameName>mid360</frameName>

        <updateRateHZ>200</updateRateHZ>
        <gaussianNoise>0.0002</gaussianNoise>

        <!-- 可选：偏置/安装误差 -->
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
    </plugin>
    </sensor>
</gazebo>

</xacro:if>

内容很容易理解，就是在base_link坐标系下定义了一个mid360坐标系，然后构建了一个固定的关节，声明了位移关系（激光雷达在机身上面），然后构建一个圆柱状碰撞体，最后就是在mid360系上构建传感器插件了，包含了IMU和Mid360雷达插件，这里需要注意一下是雷达的消息类型，其中2和3表示两种类型 将上面的内容添加进去，然后在文件最顶部加入如下选项，就可以成功开启了Mid360仿真 最后将修改内容保存，然后重新开启仿真就可以看到成功有激光雷达的加入了，在Rviz或者其他内容中使用激光点云即可 坐标系构建 这里包含了一个ROS中的坐标变换问题，因为如果详细分析BCR的Xacro中的坐标系关系，或者运行仿真之后使用rosrun rqt_tf_tree rqt_tf_tree查看TF树，就会发现其中定义的坐标变换关系是base_footprint->base_link->mid360，其中根节点是base_footprint，原生的FAST-LIO输出的坐标变换是camera_init->body，在很多SLAM框架中，“camera_init” 通常可以理解为系统启动时建立的局部世界坐标系（世界/里程计的起点）：启动时第一帧（更准确说是初始化成功时刻）的位姿被当作原点与参考方向，之后滤波/前端输出的高频位姿都是相对于这个系来表达的。所以它在工程语义上非常接近 ROS 里的 odom（局部连续、可能漂移的里程计世界系），至于 body，通常就是滤波器状态所在的机体系（body frame），往往与 IMU 坐标系一致或非常接近 但是其中有一个问题，如果直接运行仿真和FAST-LIO，就会导致TF树的割裂——有不联通的节点或者说变成了两个TF树，这在ROS中是不被允许的，也无法作为后续导航的TF树，因此必须将一个完整的TF树构建出来，第一个思想就是，既然我仿真中构建的TF树存在mid360坐标系（雷达坐标系），FAST-LIO中的body系实际上也是雷达坐标系，那么我将仿真TF树中的mid360坐标系改名为body系或者将FAST-LIO中输出的body系改名为mid360系不就可以了，但是这会导致一个TF树的节点出现两个父节点，会导致TF树的断裂——具体会表现为，TF中的雷达系节点会FAST-LIO发布的坐标变换占据，也就是说这种方法是行不通的，因为每个TF树中的节点最多有一个父节点 实际上雷达坐标系和雷达的IMU坐标系不是一个概念，这是两个坐标系，但是一般会将点云变换到IMU系下，因此在这里直接称为雷达坐标系 考虑到不论是仿真还是现实中，雷达系都是固定在车上的，也就是说雷达系因此我只需要获取一次坐标变换关系，然后求逆变换，然后使用一个静态发布节点发布即可，这样就为base_footprint添加了一个body父节点，但是需要注意的是，不能直接如此发布变换，否则会导致TF树中出现环路，这也是不允许的，因此需要专门处理。 具体的方法是，创建一个body系（FAST-LIO生成）和mid360系（仿真模型文件中定义），这两个坐标系实际上都是雷达坐标系，相当于进行了复制，然后求出base_footprint->mid360的逆向变换，该变换也等价于body->base_footprint的变换，然后使用一个静态坐标发布者就可以进行发布，如此就可以串联起整个TF树 rosrun tf tf_echo base_footprint mid360 运行仿真之后，上述指令可以输出变换关系，其中有 - Translation: t = (tx, ty, tz) - Rotation (quaternion): q = (qx, qy, qz, qw) 然后使用下列代码计算出逆变换的参数 import numpy as np from math import * tx,ty,tz = 0.0,0.0,0.25 # <-- 改成 tf_echo 打印的 translation qx,qy,qz,qw = 0.0,0.0,0.0,1.0 # <-- 改成 tf_echo 打印的 quaternion

quaternion inverse (unit quaternion)

qinv = np.array([-qx,-qy,-qz,qw], dtype=float)

rotation matrix from quaternion

x,y,z,w = qinv R = np.array([ [1-2*(y*y+z*z), 2*(x*y - z*w), 2*(x*z + y*w)], [2*(x*y + z*w), 1-2*(x*x+z*z), 2*(y*z - x*w)], [2*(x*z - y*w), 2*(y*z + x*w), 1-2*(x*x+y*y)] ], dtype=float)

t = np.array([tx,ty,tz], dtype=float) tinv = -R.dot(t)

print("t_inv:", *tinv) print("q_inv:", *qinv) 然后在launch文件中添加下面的四元数版本静态变换发布即可 然后就可以获得下面的TF树，非常完整 [图片] 导航地图构建 FAST-LIO会输出点云地图，但是这种地图是三维的，如果想执行规划算法就必须使用三维规划算法，但是FAST-Planner等三维算法很难直接应用于地面机器人上，因此需要进行转换，转为二维栅格地图进行规划，先安装 apt install ros-noetic-octomap-server OctoMap是一个基于八叉树（octree）的概率 3D 占据地图框架：用“占据 / 空闲 / 未知”的概率（通常用 log-odds 实现）来表示空间，并且支持增量更新、动态扩展、多分辨率与相对紧凑的内存占用。官方介绍里强调了它的 3D 建模、可更新（概率融合）、多分辨率与高效存储等特性 关键是：它也会发布一个 2D 投影栅格，标准话题名就是 /projected_map（nav_msgs/OccupancyGrid），这个投影之后的就是可以用于二维导航的地图，其中的投影机制如下，分为两步 A. 先把 3D 点云融合为 3D Octree（占据体素） 1. TF 变换：把输入点云从其 frame_id 变换到 octomap_server 的 frame_id（你之前遇到 target_frame 不存在，就是这里失败）。TF 不对/目标坐标系不存在时，octomap 会“收得到点云但插不进去或不发布”。（同类问题在社区里经常被归因到全局 frame 与 TF 树不匹配。） 2. 点云预过滤：例如按高度裁剪。pointcloud_[min|max]_[x|y|z] 会在插入前剔除不在范围内的点。 3. 地面滤波（可选）：filter_ground 打开后，会尝试用 PCL 的平面分割检测地面并忽略（不作为障碍插入）。 4. 射线插入：用传感器模型（sensor_model/max_range 等）对 octree 进行 free/occupied 更新。 一个非常典型的“为什么没地图”的原因：pointcloud_min_z 和 pointcloud_max_z 设得不合理，把点全过滤掉了，导致 octree 为空（Nothing to publish / octree is empty）。社区的示例里就明确指出了这种情况。 B. 再把 3D Octree 沿 Z 方向“压扁”为 2D OccupancyGrid /projected_map 的本质是：选定一个 Z 范围（例如离地 -0.2 到 2.0m），把该高度带内所有 occupied 体素投影到 XY 平面网格上： - 某个 (x,y) 的竖直柱子里，只要出现“足够占据”的体素，就把该 2D cell 标为 occupied； - 未被观测/不确定的保持 unknown（或在下游 costmap 里按配置处理）； - 分辨率由 resolution 决定，越小越细，但 CPU/内存越吃紧。 具体的代码如下所示

<param name="frame_id" type="string" value="camera_init" />
<param name="base_frame_id" type="string" value="base_footprint" />

<param name="resolution" value="0.05" />

<param name="occupancy_min_z" value="-0.1" />
<param name="occupancy_max_z" value="0.5" />

<param name="sensor_model/max_range" value="50.0" />

<param name="sensor_model/hit" value="0.7" />
<param name="sensor_model/miss" value="0.4" />
<param name="sensor_model/min" value="0.12" />
<param name="sensor_model/max" value="0.97" />

<param name="latch" value="false" />

其中重要的参数解析如下： - frame_id: 这是建图的基准坐标系。Octomap 会把所有时刻进来的点云，全部转换到这个坐标系下，然后把“砖块”（体素）一个个堆在这个坐标系里。因此必须设置为你的全局坐标系，或者说它必须是绝对静止的（相对于世界），如 FastLIO 的 camera_init - base_frame_id: 是机器人的基准坐标系。Octomap 需要知道在每一帧数据采集时机器人在哪里，用于去除动态障碍和滤除地面 - cloud_in: Remap 到 FastLIO 输出的稠密点云话题（通常是 /cloud_registered 或 /Cloud_Map，取决于你是否开启了Dense Map - occupancy_min_z / max_z: 这是物理过滤的核心，决定了多高范围内的物体会被投影到 2D 地图中。 不过有一个点需要注意一下，在很多情况下，激光雷达是不会水平放置的，会存在不同程度的倾斜，如果SLAM算法中没有初始化过程，也就是没有使用重力方向进行对齐，那么仍然使用上述的方法进行转换的话，就会导致投影得到的栅格地图是斜的，在这种情况下就需要进行处理 处理方法很简单，设置一个新的全局固定坐标系即可，也就是可以设置一个odom坐标系，该坐标系与camera_init坐标系存在一个坐标变换关系，就类似于车身与雷达一样，然后在这个坐标系下进行投影即可

导航小车仿真ROS2版本 ROS1毕竟已经不再更新了，因此给出ROS2版本的实现，先搞一个镜像 docker pull osrf/ros:humble-desktop-full 然后运行 docker run -it --rm \ -e "DISPLAY=\(DISPLAY" \ -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ -v "\)HOME/.Xauthority:/root/.Xauthority:rw" \ --gpus all \ -e "QT_X11_NO_MITSHM=1" \ -e "NVIDIA_DRIVER_CAPABILITIES=all" \ --network=host \ osrf/ros:humble-desktop-full 进去之后就开始创建 cd && mkdir -p slam_ws/src cd slam_ws/src git clone https://github.com/blackcoffeerobotics/bcr_bot.git git clone https://github.com/stm32f303ret6/livox_laser_simulation_RO2.git git clone https://github.com/Livox-SDK/livox_ros_driver2.git git clone https://github.com/liangheming/FASTLIO2_ROS2.git

下面这些内容需要单独编译安装的

git clone https://github.com/borglab/gtsam.git git clone https://github.com/Livox-SDK/Livox-SDK2.git git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout 1.22.10 然后安装一些依赖 apt update apt install ros-humble-gazebo-ros-pkgs 第一步就是在仿真小车上添加Mid360插件，在这个仓库中，作者非常好的封装了一份插件，也就是只需要把Xacro的内容插入进去就可以使用

ros2 launch bcr_bot gazebo.launch.py \ camera_enabled:=True \ mid360_enabled:=True \ stereo_camera_enabled:=False \ position_x:=0.0 \ position_y:=0.0 \ orientation_yaw:=0.0 \ odometry_source:=world \ world_file:=small_warehouse.sdf \ robot_namespace:="bcr_bot"

---

理论补充与深化

SLAM 后端理论：图优化、Bundle Adjustment 与滑动窗口

本章定位：SLAM 系统的"大脑"——后端优化理论的完整推导与深入理解 前置知识：线性代数（矩阵分解、稀疏矩阵）、概率论（贝叶斯推断、高斯分布）、非线性优化（Gauss-Newton）

---

前置自测

📋 前置自测（答不出 ≥ 2 题 → 先回前置章节复习）

1. 给定联合概率 \(P(x, z)\)，如何写出 MAP 估计的数学形式？
2. 什么是 Jacobian 矩阵？它在非线性优化中起什么作用？
3. 稀疏矩阵的 Cholesky 分解与稠密矩阵有什么区别？为什么稀疏性重要？
4. 什么是 Schur complement？给定分块矩阵 \(\begin{bmatrix} A & B \\ C & D \end{bmatrix}\)，写出关于 \(D\) 的 Schur complement。
5. 信息矩阵（information matrix）和协方差矩阵是什么关系？

---

本章目标

学完本章后，你将能够： - 从概率图模型视角理解 SLAM：将 SLAM 问题建模为因子图上的贝叶斯推断 - 手推图优化的完整数学：从 MAP 到非线性最小二乘，再到稀疏线性系统的求解 - 理解 Bundle Adjustment 的核心：Schur complement 的推导、Arrow 稀疏结构、Gauge 自由度 - 掌握滑动窗口与边缘化理论：理解 FEJ 的必要性，以及 VINS-Mono 的实际策略

---

1. 图优化与因子图理论 ⭐⭐

本节解决的问题：如何将 SLAM 这个"同时定位与建图"的问题，转化为一个结构化的、可高效求解的数学优化问题？

1.1 概率图模型视角：SLAM as Bayesian Inference ⭐⭐

动机

SLAM 问题的本质是什么？机器人在未知环境中运动，每一时刻它获得两类信息：

1. 运动信息（里程计/IMU）：告诉机器人"我大概往哪个方向走了多远"
2. 观测信息（相机/激光雷达）：告诉机器人"我看到了环境中的某些特征"

这两类信息都带有噪声。SLAM 的目标是：在所有噪声观测的约束下，找到最可能的机器人轨迹和地图。

这正是贝叶斯推断的经典场景——我们有先验知识（运动模型），有观测数据（传感器数据），我们要找后验概率最大的状态。

数学建模

设机器人在时刻 \(t=1,2,\ldots,T\) 的位姿为 \(\mathbf{x} = \{x_1, x_2, \ldots, x_T\}\)，环境中的路标点为 \(\mathbf{l} = \{l_1, l_2, \ldots, l_M\}\)，所有观测数据为 \(\mathbf{z} = \{z_1, z_2, \ldots, z_K\}\)，所有控制输入为 \(\mathbf{u} = \{u_1, u_2, \ldots, u_{T-1}\}\)。

SLAM 就是求解后验概率：

\[ P(\mathbf{x}, \mathbf{l} \mid \mathbf{z}, \mathbf{u}) \]

根据贝叶斯定理：

\[ P(\mathbf{x}, \mathbf{l} \mid \mathbf{z}, \mathbf{u}) = \frac{P(\mathbf{z} \mid \mathbf{x}, \mathbf{l}) \cdot P(\mathbf{x}, \mathbf{l} \mid \mathbf{u})}{P(\mathbf{z} \mid \mathbf{u})} \]

这里有三个关键的问题需要回答：

问题 1：似然 \(P(\mathbf{z} \mid \mathbf{x}, \mathbf{l})\) 怎么分解？

假设各观测在给定状态时条件独立（这是 SLAM 中的标准假设，因为传感器噪声通常独立）：

\[ P(\mathbf{z} \mid \mathbf{x}, \mathbf{l}) = \prod_{k=1}^{K} P(z_k \mid x_{t_k}, l_{j_k}) \]

其中 \(z_k\) 是在位姿 \(x_{t_k}\) 处对路标 \(l_{j_k}\) 的观测。每个观测只涉及一个位姿和一个路标——这个"局部连接"性质是后面稀疏性的根源。

问题 2：先验 \(P(\mathbf{x}, \mathbf{l} \mid \mathbf{u})\) 怎么分解？

运动模型给出了相邻位姿之间的关系：

\[ P(\mathbf{x} \mid \mathbf{u}) = P(x_1) \prod_{t=1}^{T-1} P(x_{t+1} \mid x_t, u_t) \]

路标先验通常取均匀分布（无信息先验），可以忽略。

问题 3：分母 \(P(\mathbf{z} \mid \mathbf{u})\) 怎么处理？

分母是归一化常数，与待估计变量 \(\mathbf{x}, \mathbf{l}\) 无关。在求 MAP（Maximum A Posteriori）估计时，我们只需要最大化分子，分母可以忽略。

完整的后验分解

将上面的分析综合起来：

\[ P(\mathbf{x}, \mathbf{l} \mid \mathbf{z}, \mathbf{u}) \propto P(x_1) \prod_{t=1}^{T-1} P(x_{t+1} \mid x_t, u_t) \prod_{k=1}^{K} P(z_k \mid x_{t_k}, l_{j_k}) \]

关键观察：后验概率是多个"局部因子"的乘积，每个因子只涉及少数几个变量。这种乘积结构正是因子图的数学基础。

为什么不直接用滤波器？

传统的 EKF-SLAM 采用递推方式处理这个后验，每一时刻只维护当前状态的边缘后验 \(P(x_t, \mathbf{l} \mid z_{1:t}, u_{1:t})\)。这样做的根本问题是：

• 信息丢失：边缘化掉历史位姿时，丢失了重新线性化的机会。一旦某个线性化点不好，误差就永远被"锁定"了
• 计算爆炸：协方差矩阵是稠密的（维度 \((3+2M) \times (3+2M)\)），每次更新 \(O(M^2)\)
• 一致性问题：线性化点不断变化导致系统可观性被错误地提升，估计器变得过度自信

图优化（Smoothing）方法避免了这些问题：它保留所有变量，利用稀疏结构高效求解。

---

1.2 因子图：变量、因子与 SLAM 的映射 ⭐⭐

动机

上一节我们看到，SLAM 的后验概率是一堆局部因子的乘积。如何把这种数学结构可视化？如何利用这种结构进行高效计算？答案是**因子图（Factor Graph）**。

因子图的定义

因子图是一种二部图（bipartite graph），包含两类节点：

节点类型	符号	含义	图中表示
变量节点	圆形	待估计的状态变量	\(x_1, x_2, \ldots, l_1, l_2, \ldots\)
因子节点	方形	对变量的约束（概率因子）	\(f_1, f_2, \ldots\)

因子图表达的全局函数是所有因子的乘积：

\[ f(\mathbf{x}) = \prod_{i} f_i(\mathbf{x}_i) \]

其中 \(\mathbf{x}_i \subseteq \mathbf{x}\) 是因子 \(f_i\) 所连接的变量子集。每个因子只连接少数变量——这就是"局部性"。

SLAM 到因子图的映射

将 SLAM 问题映射为因子图：

SLAM 元素	因子图元素	数学形式
机器人位姿 \(x_t\)	变量节点	—
路标位置 \(l_j\)	变量节点	—
先验因子	一元因子，连接 \(x_1\)	\(f_0(x_1) = P(x_1)\)
里程计因子	二元因子，连接 \(x_t, x_{t+1}\)	\(f_t^{\text{odom}}(x_t, x_{t+1}) = P(x_{t+1} \mid x_t, u_t)\)
观测因子	二元因子，连接 \(x_t, l_j\)	\(f_k^{\text{obs}}(x_t, l_j) = P(z_k \mid x_t, l_j)\)
回环因子	二元因子，连接 \(x_i, x_j\)	\(f^{\text{loop}}(x_i, x_j) = P(z_{ij} \mid x_i, x_j)\)

可视化理解：想象一条链状结构，每个位姿节点通过里程计因子顺序连接（形成"骨架"），路标节点通过观测因子与对应位姿节点相连（形成"肋骨"），回环检测因子连接远距离的位姿节点（形成"交叉连接"）。

因子图 vs 贝叶斯网络 vs 马尔可夫随机场

很多初学者会混淆这三种图模型，它们的关系是：

图模型	边的含义	SLAM 是否适用	优势
贝叶斯网络	因果方向（有向）	可以，但推断复杂	因果关系清晰
马尔可夫随机场	无向依赖	可以	对称性好
因子图	变量-因子的二部图	最适合	直接表达乘积结构，与优化算法对接

因子图的关键优势在于：它直接对应于概率分布的乘积分解，而且每个因子自然对应优化问题中的一个残差项。

如果没有因子图会怎样？

如果不用因子图，我们需要直接操作联合概率分布。对于 \(N\) 个变量的联合分布，一般需要 \(O(\exp(N))\) 的空间来表示。因子图利用条件独立性，将联合分布分解为局部因子的乘积，大大降低了表示和计算的复杂度。

---

1.3 从 MAP 到非线性最小二乘：完整推导 ⭐⭐

动机

我们已经将 SLAM 建模为因子图上的概率推断问题。现在的问题是：如何实际求解这个推断问题？ 我们需要找到使后验概率最大的状态估计——这就是 MAP 估计。本节将展示一个优美的数学推导链：

\[ \text{MAP} \xrightarrow{\text{取对数}} \text{负对数似然最小化} \xrightarrow{\text{高斯假设}} \text{非线性最小二乘} \]

Step 1：MAP 估计的定义

MAP 估计就是求后验概率的极大值点：

\[ \mathbf{x}^* = \arg\max_{\mathbf{x}} \; P(\mathbf{x} \mid \mathbf{z}) \]

利用因子图的乘积结构：

\[ \mathbf{x}^* = \arg\max_{\mathbf{x}} \; \prod_{i} f_i(\mathbf{x}_i) \]

Step 2：取负对数

乘积的最大化等价于对数之和的最大化（因为对数是单调递增函数）：

\[ \mathbf{x}^* = \arg\max_{\mathbf{x}} \; \sum_{i} \ln f_i(\mathbf{x}_i) \]

等价于最小化负对数：

\[ \mathbf{x}^* = \arg\min_{\mathbf{x}} \; \sum_{i} \left[ -\ln f_i(\mathbf{x}_i) \right] \]

为什么要取对数？ 有两个原因：（1）将乘积转化为求和，求和在数学上和计算上都更容易处理；（2）避免多个小概率值相乘导致的数值下溢。

Step 3：引入高斯噪声假设

SLAM 中的标准假设是传感器噪声服从零均值高斯分布。对于一个观测因子：

\[ z_k = h_k(\mathbf{x}_k) + \eta_k, \quad \eta_k \sim \mathcal{N}(0, \Sigma_k) \]

其中 \(h_k(\cdot)\) 是观测函数，\(\Sigma_k\) 是噪声协方差矩阵。那么：

\[ P(z_k \mid \mathbf{x}_k) \propto \exp\left( -\frac{1}{2} \| z_k - h_k(\mathbf{x}_k) \|_{\Sigma_k}^2 \right) \]

这里 \(\| \mathbf{e} \|_{\Sigma}^2 = \mathbf{e}^T \Sigma^{-1} \mathbf{e}\) 是马氏距离的平方。取负对数：

\[ -\ln P(z_k \mid \mathbf{x}_k) = \frac{1}{2} \| z_k - h_k(\mathbf{x}_k) \|_{\Sigma_k}^2 + \text{const} \]

忽略与优化变量无关的常数项。

Step 4：定义残差

定义第 \(i\) 个因子的残差（residual）为：

\[ r_i(\mathbf{x}_i) = z_i - h_i(\mathbf{x}_i) \]

以及加权残差（通过对协方差矩阵做 Cholesky 分解 \(\Sigma_i = L_i L_i^T\)，定义 \(\Sigma_i^{-1/2} = L_i^{-T}\)）：

\[ e_i(\mathbf{x}_i) = \Sigma_i^{-1/2} r_i(\mathbf{x}_i) \]

则马氏距离变为简单的欧氏范数：

\[ \| r_i \|_{\Sigma_i}^2 = r_i^T \Sigma_i^{-1} r_i = \| e_i \|_2^2 \]

Step 5：最终的非线性最小二乘问题

将所有因子的贡献加起来：

\[ \boxed{ \mathbf{x}^* = \arg\min_{\mathbf{x}} \; \sum_{i} \| e_i(\mathbf{x}_i) \|^2 = \arg\min_{\mathbf{x}} \; \sum_{i} \| r_i(\mathbf{x}_i) \|_{\Sigma_i}^2 } \]

这就是 SLAM 后端优化的核心目标函数。 整个推导链清晰地展示了：

1. SLAM 的概率推断本质（MAP）
2. 对数技巧将乘积变为求和
3. 高斯噪声假设将负对数似然变为二次型
4. 最终得到加权最小二乘问题

为什么这个推导很重要？ 因为非线性最小二乘是一个被深入研究的数学问题，有大量成熟的算法（Gauss-Newton、Levenberg-Marquardt、Dogleg 等）。通过这个推导，我们将 SLAM 与整个数值优化工具箱连接起来了。

---

1.4 图上的 Gauss-Newton 求解 ⭐⭐

动机

上一节我们得到了非线性最小二乘问题。但是 \(e_i(\mathbf{x})\) 是非线性函数，不能直接求解析解。怎么办？我们用 Gauss-Newton 方法：在当前估计点处线性化，求解一个线性最小二乘子问题，然后迭代。

Step 1：在当前估计点处线性化

设当前估计为 \(\mathbf{x}_0\)，定义增量 \(\Delta\mathbf{x} = \mathbf{x} - \mathbf{x}_0\)（在李群上需要用右扰动或左扰动，这里先用向量空间的记号）。对残差函数做一阶 Taylor 展开：

\[ e_i(\mathbf{x}_0 + \Delta\mathbf{x}) \approx e_i(\mathbf{x}_0) + J_i \Delta\mathbf{x}_i \]

其中 \(J_i = \frac{\partial e_i}{\partial \mathbf{x}_i}\Big|_{\mathbf{x}_0}\) 是残差关于变量的 Jacobian 矩阵。

注意：\(J_i\) 只对因子 \(f_i\) 连接的变量 \(\mathbf{x}_i\) 有非零列。对其他变量，Jacobian 为零。这个"局部性"是后面稀疏性的直接原因。

Step 2：将线性化代入目标函数

\[ \sum_i \| e_i(\mathbf{x}_0) + J_i \Delta\mathbf{x}_i \|^2 \]

将所有因子的残差堆叠为向量 \(\mathbf{e} = [e_1^T, e_2^T, \ldots]^T\)，所有 Jacobian 堆叠为矩阵 \(J\)：

\[ F(\Delta\mathbf{x}) = \| \mathbf{e} + J \Delta\mathbf{x} \|^2 \]

展开：

\[ F(\Delta\mathbf{x}) = \mathbf{e}^T \mathbf{e} + 2\mathbf{e}^T J \Delta\mathbf{x} + \Delta\mathbf{x}^T J^T J \Delta\mathbf{x} \]

Step 3：对增量求导并令导数为零

\[ \frac{\partial F}{\partial \Delta\mathbf{x}} = 2 J^T \mathbf{e} + 2 J^T J \Delta\mathbf{x} = 0 \]

整理得到 正规方程（Normal Equations）：

\[ \boxed{ J^T J \; \Delta\mathbf{x}^* = -J^T \mathbf{e} } \]

或者用更常见的记号：

\[ H \Delta\mathbf{x}^* = -\mathbf{b} \]

其中 \(H = J^T J\) 称为**近似 Hessian 矩阵**（也是信息矩阵的近似），\(\mathbf{b} = J^T \mathbf{e}\) 是梯度向量。

Step 4：迭代更新

求解增量后，更新估计值：

\[ \mathbf{x}_0 \leftarrow \mathbf{x}_0 + \Delta\mathbf{x}^* \]

然后在新的线性化点重新计算 \(J\) 和 \(\mathbf{e}\)，重复上述过程直到收敛（\(\|\Delta\mathbf{x}\|\) 足够小）。

在李群上（如 \(SE(3)\)），更新方式为：

\[ x_t \leftarrow x_t \cdot \text{Exp}(\Delta\xi_t) \]

其中 \(\Delta\xi_t\) 是李代数上的增量，\(\text{Exp}(\cdot)\) 是指数映射。

Gauss-Newton vs Levenberg-Marquardt

实际系统中通常使用 Levenberg-Marquardt（LM），它在正规方程中加入阻尼项：

\[ (H + \lambda \cdot \text{diag}(H)) \Delta\mathbf{x} = -\mathbf{b} \]

\(\lambda\) 大时退化为梯度下降（保守但稳健），\(\lambda\) 小时退化为 Gauss-Newton（快速但可能不稳定）。LM 根据每一步的下降效果自适应调整 \(\lambda\)。

---

1.5 稀疏性：为什么信息矩阵是稀疏的 ⭐⭐

动机

求解正规方程 \(H \Delta\mathbf{x} = -\mathbf{b}\) 是图优化的核心计算。如果 \(H\) 是稠密的 \(n \times n\) 矩阵，直接求解需要 \(O(n^3)\)。对于大规模 SLAM（数千个位姿、数万个路标），这是不可接受的。幸运的是，\(H\) 具有**稀疏结构**。

稀疏性的来源

回忆 \(H = J^T J = \sum_i J_i^T J_i\)。每个因子 \(f_i\) 只连接少数变量（通常 2 个），所以 \(J_i\) 只在对应变量的列上有非零值。

考虑一个连接变量 \(x_a\) 和 \(x_b\) 的因子 \(f_i\)。它对 \(H\) 的贡献为：

\[ J_i^T J_i = \begin{bmatrix} J_{ia}^T J_{ia} & J_{ia}^T J_{ib} \\ J_{ib}^T J_{ia} & J_{ib}^T J_{ib} \end{bmatrix} \]

这只填充 \(H\) 中 \((x_a, x_a)\)、\((x_a, x_b)\)、\((x_b, x_a)\)、\((x_b, x_b)\) 四个块。

关键洞察：\(H\) 中第 \((a, b)\) 块非零，当且仅当存在至少一个因子同时连接变量 \(x_a\) 和 \(x_b\)。换言之，\(H\) 的稀疏模式（sparsity pattern）直接反映了因子图的连接结构。

对于典型的 SLAM 问题： - 里程计因子连接相邻位姿 → 产生带状结构 - 观测因子连接位姿和路标 → 产生块稀疏结构 - 回环因子连接远距离位姿 → 产生少量非对角线元素

结果是：\(H\) 矩阵中绝大多数块是零。对于 \(N\) 个位姿、\(M\) 个路标的问题，\(H\) 的维度是 \((6N + 3M) \times (6N + 3M)\)（3D 情况），但非零元素数量只与因子数量（约束数量）成正比，远小于 \((6N + 3M)^2\)。

稀疏性的工程意义

方面	稠密矩阵	稀疏矩阵
存储	\(O(n^2)\)	\(O(\text{nnz})\)，nnz = 非零元素数
Cholesky 分解	\(O(n^3)\)	\(O(\text{nnz} \cdot n)\) 甚至更优
实际 SLAM	不可行（\(n > 10^4\)）	可在线运行

---

1.6 变量消元与 Fill-in ⭐⭐⭐

动机

知道 \(H\) 是稀疏的之后，我们需要高效地求解 \(H\Delta\mathbf{x} = -\mathbf{b}\)。最常用的方法是 Cholesky 分解 \(H = L L^T\)（\(H\) 对称正定时）。但是，Cholesky 分解过程中会产生 fill-in：原来为零的位置变成非零。Fill-in 越多，计算越慢。

变量消元的直觉

Cholesky 分解本质上是按顺序消元：先消去第一个变量，然后消去第二个，依次类推。消去一个变量时，该变量的所有邻居之间会建立新的连接（因为消元等价于边缘化，边缘化会产生 fill-in）。

例子：假设变量 \(x_1\) 与 \(x_2, x_3, x_4\) 都有连接（但 \(x_2, x_3, x_4\) 之间没有直接连接）。消去 \(x_1\) 后，\(x_2, x_3, x_4\) 之间会产生新的连接（fill-in）。这被称为 fill-in 效应。

直觉上，消去一个连接很多变量的"枢纽"变量会产生大量 fill-in，而消去只连接少数变量的"叶子"变量则几乎不产生 fill-in。

消元顺序的重要性

Fill-in 的数量**强烈依赖于消元顺序（elimination ordering）**。不同的顺序可能导致 fill-in 数量相差几个数量级。

极端例子：一个星形图（中心节点连接所有其他节点）。如果先消去中心节点，所有外围节点之间产生 fill-in，变成完全图（\(O(n^2)\) 个非零元素）。如果先消去所有外围节点，每次消元不产生 fill-in，最后消去中心节点。

AMD/COLAMD 启发式

找到最优消元顺序是 NP-hard 问题（等价于求最小 fill-in 消元），但有很好的启发式算法：

算法	全称	策略	特点
AMD	Approximate Minimum Degree	每步消去当前度数最小的节点	简单有效
COLAMD	Column Approximate Minimum Degree	基于列的消元顺序优化	适合矩形矩阵
Nested Dissection	—	递归地将图切成两半	适合网格状结构

在 SLAM 中，COLAMD 是最常用的。GTSAM 和 g2o 等框架默认使用 COLAMD 来确定消元顺序。

COLAMD 为什么适合 SLAM？ SLAM 的因子图通常是"长链 + 少量交叉连接"的结构。COLAMD 的列度数启发式恰好倾向于先消去度数低的路标节点（每个路标通常只被少数帧观测），然后再消去位姿节点。这与 Bundle Adjustment 中 Schur complement 先消去路标的策略不谋而合。

Fill-in 与稀疏矩阵存储

实际实现中，稀疏矩阵使用 CSC（Compressed Sparse Column） 或 CSR（Compressed Sparse Row） 格式存储，只保存非零元素及其索引。Fill-in 会增加非零元素数量，因此好的消元顺序直接影响内存占用和计算速度。

---

1.7 信息矩阵 vs 协方差矩阵 ⭐⭐

动机

在 SLAM 中，有两种等价的方式表示不确定性：协方差矩阵 \(\Sigma\) 和 信息矩阵 \(\Lambda = \Sigma^{-1}\)。它们是互逆关系，但在不同的 SLAM 方法中扮演不同角色。理解它们的区别对于理解 EKF-SLAM 和图优化 SLAM 的本质差异至关重要。

信息矩阵的定义

对于高斯分布 \(\mathcal{N}(\boldsymbol{\mu}, \Sigma)\)，其概率密度为：

\[ p(\mathbf{x}) \propto \exp\left( -\frac{1}{2} (\mathbf{x} - \boldsymbol{\mu})^T \Sigma^{-1} (\mathbf{x} - \boldsymbol{\mu}) \right) \]

也可以写成**信息形式（canonical form / information form）**：

\[ p(\mathbf{x}) \propto \exp\left( -\frac{1}{2} \mathbf{x}^T \Lambda \mathbf{x} + \boldsymbol{\eta}^T \mathbf{x} \right) \]

其中 \(\Lambda = \Sigma^{-1}\) 是信息矩阵，\(\boldsymbol{\eta} = \Lambda \boldsymbol{\mu}\) 是信息向量。

稀疏 vs 稠密的关键对比

这是一个深刻的结论——在 SLAM 中，信息矩阵是稀疏的，但协方差矩阵是稠密的：

性质	信息矩阵 \(\Lambda\)	协方差矩阵 \(\Sigma\)
稀疏性	稀疏	稠密
零元素含义	\(\Lambda_{ij} = 0\) 意味着 \(x_i\) 和 \(x_j\) 条件独立（给定其他变量）	\(\Sigma_{ij} = 0\) 意味着 \(x_i\) 和 \(x_j\) 边缘独立
SLAM 中	大部分变量对条件独立	通过路标传递，几乎所有变量对都边缘相关
更新方式	新观测 → 加法更新（局部修改）	新观测 → 全局更新（整个矩阵改变）

为什么信息矩阵稀疏？ 因为 SLAM 中的每个观测只连接少数变量（一个位姿和一个路标），所以它只贡献信息矩阵中对应的几个块。

为什么协方差矩阵稠密？ 虽然位姿 \(x_1\) 和路标 \(l_{100}\) 之间没有直接观测，但通过中间的位姿-路标-位姿链条，它们的估计不确定性是相互关联的。这种"通过路标传播的相关性"使得协方差矩阵几乎处处非零。

一个直觉类比

想象一个社交网络： - **信息矩阵**像"直接朋友关系"：你和直接朋友之间有连接，和陌生人之间没有 - **协方差矩阵**像"间接影响关系"：你朋友的朋友的朋友也会间接影响你，导致几乎所有人之间都有（弱）关联

---

1.8 EKF-SLAM vs Graph-SLAM 对比 ⭐⭐

核心对比

特性	EKF-SLAM	Graph-SLAM
表示形式	协方差形式 \((\boldsymbol{\mu}, \Sigma)\)	信息形式 \(H\Delta\mathbf{x} = -\mathbf{b}\)
矩阵结构	稠密协方差 \((3+2M) \times (3+2M)\)	稀疏信息矩阵
更新复杂度	\(O(M^2)\) per step（\(M\) = 路标数）	\(O(\text{nnz})\) per iteration
扩展性	\(M < 1000\) 实际可行	\(M > 100{,}000\) 可行
线性化	只在当前估计点线性化一次	可迭代重新线性化
一致性	容易不一致（可观性被错误提升）	通过重新线性化改善
历史轨迹	被边缘化掉，无法恢复	保留所有位姿
回环闭合	难以处理（需全局更新）	自然支持（加一个因子即可）

计算复杂度的深入分析

EKF-SLAM 为什么是 \(O(M^2)\)？

EKF 的更新步骤涉及卡尔曼增益 \(K = \Sigma H^T (H \Sigma H^T + R)^{-1}\)，其中 \(\Sigma\) 是 \((3+2M) \times (3+2M)\) 的稠密矩阵。\(\Sigma H^T\) 的计算需要 \(O(M)\) 次与 \(\Sigma\) 列的乘法（因为 \(H\) 是稀疏的），但更新 \(\Sigma \leftarrow \Sigma - K H \Sigma\) 涉及稠密矩阵减法，复杂度为 \(O(M^2)\)。

Graph-SLAM 为什么可以做到准线性？

图优化的每次迭代需要：（1）计算所有残差和 Jacobian，\(O(K)\)（\(K\) = 因子数）；（2）组装 \(H\) 和 \(\mathbf{b}\)，\(O(\text{nnz})\)；（3）求解稀疏线性系统，取决于 fill-in。对于良好排序的稀疏矩阵，Cholesky 分解的复杂度远低于 \(O(n^3)\)。实验表明，当路标数 \(N > 600\) 时，图优化的每步计算量已经低于 EKF。

工程决策指导

什么时候用 EKF，什么时候用图优化？

场景	推荐方法	原因
路标少（< 100）、需要实时	EKF	实现简单，开销可控
大规模户外 SLAM	图优化	EKF 扩展性不够
需要高精度地图	图优化	可重新线性化
资源极度受限的嵌入式	EKF 或 SEIF	固定计算量

---

1.9 常见陷阱 ⚠️

⚠️ 概念误区：认为"信息矩阵就是 Hessian 矩阵"

新手想法："\(H = J^T J\) 不就是目标函数的 Hessian 矩阵吗？所以信息矩阵 = Hessian。"

实际上：\(J^T J\) 只是 Hessian 的 Gauss-Newton 近似。真正的 Hessian 还包含二阶项 \(\sum_i r_i \nabla^2 r_i\)，Gauss-Newton 忽略了这些项。在残差较小（接近最优解）时这个近似很好，但在远离最优解时可能导致收敛问题。此外，概率意义上的 Fisher 信息矩阵是 \(E[J^T J]\)（取期望），而不是某一次计算的 \(J^T J\)。

正确理解：\(J^T J\) 是信息矩阵的一个近似，在高斯假设和小残差下近似很好。正规方程中的 \(H\) 是 Gauss-Newton 近似 Hessian，不是精确 Hessian。

⚠️ 思维陷阱：认为"因子越多估计越准"

新手想法："加更多的传感器约束（因子）一定能提高精度。"

实际上：如果新增的因子包含错误的数据关联（比如错误的回环检测），它会严重损害估计质量。一个错误的回环因子可能比一百个正确因子的负面影响还大。这就是为什么 SLAM 系统需要鲁棒的前端（outlier rejection）和鲁棒核函数（Huber loss、Cauchy loss 等）。

正确做法：在添加因子前做严格的验证（几何一致性检查、\(\chi^2\) 检验），并在优化中使用鲁棒核函数。

⚠️ 编程陷阱：忽略因子图中的消元顺序

错误做法：在 g2o 或 GTSAM 中使用默认的自然顺序（按变量 ID 排序）进行 Cholesky 分解。

现象：大规模问题中求解时间急剧增长，内存占用远超预期。

根本原因：自然顺序通常产生大量 fill-in。例如在 pose-graph 中，自然顺序先消去 \(x_1\)，\(x_1\) 与后续多个回环位姿相连，产生大量 fill-in。

正确做法：使用 COLAMD 或 AMD 重排序。在 GTSAM 中 GaussianFactorGraph::optimize() 默认使用 COLAMD。在 g2o 中需要显式设置 solver->setBlockOrdering(true)。

1.10 练习

练习 1（手推，⭐⭐）：考虑一个简单的 1D SLAM 问题：机器人从 \(x_1\) 出发，经过 \(x_2\) 到达 \(x_3\)。里程计测量 \(u_{12} = 1.0\)（\(\sigma = 0.1\)），\(u_{23} = 1.5\)（\(\sigma = 0.2\)）。在 \(x_1\) 和 \(x_3\) 都观测到同一个路标 \(l_1\)：\(z_{1} = 2.0\)（\(\sigma = 0.15\)），\(z_{3} = -0.5\)（\(\sigma = 0.15\)）。先验 \(x_1 = 0\)（\(\sigma = 0.01\)）。 - (a) 画出因子图 - (b) 写出所有残差函数 - (c) 列出 \(H\) 矩阵的稀疏结构（哪些块非零） - (d) 用一次 Gauss-Newton 迭代求解（初始值 \(x_1=0, x_2=1, x_3=2.5, l_1=2\)）

练习 2（思考，⭐⭐⭐）：为什么说"EKF-SLAM 是图优化 SLAM 的一种特殊情况"？提示：考虑如果在图优化中，每加入一个新位姿就立即边缘化掉最旧的位姿，会得到什么？

练习 3（编程，⭐⭐）：使用 GTSAM（C++ 或 Python 接口）构建上述练习 1 的因子图，比较不同消元顺序下的求解时间和 fill-in 数量。打印 \(H\) 矩阵的稀疏结构图。

---

2. Bundle Adjustment 理论 ⭐⭐⭐

本节解决的问题：在视觉 SLAM 中，如何同时优化所有相机位姿和三维点，使得重投影误差最小？如何利用问题的特殊结构高效求解？

2.1 BA 的定义与问题建模 ⭐⭐

动机

Bundle Adjustment（BA）的名字来自"光束法平差"：多束光线（从 3D 点投射到多个相机的光线）需要同时调整（adjust），使得所有投影都尽可能吻合观测。

BA 是视觉 SLAM 后端的核心，也是 SfM（Structure from Motion）的最后一步。它可以看作图优化的一个特例——因子图中的变量是相机位姿和 3D 路标点，因子是重投影误差。

数学定义

设有 \(m\) 个相机位姿 \(\{C_1, \ldots, C_m\}\)（每个 \(C_i \in SE(3)\)，6 自由度）和 \(n\) 个 3D 点 \(\{P_1, \ldots, P_n\}\)（每个 \(P_j \in \mathbb{R}^3\)）。如果相机 \(C_i\) 观测到了点 \(P_j\)，记观测到的像素坐标为 \(\mathbf{z}_{ij} \in \mathbb{R}^2\)。

重投影函数：给定相机内参 \(K\)、相机位姿 \(C_i\)（即外参 \([R_i \mid t_i]\)）和 3D 点 \(P_j\)，重投影为：

\[ \hat{\mathbf{z}}_{ij} = \pi(C_i, P_j) = K \cdot \text{proj}\left( R_i P_j + t_i \right) \]

其中 \(\text{proj}([X, Y, Z]^T) = [X/Z, Y/Z]^T\) 是针孔投影。

BA 优化目标：

\[ \min_{\{C_i\}, \{P_j\}} \sum_{(i,j) \in \mathcal{O}} \| \mathbf{z}_{ij} - \pi(C_i, P_j) \|_{\Sigma_{ij}}^2 \]

其中 \(\mathcal{O}\) 是所有观测对的集合。

与一般图优化的关系：BA 就是图优化中，变量分为"相机"和"点"两类，因子全部是重投影因子的特殊情况。

---

2.2 Arrow/Block 稀疏结构 ⭐⭐⭐

动机

BA 的 Hessian 矩阵 \(H = J^T J\) 有一种非常特殊的稀疏模式，称为 Arrow 结构（箭头结构）。理解这个结构是理解 Schur complement 加速的前提。

Jacobian 的结构

BA 中有两类变量：相机参数 \(\mathbf{c} = [C_1, \ldots, C_m]\) 和点参数 \(\mathbf{p} = [P_1, \ldots, P_n]\)。将状态向量排列为 \(\mathbf{x} = [\mathbf{c}^T, \mathbf{p}^T]^T\)。

每个观测因子的 Jacobian 为：

\[ J_{ij} = \begin{bmatrix} \frac{\partial e_{ij}}{\partial C_i} & \frac{\partial e_{ij}}{\partial P_j} \end{bmatrix} \]

关键是：每个观测只涉及一个相机和一个点。因此整体 Jacobian 矩阵非常稀疏。

Hessian \(H = J^T J\) 的分块结构

将 \(H\) 按相机-点分块：

\[ H = \begin{bmatrix} H_{cc} & H_{cp} \\ H_{pc} & H_{pp} \end{bmatrix} \]

其中： - \(H_{cc}\)（相机-相机块）：\(6m \times 6m\)，块对角（如果不考虑相机间的直接约束的话；每个观测只涉及一个相机，所以不同相机之间没有直接交叉项） - \(H_{pp}\)（点-点块）：\(3n \times 3n\)，块对角（每个观测只涉及一个点，所以不同点之间没有直接交叉项） - \(H_{cp}\)（相机-点块）：\(6m \times 3n\)，稀疏（只有在相机 \(i\) 观测到点 \(j\) 时才有非零块）

这就是 Arrow 结构：\(H_{pp}\) 是块对角的，所以从 \(H\) 的整体来看，如果将点放在右下角，矩阵呈现箭头形状。

重要修正：准确地说，\(H_{cc}\) 不一定是块对角的。当多个观测涉及同一个点时，\(\sum_j J_{ij}^{(c)T} J_{kj}^{(c)}\)（\(i \neq k\)）可能产生非零的相机-相机交叉块。但 \(H_{pp}\) 严格块对角，因为不同点从不出现在同一个因子中。

---

2.3 Schur Complement：完整推导 ⭐⭐⭐

动机

BA 的正规方程为：

\[ \begin{bmatrix} H_{cc} & H_{cp} \\ H_{pc} & H_{pp} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{c} \\ \Delta\mathbf{p} \end{bmatrix} = -\begin{bmatrix} \mathbf{b}_c \\ \mathbf{b}_p \end{bmatrix} \]

直接求解这个 \((6m + 3n) \times (6m + 3n)\) 的系统开销很大。但 \(H_{pp}\) 是块对角的——每个 \(3 \times 3\) 块可以独立求逆。能否利用这个性质？这正是 Schur complement 的核心思想：先消去"便宜"的变量（点），得到一个只关于"贵"的变量（相机）的小系统。

Step 1：展开为两个方程

\[ H_{cc} \Delta\mathbf{c} + H_{cp} \Delta\mathbf{p} = -\mathbf{b}_c \quad \cdots (1) \]

\[ H_{pc} \Delta\mathbf{c} + H_{pp} \Delta\mathbf{p} = -\mathbf{b}_p \quad \cdots (2) \]

Step 2：从方程 (2) 中解出 \(\Delta\mathbf{p}\)

因为 \(H_{pp}\) 是块对角的，\(H_{pp}^{-1}\) 可以通过逐个求逆每个 \(3 \times 3\) 块得到（\(O(n)\) 复杂度）：

\[ \Delta\mathbf{p} = H_{pp}^{-1} \left( -\mathbf{b}_p - H_{pc} \Delta\mathbf{c} \right) \quad \cdots (3) \]

Step 3：将 (3) 代入 (1)，消去 \(\Delta\mathbf{p}\)

\[ H_{cc} \Delta\mathbf{c} + H_{cp} \left[ H_{pp}^{-1} (-\mathbf{b}_p - H_{pc} \Delta\mathbf{c}) \right] = -\mathbf{b}_c \]

展开：

\[ H_{cc} \Delta\mathbf{c} - H_{cp} H_{pp}^{-1} H_{pc} \Delta\mathbf{c} - H_{cp} H_{pp}^{-1} \mathbf{b}_p = -\mathbf{b}_c \]

整理：

\[ \left( H_{cc} - H_{cp} H_{pp}^{-1} H_{pc} \right) \Delta\mathbf{c} = -\mathbf{b}_c + H_{cp} H_{pp}^{-1} \mathbf{b}_p \]

Step 4：定义 Schur complement

\[ \boxed{ S = H_{cc} - H_{cp} H_{pp}^{-1} H_{pc} } \]

\(S\) 称为 \(H\) 关于 \(H_{pp}\) 的 Schur complement。最终得到 Reduced Camera System：

\[ \boxed{ S \; \Delta\mathbf{c} = -\mathbf{b}_c + H_{cp} H_{pp}^{-1} \mathbf{b}_p } \]

Step 5：回代求 \(\Delta\mathbf{p}\)

先求解上面的 \(6m \times 6m\) 系统得到 \(\Delta\mathbf{c}\)，然后代入方程 (3) 得到 \(\Delta\mathbf{p}\)：

\[ \Delta\mathbf{p} = H_{pp}^{-1} \left( -\mathbf{b}_p - H_{pc} \Delta\mathbf{c} \right) \]

这一步同样利用 \(H_{pp}\) 的块对角性质，\(O(n)\) 完成。

计算量分析

步骤	计算量	说明
求 \(H_{pp}^{-1}\)	\(O(n)\)	\(n\) 个 \(3 \times 3\) 块逆
组装 \(S\)	\(O(m^2 n)\)	需要计算 \(H_{cp} H_{pp}^{-1} H_{pc}\)
求解 \(S \Delta\mathbf{c} = \ldots\)	\(O(m^3)\)	\(S\) 是 \(6m \times 6m\) 稠密矩阵
回代求 \(\Delta\mathbf{p}\)	\(O(n)\)	利用 \(H_{pp}^{-1}\) 的块对角性

关键收益：原问题维度 \(6m + 3n\)，在视觉 SLAM 中 \(n \gg m\)（点远多于相机），通过 Schur complement 将问题降维到 \(6m\)，节省了大量计算。

数值例子：假设 \(m = 100\) 个相机，\(n = 10{,}000\) 个点。原系统维度 \(6 \times 100 + 3 \times 10{,}000 = 30{,}600\)。Schur complement 后降为 \(6 \times 100 = 600\)。降维比 \(51:1\)。

Schur complement 的数学性质

\(S\) 是对称正定的（当 \(H\) 对称正定时），因此可以用 Cholesky 分解求解。证明思路：\(S\) 可以看作将 \(H\) 中点变量的"信息"投影到相机变量空间后的结果。

---

2.4 Gauge Freedom（规范自由度） ⭐⭐⭐

动机

尝试对单目 BA 直接求解时，你会发现 \(H\) 矩阵是**奇异的**（不可逆）。这不是 bug，而是问题本身的固有歧义性。

什么是 Gauge Freedom？

BA 优化目标只涉及重投影误差——这是**相对量**。如果我们对所有相机和所有点同时做一个刚体变换（平移 + 旋转），重投影误差不变。对于单目视觉，如果再同时缩放所有深度和位移，重投影误差仍然不变。

这意味着目标函数有一个 7 维零空间（单目情况）：

自由度	维度	解释
平移	3	整体平移不改变投影
旋转	3	整体旋转不改变投影
尺度	1	单目无法确定绝对尺度
合计	7	单目 BA 的 Gauge 自由度

对于双目或 RGB-D，尺度可以确定，Gauge 自由度为 6。对于有 IMU 的系统（如 VIO），重力方向被约束，Gauge 自由度进一步减少。

如何处理 Gauge Freedom？

有几种常用方法：

方法 1：固定变量（Gauge Fixing）

固定一个相机的位姿（6 DOF）和一条基线的长度（1 DOF），共消除 7 个自由度。具体做法是：在优化中不更新第一个相机的位姿，将 \(\Delta C_1 = 0\)。

方法 2：添加先验因子

给第一个相机的位姿添加一个强先验 \(f_{\text{prior}}(C_1)\)，等价于在 \(H\) 的对角线上加一个大数。

方法 3：Free Gauge（自由规范）

不固定 Gauge，让优化器自由选择。虽然 \(H\) 奇异，但可以通过正则化（LM 的阻尼项 \(\lambda I\)）使其可逆。结果依赖于初始值的选择，但误差空间是唯一的。

工程建议：大多数 SLAM 系统使用方法 1 或 2，因为它们数值上更稳定。GTSAM 中通常给第一个位姿添加 PriorFactor。

如果不处理 Gauge Freedom 会怎样？

• 纯 Gauss-Newton：\(H\) 奇异，无法 Cholesky 分解，程序崩溃
• Levenberg-Marquardt：\(H + \lambda I\) 可逆，但 \(\lambda\) 需要很大才能克服奇异性，导致收敛极慢，且解可能在零空间方向上漂移

---

2.5 常见陷阱 ⚠️

⚠️ 概念误区：认为"Schur complement 只是 BA 的技巧"

新手想法："Schur complement 是 BA 特有的，其他问题用不上。"

实际上：Schur complement 是线性代数中的通用工具，在 SLAM 中至少有三个不同的应用场景：（1）BA 中消去点变量；（2）滑动窗口中边缘化旧变量；（3）SEIF 中稀疏化信息矩阵。理解它的通用数学形式（分块矩阵的消元）比只记住 BA 中的特殊形式重要得多。

延伸：Schur complement 在控制理论（LQR 的 Riccati 方程）、统计学（条件高斯分布）、博弈论中都有应用。

⚠️ 编程陷阱：忘记处理 Gauge Freedom 导致优化失败

错误做法：直接构建单目 BA 问题并调用 Gauss-Newton 求解器。

现象：Cholesky 分解失败（矩阵不正定），或者 LM 迭代不收敛，\(\lambda\) 不断增大。

根本原因：\(H\) 矩阵有 7 维零空间（单目），不满足正定条件。

正确做法：在 Ceres 中设置 problem.SetParameterBlockConstant(camera_0) 固定第一个相机；在 GTSAM 中添加 PriorFactor<Pose3> 到第一个位姿；或者确认你使用的是 LM 求解器（阻尼项可以正则化零空间）。

自检方法：打印 \(H\) 矩阵的最小特征值，如果有 7 个接近零的特征值（单目），说明 Gauge 未被固定。

2.6 练习

练习 1（手推，⭐⭐⭐）：考虑最简单的 BA 问题——2 个相机、3 个点。写出 \(H\) 矩阵的分块结构，标明哪些块非零。假设每个相机都观测到所有 3 个点，验证 \(H_{pp}\) 是块对角的。手动执行 Schur complement 消去点变量。

练习 2（思考，⭐⭐⭐⭐）：在大规模 BA（\(m = 1000\) 相机，\(n = 1{,}000{,}000\) 点）中，Schur complement 后的 \(S\) 矩阵维度为 \(6000 \times 6000\)。如果 \(S\) 是稠密的，求解需要 \(O(6000^3)\)。在什么条件下 \(S\) 也可以是稀疏的？提示：考虑相机之间的共视关系（两个相机看到同一个点就产生 \(S\) 中的非零块）。

---

3. 滑动窗口与边缘化 ⭐⭐⭐

本节解决的问题：图优化保留所有历史变量，计算量随时间增长。如何在保证估计质量的同时，将计算量控制在有界范围内？

3.1 为什么需要滑动窗口 ⭐⭐

动机

全局图优化（full smoothing）保留了所有历史位姿和路标，理论上给出最优估计。但它有一个致命问题：计算量随时间无限增长。

假设机器人以 10 Hz 运行 1 小时，产生 36,000 个位姿节点。即使利用稀疏性，每次优化也需要数秒甚至数十秒。对于需要毫秒级响应的 VIO（视觉惯性里程计），这完全不可接受。

解决方案：只维护最近 \(W\) 个关键帧的优化窗口（\(W\) 通常为 10-20），旧的变量通过**边缘化（marginalization）**移除，同时将它们提供的信息保留为一个先验因子。

方法	变量数量	计算量	精度
滤波（EKF）	固定（当前状态）	\(O(M^2)\)	较低（不可重新线性化）
全局优化	无限增长	无界增长	最高
滑动窗口	有界（\(W\) 帧）	有界	接近全局最优

滑动窗口方法是滤波和全局优化之间的最佳折中。

---

3.2 边缘化 = 信息矩阵上的 Schur Complement ⭐⭐⭐

动机

滑动窗口中，我们要移除旧变量 \(\mathbf{x}_m\)（margin variables），保留当前变量 \(\mathbf{x}_r\)（remaining variables）。如何在移除 \(\mathbf{x}_m\) 的同时，保留它对 \(\mathbf{x}_r\) 的约束信息？

答案就是边缘化——在信息矩阵上执行 Schur complement。

从概率角度理解边缘化

联合分布为 \(P(\mathbf{x}_r, \mathbf{x}_m) = \mathcal{N}(\boldsymbol{\mu}, \Lambda^{-1})\)，其中信息矩阵分块为：

\[ \Lambda = \begin{bmatrix} \Lambda_{rr} & \Lambda_{rm} \\ \Lambda_{mr} & \Lambda_{mm} \end{bmatrix} \]

边缘分布 \(P(\mathbf{x}_r)\) 的信息矩阵为：

\[ \boxed{ \Lambda_r^{\text{margin}} = \Lambda_{rr} - \Lambda_{rm} \Lambda_{mm}^{-1} \Lambda_{mr} } \]

这正是 Schur complement！ 与 BA 中消去点变量的数学形式完全一致，只是这里消去的是"旧的位姿变量"。

信息保留定理

一个关键性质：边缘化不丢失关于保留变量的任何信息。

更精确地说：在线性高斯假设下，\(P(\mathbf{x}_r)\) 是 \(P(\mathbf{x}_r, \mathbf{x}_m)\) 关于 \(\mathbf{x}_r\) 的充分统计量。也就是说，如果你只关心 \(\mathbf{x}_r\) 的估计，边缘化后你拥有的信息和边缘化前**完全一样**。

直觉理解：边缘化就是"积分掉"不需要的变量。在高斯分布中，积分掉变量只会让剩余变量的分布"更宽"（不确定性更大），但不会丢失对剩余变量的相对约束关系。

但是——这只在**线性**高斯假设下严格成立。在非线性问题中，边缘化会"固定"被边缘化变量的线性化点，这引出了下面的问题。

---

3.3 Fill-in 问题：边缘化的代价 ⭐⭐⭐

动机

边缘化虽然保留了信息，但有一个副作用：产生 fill-in，使得先验因子变得稠密。

为什么会产生 fill-in？

当边缘化变量 \(x_m\) 时，所有与 \(x_m\) 相连的变量之间会建立新的连接。这是因为 \(\Lambda_{rm} \Lambda_{mm}^{-1} \Lambda_{mr}\) 项在所有与 \(x_m\) 相邻的变量对之间引入了非零信息。

例子：假设关键帧 \(x_3\) 要被边缘化，而 \(x_3\) 观测到了路标 \(l_1, l_2, l_5\)，并与 \(x_2, x_4\) 有里程计/IMU 因子。边缘化 \(x_3\) 后，\(\{x_2, x_4, l_1, l_2, l_5\}\) 之间会产生一个稠密的先验因子（所有变量对之间都有关联）。

fill-in 的累积效应

随着滑动窗口的推进，每次边缘化都增加先验因子的稠密度。经过多次边缘化后，先验因子可能涉及窗口内所有变量，完全丧失稀疏性。

应对策略：

策略	方法	代价
接受 fill-in	让先验因子稠密化	计算量略增，但保持最优性
稀疏化近似	用稀疏因子近似稠密先验	丢失部分信息，引入近似误差
谨慎选择边缘化顺序	先边缘化连接少的变量	减缓 fill-in 积累

VINS-Mono 采用的策略是接受 fill-in，因为窗口较小（约 10 帧），先验因子的维度有限。

---

3.4 FEJ：First-Estimate Jacobian ⭐⭐⭐⭐

动机

边缘化还有一个更微妙的问题：线性化点被固定。 当我们边缘化一个变量时，与该变量相关的 Jacobian 在当前估计点处被计算并"冻结"。在后续优化中，即使保留变量的估计值发生了显著变化，先验因子中的 Jacobian 不会更新。

这导致了**一致性问题**：不同的因子在不同的线性化点处被评估，系统的数学性质（特别是可观性）可能被破坏。

不一致性的根源

考虑一个简单的例子。假设在时刻 \(t_1\)，我们在线性化点 \(\hat{x}_1^{(1)}\) 处计算了因子 \(f_1\) 的 Jacobian \(J_1^{(1)}\)。然后边缘化了一些变量，\(J_1^{(1)}\) 被锁定在先验因子中。

在时刻 \(t_2\)，优化更新了 \(x_1\) 的估计为 \(\hat{x}_1^{(2)}\)。但先验因子中 \(J_1\) 仍然是在 \(\hat{x}_1^{(1)}\) 处计算的。而新加入的因子 \(f_2\) 的 Jacobian 是在 \(\hat{x}_1^{(2)}\) 处计算的。

问题在于：同一个变量 \(x_1\) 在不同的因子中使用了不同的线性化点。 这破坏了系统的自洽性。

在 VIO 中的具体表现是：系统的实际不可观子空间是 4 维的（全局位置 3D + yaw 角 1D），但由于不一致的线性化，估计器会"虚假地"获得这些方向上的信息，导致过度自信的估计（协方差比实际偏小）。

FEJ 的解决方案

First-Estimate Jacobian（FEJ）的核心思想极其简单：对于每个变量，始终使用它"第一次被估计"时的值作为该变量所有 Jacobian 的线性化点。

具体来说： 1. 变量 \(x_i\) 第一次被加入优化窗口时，记录其估计值 \(\hat{x}_i^{(0)}\) 2. 此后，无论 \(x_i\) 的当前估计如何变化，**所有**涉及 \(x_i\) 的因子（包括新加入的和先验中的）都在 \(\hat{x}_i^{(0)}\) 处计算 Jacobian

为什么 FEJ 能恢复一致性？ 因为当所有 Jacobian 在同一个线性化点处评估时，系统的可观性矩阵（observability matrix）具有正确的零空间维度。不同的线性化点破坏零空间结构，而 FEJ 通过统一线性化点来避免这个问题。

FEJ 的代价是什么？ 使用固定的（可能不准确的）线性化点意味着线性化误差不会随迭代减小。这会导致次优的估计精度。但在实际中，这个代价远小于不一致性带来的问题。

FEJ 的改进变体

原始 FEJ 可能过于保守（线性化点可能距离真值较远）。后续工作提出了改进：

方法	思想	特点
FEJ (Huang et al., 2008)	所有 Jacobian 用第一次估计	简单但可能次优
FEJ2 (Chen et al., 2022)	自适应选择线性化点	更好的精度-一致性平衡
OC-EKF (Huang et al., 2010)	直接修正可观性矩阵	理论更优美，实现更复杂

---

3.5 VINS-Mono 的实际边缘化策略 ⭐⭐

动机

理论再好，最终要落地到工程中。VINS-Mono 是一个经典的滑动窗口 VIO 系统，它的边缘化策略清晰实用，值得深入学习。

窗口结构

VINS-Mono 维护一个包含最近 \(W\)（默认 10）个关键帧的滑动窗口。窗口内的状态包括：

\[ \mathcal{X} = \{x_1, x_2, \ldots, x_W, \; l_1, l_2, \ldots, l_L, \; \text{prior}\} \]

其中 \(x_i\) 是关键帧的位姿 + 速度 + IMU bias，\(l_j\) 是被当前窗口观测到的路标点。

两种边缘化策略

VINS-Mono 根据**最新帧是否为关键帧**选择不同的边缘化策略：

策略 A：最新帧是关键帧

当最新帧（第 \(W\) 帧）被判定为关键帧时： 1. 边缘化**最旧**的关键帧 \(x_1\) 2. 同时边缘化与 \(x_1\) 相关的所有视觉和 IMU 观测 3. 这些被边缘化的约束形成新的先验因子，叠加到已有先验上 4. 窗口内保留空间分布良好的关键帧集合

策略 B：最新帧不是关键帧

当最新帧（第 \(W\) 帧）被判定为非关键帧时（与前一帧视差不够大）： 1. 边缘化**次新**帧 \(x_{W-1}\)（注意不是最旧帧！） 2. 保留与 \(x_{W-1}\) 相关的视觉观测（通过边缘化传递给相邻帧） 3. 但**丢弃** \(x_{W-1}\) 的 IMU 预积分因子（因为直接连接 \(x_{W-2}\) 和 \(x_W\) 更合理）

为什么策略 B 不边缘化最旧帧？ 因为如果最新帧不是关键帧（视差小），说明机器人运动不大。此时丢弃最旧帧会减少窗口内的基线长度，不利于三角化精度。保留旧帧、丢弃"冗余"的次新帧更合理。

边缘化策略的目标

VINS-Mono 边缘化策略的核心目标是：保持窗口内关键帧的空间分布尽量均匀。空间分布均匀意味着： - 足够的视差（parallax）用于特征三角化 - 足够的加速度激励用于 IMU bias 可观性 - 足够的基线长度用于位姿估计精度

先验因子的维护

每次边缘化后，先验因子通过以下方式更新：

\[ \Lambda_{\text{prior}}^{\text{new}} = \Lambda_{\text{prior}}^{\text{old}} + \Lambda_{\text{margin}} \]

其中 \(\Lambda_{\text{margin}}\) 是本次边缘化通过 Schur complement 产生的新信息。先验因子是一个关于窗口内所有"曾与被边缘化变量相连"的变量的联合约束。

工程要点：先验因子的 Jacobian 和残差需要在 FEJ 线性化点处计算，以保持一致性。VINS-Mono 代码中使用 last_marginalization_parameter_blocks 记录了先验涉及的变量及其固定线性化点。

---

3.6 常见陷阱 ⚠️

⚠️ 概念误区：认为"边缘化 = 直接删除变量"

新手想法："滑动窗口就是把旧的变量删掉，只保留最近的。"

实际上：直接删除变量意味着**丢弃**与该变量相关的所有约束信息。而边缘化是在删除变量的同时，将该变量的信息"投影"到保留变量上，形成先验因子。两者的区别巨大：

• 直接删除：保留变量的不确定性增大（丢失了约束），估计精度退化
• 边缘化：保留变量的不确定性不变（信息完整保留），估计精度不受影响

一个简单的实验可以验证：在 GTSAM 中比较"移除因子"和"边缘化因子"后剩余变量的协方差。

正确理解：边缘化 = 在信息矩阵上执行 Schur complement = 积分掉不需要的变量 = 信息保留的变量移除。

⚠️ 思维陷阱：认为"FEJ 损失精度所以不该用"

新手想法："FEJ 固定了线性化点，不能重新线性化，所以精度一定比不用 FEJ 差。"

实际上：不用 FEJ 时，估计器可能变得**不一致**——它报告的不确定性（协方差）远小于实际误差。一个不一致的估计器不仅不准，而且**不知道自己不准**，这比"知道自己不太准"（FEJ 的情况）危险得多。

在实际系统中，FEJ 通常同时提高了**一致性**（NEES 更接近理论值）和**精度**（RMSE 更小），因为不一致性会导致错误的信息融合，反而让精度更差。

正确思维：FEJ 损失的是"理论上的局部最优性"，换来的是"全局一致性"。这是一个非常值得的交换。验证方法：跑 Monte Carlo 仿真，比较 NEES 和 RMSE。

⚠️ 编程陷阱：边缘化后忘记更新先验的线性化点

错误做法：在滑动窗口优化中，边缘化旧变量后，对保留变量继续迭代优化，但先验因子的 Jacobian 和残差仍在旧的线性化点处评估。

现象：系统短期运行正常，但长时间运行后轨迹开始发散，或者协方差估计变得不自洽（NEES 显著大于 1 或显著小于 1）。

根本原因：先验因子和新因子在不同线性化点处评估，破坏了系统的可观性结构。累积的不一致性最终导致发散。

正确做法：实现 FEJ——为每个变量维护一个固定的线性化点。在 VINS-Mono 代码中，查看 marginalization_factor.cpp 中的 Evaluate() 函数，注意它如何使用存储的线性化点计算残差。

3.7 练习

练习 1（手推，⭐⭐⭐）：考虑一个 4 变量的信息矩阵：

\[ \Lambda = \begin{bmatrix} 4 & -1 & 0 & -1 \\ -1 & 4 & -1 & 0 \\ 0 & -1 & 3 & -1 \\ -1 & 0 & -1 & 3 \end{bmatrix} \]

(a) 画出对应的无向图（\(\Lambda_{ij} \neq 0\) 表示 \(x_i\) 和 \(x_j\) 之间有边） (b) 边缘化 \(x_4\)（即消去第 4 个变量），计算剩余 3 个变量的边缘信息矩阵 (c) 比较边缘化前后图结构的变化——产生了哪些 fill-in？ (d) 如果改为先边缘化 \(x_3\)，fill-in 情况有什么不同？

练习 2（思考，⭐⭐⭐⭐）：在 VINS-Mono 的策略 B 中，为什么要丢弃次新帧的 IMU 预积分因子而不是同样进行边缘化？提示：考虑 IMU 预积分因子连接的变量，以及边缘化该因子会产生的 fill-in。

---

本章小结

主题	核心思想	关键公式	难度
SLAM as Bayesian Inference	后验 = 因子乘积	\(P(\mathbf{x} \mid \mathbf{z}) \propto \prod_i f_i(\mathbf{x}_i)\)	⭐⭐
因子图	变量 + 因子的二部图	—	⭐⭐
MAP → 最小二乘	高斯假设下取负对数	\(\min \sum_i \|r_i\|_{\Sigma_i}^2\)	⭐⭐
Gauss-Newton	线性化 + 正规方程	\(J^T J \Delta\mathbf{x} = -J^T \mathbf{e}\)	⭐⭐
稀疏性	因子局部连接 → \(H\) 稀疏	\(H_{ab} \neq 0 \Leftrightarrow\) 存在连接因子	⭐⭐
消元与 fill-in	消元顺序影响 fill-in	COLAMD 启发式	⭐⭐⭐
信息矩阵 vs 协方差	\(\Lambda\) 稀疏，\(\Sigma\) 稠密	\(\Lambda = \Sigma^{-1}\)	⭐⭐
BA Schur Complement	消去点变量降维	\(S = H_{cc} - H_{cp} H_{pp}^{-1} H_{pc}\)	⭐⭐⭐
Gauge Freedom	单目 7 DOF 歧义	固定 / 先验 / 正则化	⭐⭐⭐
边缘化	信息矩阵上的 Schur complement	\(\Lambda_r = \Lambda_{rr} - \Lambda_{rm} \Lambda_{mm}^{-1} \Lambda_{mr}\)	⭐⭐⭐
FEJ	固定线性化点保一致性	第一次估计值作为线性化点	⭐⭐⭐⭐

---

累积项目：Mini-LIO 本章新增模块

本章新增：因子图后端模块 - 使用 GTSAM 构建 pose-graph - 添加里程计因子、观测因子 - 实现 Gauss-Newton / LM 求解 - 可视化 \(H\) 矩阵的稀疏模式 - 实现简单的滑动窗口（固定窗口大小 + 边缘化最旧帧）

---

延伸阅读

资料	难度	说明
Dellaert & Kaess, "Factor Graphs for Robot Perception" (2017)	⭐⭐	因子图在机器人感知中的综述
Triggs et al., "Bundle Adjustment — A Modern Synthesis" (2000)	⭐⭐⭐	BA 的经典参考文献
Kaess et al., "iSAM2" (IJRR 2012)	⭐⭐⭐	增量式因子图优化
Huang et al., "A First-Estimates Jacobian EKF" (ISER 2008)	⭐⭐⭐⭐	FEJ 的原始论文
Chen et al., "FEJ2" (ICRA 2022)	⭐⭐⭐⭐	FEJ 的改进版
Qin et al., "VINS-Mono" (TRO 2018)	⭐⭐⭐	滑动窗口 VIO 的经典实现
Strasdat et al., "Why Filter?" (ICRA 2010)	⭐⭐	EKF vs 图优化的实验对比

SLAM 理论进阶：回环检测与李群深化

---

第一部分：回环检测理论 ⭐⭐

动机：为什么需要回环检测？

SLAM 系统的前端（视觉里程计 / 激光里程计）本质上是一个**递推估计器**：每一帧的位姿都是在上一帧基础上"叠加"一个增量。这种递推结构带来一个无法回避的根本问题——累积漂移（drift）。

想象你在一栋大楼的走廊里闭着眼走路，每一步都可能偏一点点。走了一圈回到起点后，你"以为"自己在的位置和实际位置之间可能已经差了好几米。这个偏差不是某一步的错误造成的，而是成百上千步的微小误差**累积叠加**的结果。

数学上，假设每帧相对位姿估计的旋转误差为 \(\delta\theta\)，平移误差为 \(\delta t\)，经过 \(N\) 帧后：

\[ \text{旋转漂移} \sim O(\sqrt{N} \cdot \delta\theta), \quad \text{平移漂移} \sim O(\sqrt{N} \cdot \delta t) \]

这是随机游走（random walk）的性质——误差以 \(\sqrt{N}\) 的速率增长，永远不会收敛。

关键洞察：局部的帧间约束（odometry edges）只能约束相邻帧之间的相对关系，无法纠正全局漂移。唯一能"拉回"全局一致性的手段，就是检测到"我曾经来过这里"——这就是**回环检测（Loop Closure Detection）**。

回环检测一旦成功，就在位姿图中加入一条长距离的约束边（loop closure edge），连接当前帧和历史某帧。后端优化器（如 g2o、GTSAM）利用这条约束，将累积的漂移"分摊"到整个轨迹上，从而恢复全局一致性。

无回环检测	有回环检测
轨迹漂移，地图不闭合	轨迹闭合，地图全局一致
误差随时间单调增长	误差被周期性修正
只有局部约束	局部约束 + 全局约束

问题定义 ⭐

回环检测的核心问题可以形式化为：

给定当前时刻 \(t\) 的观测 \(\mathcal{Z}_t\)（一张图像或一帧点云），判断历史观测序列 \(\{\mathcal{Z}_1, \mathcal{Z}_2, \ldots, \mathcal{Z}_{t-1}\}\) 中是否存在某个 \(\mathcal{Z}_k\)，使得 \(\mathcal{Z}_t\) 和 \(\mathcal{Z}_k\) 是在同一地点拍摄的。

这个问题有两个关键挑战：

1. 效率：历史帧可能有数万张，逐一比对不可行。需要 \(O(1)\) 或 \(O(\log N)\) 的检索方法。
2. 鲁棒性：同一地点在不同时间、光照、视角下的观测可能差异很大；不同地点的观测可能看起来极为相似。

词袋模型 (Bag of Words, BoW) ⭐⭐

核心思想：从特征到"视觉单词"

词袋模型的灵感来自自然语言处理（NLP）。在 NLP 中，一篇文章可以用"词频统计"来表示——不关心词的顺序，只关心每个词出现了多少次。类似地，一张图像可以用"视觉单词的出现频率"来表示。

视觉词汇表（Visual Vocabulary）的构建过程：

1. 采集训练数据：从大量图像中提取局部特征描述子（如 ORB 描述子，每个是 256-bit 二进制向量）
2. 聚类：对所有描述子进行 K-means 聚类（或层次化聚类），得到 \(K\) 个聚类中心
3. 每个聚类中心 = 一个视觉单词（visual word）：所有聚类中心的集合就是**视觉词汇表**

为什么这样做有效？因为同类场景（如门、窗户、走廊转角）会产生相似的局部特征，这些特征会被聚到同一个 visual word。通过统计 visual words 的出现频率，我们得到了场景的一种**紧凑且鲁棒**的表示。

图像表示与 TF-IDF 加权

有了词汇表后，每张图像可以表示为一个**词频直方图** \(\mathbf{v} = (v_1, v_2, \ldots, v_K)\)，其中 \(v_i\) 表示第 \(i\) 个 visual word 的"重要程度"。

朴素方法是直接统计词频（Term Frequency），但这样做有一个问题：某些 visual word 在几乎所有图像中都出现（例如地面纹理），它们对区分场景毫无帮助。这和 NLP 中 "the"、"is" 等停用词的问题完全一样。

解决方案是 TF-IDF（Term Frequency-Inverse Document Frequency） 加权：

\[ \text{TF}_i = \frac{n_i}{n} \]

其中 \(n_i\) 是图像中属于 word \(i\) 的特征数量，\(n\) 是图像中特征总数。TF 衡量"这个词在当前图像中有多重要"。

\[ \text{IDF}_i = \log \frac{N}{N_i} \]

其中 \(N\) 是数据库中图像总数，\(N_i\) 是包含 word \(i\) 的图像数量。IDF 衡量"这个词的区分度有多高"——如果几乎每张图都有这个词，\(N_i \approx N\)，\(\text{IDF}_i \approx 0\)；如果只有少数图有这个词，IDF 值就大。

最终权重：

\[ w_i = \text{TF}_i \times \text{IDF}_i = \frac{n_i}{n} \cdot \log \frac{N}{N_i} \]

每张图像的 BoW 向量为 \(\mathbf{v} = (w_1, w_2, \ldots, w_K)\)，通常还做 L1 或 L2 归一化。

相似度度量

有了两张图像的 BoW 向量 \(\mathbf{v}_a\) 和 \(\mathbf{v}_b\) 后，用相似度函数衡量它们有多"像"：

L1-score（DBoW2 默认）：

\[ s(\mathbf{v}_a, \mathbf{v}_b) = 1 - \frac{1}{2} \|\mathbf{v}_a - \mathbf{v}_b\|_1 = 1 - \frac{1}{2} \sum_{i=1}^{K} |v_a^{(i)} - v_b^{(i)}| \]

余弦相似度：

\[ s(\mathbf{v}_a, \mathbf{v}_b) = \frac{\mathbf{v}_a \cdot \mathbf{v}_b}{\|\mathbf{v}_a\| \cdot \|\mathbf{v}_b\|} \]

得分越高，两张图越相似。通常还会对得分做归一化：\(s_{\text{normalized}} = s(\mathbf{v}_a, \mathbf{v}_b) / s(\mathbf{v}_a, \mathbf{v}_{\text{prev}})\)，其中 \(\mathbf{v}_{\text{prev}}\) 是上一帧，用来排除"因为环境整体相似导致所有帧得分都高"的情况。

DBoW2 详解 (Galvez-Lopez & Tardos, 2012) ⭐⭐

DBoW2 是视觉 SLAM 中使用最广泛的回环检测库（ORB-SLAM、ORB-SLAM2、ORB-SLAM3 均使用它）。它在基本 BoW 模型上做了三个关键改进：层次化词汇树、倒排索引、正排索引。

层次化词汇树（Hierarchical Vocabulary Tree）

朴素 BoW 需要将每个描述子与 \(K\) 个 visual words 逐一比对，复杂度 \(O(K)\)。当 \(K\) 很大时（如 \(10^6\)），这太慢了。

DBoW2 用**层次化树结构**加速查找：

• 分支因子 \(k\)（branching factor）：每个节点有 \(k\) 个子节点
• 层数 \(L\)（depth levels）：树的深度
• 叶节点数 = \(k^L\)：每个叶节点就是一个 visual word

例如 ORB-SLAM 使用 \(k=10, L=6\)，得到 \(10^6 = 100\) 万个 visual words。

查找过程（将一个描述子分配到 visual word）：

1. 从根节点开始，比较描述子与 \(k\) 个子节点的中心，选最近的
2. 进入该子节点，重复上述过程
3. 经过 \(L\) 层到达叶节点，该叶节点就是对应的 visual word

复杂度从 \(O(k^L)\) 降到了 \(O(k \cdot L)\)——对于 \(k=10, L=6\)，这是从 \(10^6\) 降到 \(60\)，加速了 \(1.6 \times 10^4\) 倍。

倒排索引（Inverted Index）

倒排索引的数据结构是：visual word \(\rightarrow\) 包含该 word 的所有图像 ID 列表。

\[ \text{InvertedIndex}[w_i] = \{(\text{img}_1, \text{weight}_1), (\text{img}_2, \text{weight}_2), \ldots\} \]

当查询图像 \(\mathcal{Z}_q\) 到来时： 1. 提取其所有特征，量化到 visual words，得到 \(\{w_1, w_2, \ldots, w_m\}\) 2. 对每个 \(w_j\)，查倒排索引，找到所有包含 \(w_j\) 的历史图像 3. 累加得分：对每个候选图像，其得分是所有共享 word 的 TF-IDF 权重之和

这实现了 \(O(1)\)（均摊）的候选检索——只需要访问查询图像包含的那些 words 对应的倒排列表，而不需要遍历整个数据库。

正排索引（Direct Index）

正排索引的数据结构是：图像 ID \(\rightarrow\) 该图像在词汇树各中间节点上的特征列表。

\[ \text{DirectIndex}[\text{img}_k] = \{(\text{node}_j, [\text{feat}_{j1}, \text{feat}_{j2}, \ldots])\} \]

正排索引的作用不是用于候选检索，而是用于**加速特征匹配**。当回环检测找到候选图像对 \((I_a, I_b)\) 后，需要在它们之间做精确的特征匹配来验证几何一致性。

不用正排索引：需要在 \(I_a\) 的所有特征和 \(I_b\) 的所有特征之间暴力匹配，\(O(n^2)\)。

用正排索引：只需在**同一词汇树节点**下的特征之间匹配。如果在词汇树的第 \(l\) 层做匹配（\(l < L\)），每个节点下平均只有 \(n/k^l\) 个特征，匹配量大幅减少。ORB-SLAM 在第 \(l=2\) 或 \(l=3\) 层做特征匹配。

时间一致性检验（Temporal Consistency）

单帧的 BoW 匹配可能出现偶然的高得分（误检）。DBoW2 引入**时间一致性检验**来提高可靠性：

• 不依赖单帧结果，而是要求**连续 \(N\) 帧**（通常 \(N=3\)）都检测到同一个回环候选区域
• 具体做法：用时间窗口（temporal grouping）将连续帧的候选聚合，只有当一个候选在窗口内持续出现时才通过
• 直觉：如果你真的回到了一个地方，连续多帧都应该能匹配上；如果只是偶然的感知混叠，很难连续多帧都巧合

几何验证 ⭐⭐

BoW 只衡量了**外观相似度**，但外观相似不等于是同一地点。**几何验证**是回环检测的第二道关卡——它检查两张图像之间是否存在合理的几何变换。

几何验证流程：

1. 特征匹配：利用正排索引，在候选图像对之间找到特征匹配点对 \(\{(\mathbf{p}_i^a, \mathbf{p}_i^b)\}\)
2. RANSAC + 模型估计：
3. 单目相机：估计本质矩阵 \(E\) 或基础矩阵 \(F\)
4. 双目 / RGB-D：估计 3D-3D 的刚体变换 \([R|t]\)
5. ORB-SLAM3：估计 Sim(3) 变换（包含尺度），因为单目 SLAM 的尺度可能漂移
6. 判断 inlier 比例：如果 RANSAC 找到的内点（inlier）数量超过阈值（如 ORB-SLAM3 中 \(\geq 20\) 个），则确认回环

ORB-SLAM3 的具体流程：

\[ \text{BoW 候选} \xrightarrow{\text{特征匹配}} \text{匹配对} \xrightarrow{\text{RANSAC + Sim3}} \hat{S}_{ij} \xrightarrow{\text{inlier} > \tau} \text{确认回环} \]

其中 \(\hat{S}_{ij} \in \text{Sim}(3)\) 是 7 自由度的相似变换（3 旋转 + 3 平移 + 1 尺度），解决了单目 SLAM 中尺度漂移的问题。

深度学习方法 ⭐⭐⭐

传统 BoW 方法依赖手工特征（ORB、BRIEF 等），在场景外观剧烈变化时（白天/夜晚、季节变化、天气变化）性能急剧下降。深度学习方法通过端到端训练，学习对这些变化更鲁棒的表示。

NetVLAD (Arandjelovic et al., CVPR 2016)

NetVLAD 的核心创新是将传统的 VLAD（Vector of Locally Aggregated Descriptors）编码嵌入到 CNN 中，使其可以端到端训练。

架构：

1. CNN 主干网络（如 VGG-16）：提取 \(H \times W \times D\) 的特征图，每个空间位置是一个 \(D\) 维局部描述子
2. NetVLAD 层：将所有局部描述子聚合为一个固定长度的全局描述子
3. 有 \(K\) 个聚类中心 \(\{\mathbf{c}_k\}\)（可训练参数）
4. 对每个描述子 \(\mathbf{x}_i\)，用 softmax 计算其对每个中心的软分配权重 \(\bar{a}_k(\mathbf{x}_i)\)
5. VLAD 核心：\(\mathbf{V}(j,k) = \sum_{i=1}^{N} \bar{a}_k(\mathbf{x}_i)(x_i(j) - c_k(j))\)
6. 输出：将 \(K \times D\) 的矩阵 \(\mathbf{V}\) 展平并做列内 L2 归一化 + 全局 L2 归一化，得到全局描述子
7. 训练：使用弱监督三元组损失（triplet ranking loss），从 Google Street View Time Machine 数据中训练

相比 BoW 的优势： - 对光照、季节变化鲁棒（CNN 学到了不变性特征） - 全局描述子，无需逐 word 匹配 - 可在 GPU 上高效计算

Patch-NetVLAD (Hausler et al., CVPR 2021)

Patch-NetVLAD 在 NetVLAD 的基础上引入局部特征匹配： - 先用 NetVLAD 全局描述子做粗筛（top-K 候选） - 再对候选图像做 patch 级别的局部匹配验证 - 结合了全局检索的效率和局部匹配的精度

趋势与对比

方法	表示类型	白天/夜晚	季节变化	计算速度	内存需求
DBoW2	稀疏 BoW 向量	差	差	极快（CPU）	低
NetVLAD	全局密集向量	好	好	快（GPU）	中
Patch-NetVLAD	全局 + 局部	很好	很好	中等	高

当前趋势：学习型方法在外观变化大的场景中显著优于传统 BoW，但在计算资源受限（如嵌入式平台）或外观变化小的室内环境中，DBoW2 仍然是实用的首选。ORB-SLAM3 至今仍使用 DBoW2，因为在其目标场景下足够好且极其高效。

⚠️ 常见陷阱

概念误区：感知混叠（Perceptual Aliasing）

新手想法："BoW 得分高就说明是同一个地方"

实际上：不同地方可能看起来一模一样——走廊的两端、重复的办公室门口、对称的建筑立面。这些场景的 BoW 向量可能几乎相同，导致**虚假回环（false positive）**。

后果：虚假回环会在位姿图中加入一条**错误的约束**，后端优化器会试图满足这条约束，导致整个地图被严重扭曲——这比没有回环还糟糕！

根本原因：BoW 丢弃了空间结构信息，只保留了"有什么"而没有保留"在哪里"。两个空间布局完全不同的场景，只要包含相似的局部特征，就会产生高 BoW 得分。

正确做法：始终在 BoW 之后做几何验证（RANSAC）。宁可漏掉真回环（false negative），也不能引入假回环（false positive）。因为漏掉回环只是地图不够准，引入假回环会毁掉整个地图。

思维陷阱：回环检测的代价不对称性

新手想法："应该尽量提高回环检测的召回率，不要漏掉任何回环"

实际上：False Positive（误报）和 False Negative（漏报）的代价严重不对称： - FP（虚假回环）：灾难性后果——整个地图扭曲，不可恢复 - FN（遗漏真回环）：温和后果——地图有漂移，但局部一致性不受影响

正确思维：回环检测系统应该**高精度优先**（precision > recall）。实际系统中，宁可 recall 只有 30%（漏掉 70% 的真回环），也要保证 precision 接近 100%（几乎零误报）。

自检方法：在你的回环检测模块中加入 ground truth 评估，画 Precision-Recall 曲线，确保在 100% precision 处的 recall 值。

编程陷阱：忽略 BoW 得分归一化

错误做法：直接用原始 BoW 相似度得分做阈值判断

现象：在某些环境中几乎每帧都触发回环，在另一些环境中从不触发

根本原因：不同环境的整体纹理丰富度不同。在纹理丰富的环境中，相邻帧的 BoW 得分本身就很高（如 0.9），回环候选的得分可能是 0.85；在纹理贫乏的环境中，相邻帧得分可能只有 0.3，回环候选可能是 0.25。用固定阈值无法适应两种情况。

正确做法：使用相对归一化——将候选得分除以"与上一帧的相似度"或"近邻帧的中位数相似度"。ORB-SLAM 使用 \(s_{\text{norm}} = s(\mathbf{v}_q, \mathbf{v}_c) / s(\mathbf{v}_q, \mathbf{v}_{q-1})\)。

练习

练习 1（概念，⭐⭐）：假设词汇表有 5 个 visual words，数据库中有 3 张图像，各图像的词频如下表。IDF 权重已给出。手工计算图像 A 和 B 的 TF-IDF 向量及 L1 相似度得分。

Word	图像A词频	图像B词频	图像C词频	IDF
\(w_1\)	3	0	1	0.41
\(w_2\)	0	2	2	0.41
\(w_3\)	2	3	0	0.41
\(w_4\)	5	4	5	0.00
\(w_5\)	0	1	0	1.10

提示：\(w_4\) 的 IDF 为 0 说明什么？这对相似度计算有何影响？

练习 2（编程，⭐⭐⭐）：用 C++ 和 DBoW2 库实现一个最小回环检测器。给出以下代码框架，填写 TODO 部分：

#include <DBoW2/DBoW2.h>
#include <opencv2/features2d.hpp>

// 已有: ORB 词汇表 vocab, 图像序列 images
OrbDatabase db(vocab, false, 0); // 不使用 direct index

for (size_t i = 0; i < images.size(); i++) {
    // TODO 1: 提取 ORB 特征并转化为 BoW 向量
    // TODO 2: 查询数据库, 获取候选回环 (排除最近 50 帧)
    // TODO 3: 对得分最高的候选做时间一致性检查
    // TODO 4: 将当前帧加入数据库
}

要求：解释为什么需要"排除最近 50 帧"。

练习 3（思考，⭐⭐⭐⭐）：在一个高度对称的环境中（如方形走廊，四个转角看起来完全相同），BoW + 几何验证仍然可能产生虚假回环。请分析为什么几何验证也会失败，并提出至少两种改进策略。

---

第二部分：李群理论深化 ⭐⭐⭐

动机：为什么需要超越基础李群知识？

在之前的笔记中，我们学习了 \(SO(3)\)/\(SE(3)\) 的定义、指数映射（\(\exp\)）、对数映射（\(\log\)）和 Rodrigues 公式。这些基础工具让我们能在旋转和刚体变换上做加减法——把李群问题映射到线性的李代数上处理。

但当你真正开始做 **SLAM 优化**时，你会发现这些基础工具不够用。具体来说，有以下三个场景需要更深入的工具：

场景 1：位姿的增量更新

优化器算出了一个李代数上的增量 \(\delta\boldsymbol{\phi}\)，需要更新当前的旋转估计 \(R\)。直觉上应该是 \(R_{\text{new}} = R \cdot \text{Exp}(\delta\boldsymbol{\phi})\)。但如果你想在李代数上表达这个更新——即找到 \(\boldsymbol{\phi}_{\text{new}}\) 使得 \(\exp(\boldsymbol{\phi}_{\text{new}}^{\wedge}) = \exp(\boldsymbol{\phi}^{\wedge}) \cdot \exp(\delta\boldsymbol{\phi}^{\wedge})\)——你就需要 BCH 公式。

场景 2：雅可比矩阵的计算

非线性优化的核心是计算残差关于状态量的雅可比矩阵。当状态量是李群元素时，这个雅可比矩阵的推导中不可避免地出现**右雅可比 \(\mathbf{J}_r\)** 和**左雅可比 \(\mathbf{J}_l\)**。例如 IMU 预积分（Forster et al., 2017）中的雅可比推导就大量使用这两个工具。

场景 3：不确定性传播

SLAM 不只估计位姿的最优值，还要估计其不确定性（协方差矩阵 \(\Sigma\)）。当两个带不确定性的位姿做复合运算 \(T_{12} = T_1 \cdot T_2\) 时，如何从 \(\Sigma_1, \Sigma_2\) 算出 \(\Sigma_{12}\)？这需要**伴随表示（Adjoint）**来"搬运"不同参考系下的协方差。

没有这些工具会怎样？ 你会发现： - 读 ORB-SLAM3、VINS-Mono、LIO-SAM 的论文时，公式推导看不懂 - 写优化器时不知道雅可比该怎么求 - 多传感器融合时不知道协方差该怎么传播

BCH 近似 (Baker-Campbell-Hausdorff) ⭐⭐⭐

问题背景：李群乘法与李代数加法的关系

在实数域中，\(e^a \cdot e^b = e^{a+b}\)——指数映射把加法变成了乘法。但对矩阵来说，当 \(A\) 和 \(B\) 不可交换（\(AB \neq BA\)）时，\(e^A \cdot e^B \neq e^{A+B}\)。

这就带来了一个根本问题：我们知道如何在李群上做乘法（矩阵相乘），也知道如何在李代数上做加法（向量相加），但**李群上的乘法对应于李代数上的什么运算？**

BCH 公式给出了答案。

历史注记：该公式由三位数学家独立发现：Henry Frederick Baker（1905）、John Edward Campbell（1897）和 Felix Hausdorff（1906）。它是李群理论中最深刻的结果之一，连接了群的"全局"乘法结构和代数的"局部"线性结构。

完整 BCH 公式

对于李代数元素 \(\boldsymbol{\phi}_1, \boldsymbol{\phi}_2\)，它们对应的李群元素之乘积为：

\[ \ln\!\left(\exp(\boldsymbol{\phi}_1^{\wedge}) \cdot \exp(\boldsymbol{\phi}_2^{\wedge})\right)^{\vee} = \boldsymbol{\phi}_1 + \boldsymbol{\phi}_2 + \frac{1}{2}[\boldsymbol{\phi}_1, \boldsymbol{\phi}_2] + \frac{1}{12}\big([\boldsymbol{\phi}_1, [\boldsymbol{\phi}_1, \boldsymbol{\phi}_2]] + [\boldsymbol{\phi}_2, [\boldsymbol{\phi}_2, \boldsymbol{\phi}_1]]\big) + \cdots \]

其中 \([\cdot, \cdot]\) 是李括号（Lie bracket）。对于矩阵李代数，\([\boldsymbol{\phi}_1, \boldsymbol{\phi}_2] = \boldsymbol{\phi}_1^{\wedge} \boldsymbol{\phi}_2^{\wedge} - \boldsymbol{\phi}_2^{\wedge} \boldsymbol{\phi}_1^{\wedge}\)（矩阵的对易子/交换子）。

关键观察：如果 \(\boldsymbol{\phi}_1\) 和 \(\boldsymbol{\phi}_2\) 可交换（\([\boldsymbol{\phi}_1, \boldsymbol{\phi}_2] = 0\)），则所有高阶项消失，回到简单的 \(\boldsymbol{\phi}_1 + \boldsymbol{\phi}_2\)。不可交换性是一切复杂性的来源。

对于 \(so(3)\)，李括号就是向量叉乘：\([\boldsymbol{\phi}_1, \boldsymbol{\phi}_2] = \boldsymbol{\phi}_1 \times \boldsymbol{\phi}_2\)。两个平行的旋转轴对应的旋转是可交换的（\(\boldsymbol{\phi}_1 \times \boldsymbol{\phi}_2 = \mathbf{0}\)），这与直觉一致——绕同一轴的两次旋转可以直接相加。

一阶近似：小扰动情况

在 SLAM 优化中，我们最常遇到的情况是：一个"大的"旋转 \(\boldsymbol{\phi}\) 加上一个"小的"扰动 \(\delta\boldsymbol{\phi}\)。此时 BCH 公式可以做一阶近似。

右扰动模型（扰动乘在右边）：

\[ \ln\!\left(\exp(\boldsymbol{\phi}^{\wedge}) \cdot \exp(\delta\boldsymbol{\phi}^{\wedge})\right)^{\vee} \approx \boldsymbol{\phi} + \mathbf{J}_r^{-1}(\boldsymbol{\phi}) \cdot \delta\boldsymbol{\phi} \]

推导思路：

Step 1：展开 BCH 公式，只保留对 \(\delta\boldsymbol{\phi}\) 的线性项（\(\delta\boldsymbol{\phi}\) 是小量，其高次项忽略）：

\[ \ln(\exp(\boldsymbol{\phi}^{\wedge}) \cdot \exp(\delta\boldsymbol{\phi}^{\wedge}))^{\vee} \approx \boldsymbol{\phi} + \delta\boldsymbol{\phi} + \frac{1}{2}[\boldsymbol{\phi}, \delta\boldsymbol{\phi}] + \frac{1}{12}[\boldsymbol{\phi}, [\boldsymbol{\phi}, \delta\boldsymbol{\phi}]] + \cdots \]

Step 2：注意到所有保留的项都是 \(\delta\boldsymbol{\phi}\) 的线性函数，因此可以提取出一个矩阵 \(\mathbf{J}_r^{-1}(\boldsymbol{\phi})\) 使得：

\[ \boldsymbol{\phi} + \delta\boldsymbol{\phi} + \frac{1}{2}[\boldsymbol{\phi}, \delta\boldsymbol{\phi}] + \frac{1}{12}[\boldsymbol{\phi}, [\boldsymbol{\phi}, \delta\boldsymbol{\phi}]] + \cdots = \boldsymbol{\phi} + \mathbf{J}_r^{-1}(\boldsymbol{\phi}) \cdot \delta\boldsymbol{\phi} \]

Step 3：利用 \(so(3)\) 中李括号的性质 \([\boldsymbol{\phi}, \delta\boldsymbol{\phi}] = [\boldsymbol{\phi}]_{\times} \delta\boldsymbol{\phi}\)（反对称矩阵乘向量 = 叉乘），得到：

\[ \mathbf{J}_r^{-1}(\boldsymbol{\phi}) = \mathbf{I} + \frac{1}{2}[\boldsymbol{\phi}]_{\times} + \frac{1}{12}[\boldsymbol{\phi}]_{\times}^2 + \cdots \]

这就是右雅可比逆的级数展开形式。

左扰动模型（扰动乘在左边）：

\[ \ln\!\left(\exp(\delta\boldsymbol{\phi}^{\wedge}) \cdot \exp(\boldsymbol{\phi}^{\wedge})\right)^{\vee} \approx \boldsymbol{\phi} + \mathbf{J}_l^{-1}(\boldsymbol{\phi}) \cdot \delta\boldsymbol{\phi} \]

为什么 BCH 近似如此重要？ 它建立了一座桥梁——把李群上的乘法（非线性）关联到了李代数上的线性运算。有了这个近似，我们就可以在李代数上做加减法来代替李群上的乘除法，这正是 SLAM 优化器所需要的。具体地，优化器计算出增量 \(\delta\boldsymbol{\phi}\) 后，通过右扰动模型更新状态：\(R_{\text{new}} = R \cdot \text{Exp}(\delta\boldsymbol{\phi})\)，而 BCH 近似告诉我们对应的李代数更新为 \(\boldsymbol{\phi}_{\text{new}} \approx \boldsymbol{\phi} + \mathbf{J}_r^{-1}(\boldsymbol{\phi}) \delta\boldsymbol{\phi}\)。

右雅可比与左雅可比 (\(\mathbf{J}_r\) and \(\mathbf{J}_l\)) ⭐⭐⭐

定义

右雅可比 \(\mathbf{J}_r(\boldsymbol{\phi})\) 定义为指数映射关于右扰动的微分：

\[ \exp\!\left((\boldsymbol{\phi} + \delta\boldsymbol{\phi})^{\wedge}\right) = \exp(\boldsymbol{\phi}^{\wedge}) \cdot \exp\!\left((\mathbf{J}_r(\boldsymbol{\phi}) \cdot \delta\boldsymbol{\phi})^{\wedge}\right) \]

直观理解：当李代数向量从 \(\boldsymbol{\phi}\) 变化了 \(\delta\boldsymbol{\phi}\) 时，对应的李群元素的变化**不是**简单地右乘 \(\exp(\delta\boldsymbol{\phi}^{\wedge})\)，而是右乘 \(\exp((\mathbf{J}_r \cdot \delta\boldsymbol{\phi})^{\wedge})\)。\(\mathbf{J}_r\) 就是那个"修正因子"。

为什么不是直接 \(\delta\boldsymbol{\phi}\)？ 因为李代数到李群的映射（指数映射）是非线性的。\(\mathbf{J}_r\) 就是这个非线性映射在 \(\boldsymbol{\phi}\) 处的局部线性近似——它描述了"李代数中的微小平移"如何映射为"李群中的微小移动"。当 \(\boldsymbol{\phi} = \mathbf{0}\) 时 \(\mathbf{J}_r = \mathbf{I}\)，表示在原点附近指数映射就是恒等映射。

SO(3) 的闭合表达式推导

令 \(\boldsymbol{\phi} = \theta \hat{\mathbf{a}}\)，其中 \(\theta = \|\boldsymbol{\phi}\|\) 是旋转角度，\(\hat{\mathbf{a}} = \boldsymbol{\phi}/\theta\) 是单位旋转轴。

Step 1：从级数定义出发。利用指数映射的 Taylor 展开和右雅可比的定义，可以得到：

\[ \mathbf{J}_r(\boldsymbol{\phi}) = \sum_{n=0}^{\infty} \frac{(-1)^n}{(n+1)!} ([\boldsymbol{\phi}]_{\times})^n = \mathbf{I} - \frac{1}{2!}[\boldsymbol{\phi}]_{\times} + \frac{1}{3!}[\boldsymbol{\phi}]_{\times}^2 - \frac{1}{4!}[\boldsymbol{\phi}]_{\times}^3 + \cdots \]

Step 2：利用 \(so(3)\) 的循环性质。对于 \([\boldsymbol{\phi}]_{\times} = \theta[\hat{\mathbf{a}}]_{\times}\)，有关键性质：

\[ [\boldsymbol{\phi}]_{\times}^3 = -\theta^2 [\boldsymbol{\phi}]_{\times} \]

由此可以归纳得到： - 奇数次幂：\([\boldsymbol{\phi}]_{\times}^{2k+1} = (-1)^k \theta^{2k} [\boldsymbol{\phi}]_{\times}\) - 偶数次幂：\([\boldsymbol{\phi}]_{\times}^{2k+2} = (-1)^k \theta^{2k} [\boldsymbol{\phi}]_{\times}^2\)

Step 3：将级数按奇偶分组：

\[ \mathbf{J}_r = \mathbf{I} + \left(\sum_{k=0}^{\infty} \frac{(-1)^{k+1} \theta^{2k}}{(2k+2)!}\right) [\boldsymbol{\phi}]_{\times} + \left(\sum_{k=0}^{\infty} \frac{(-1)^{k+1} \theta^{2k}}{(2k+3)!}\right) [\boldsymbol{\phi}]_{\times}^2 \]

Step 4：识别已知的 Taylor 级数。第一个括号内的求和可以化为：

\[ \sum_{k=0}^{\infty} \frac{(-1)^{k+1} \theta^{2k}}{(2k+2)!} = -\frac{1}{\theta^2}\sum_{k=0}^{\infty} \frac{(-1)^{k} \theta^{2k+2}}{(2k+2)!} = -\frac{1}{\theta^2}(\cos\theta - 1) = \frac{1-\cos\theta}{\theta^2} \]

等等，让我们更仔细地追踪符号。级数的第 \(n\) 项是 \(\frac{(-1)^n}{(n+1)!}[\boldsymbol{\phi}]_{\times}^n\)，对于 \(n=1\)（奇数，\(k=0\)）：\(\frac{(-1)^1}{2!}[\boldsymbol{\phi}]_{\times} = -\frac{1}{2}[\boldsymbol{\phi}]_{\times}\)，是负号。合并后，\([\boldsymbol{\phi}]_{\times}\) 项的系数为 \(-\frac{1-\cos\theta}{\theta^2}\)。

类似地，\([\boldsymbol{\phi}]_{\times}^2\) 项的系数：

\[ \sum_{k=0}^{\infty} \frac{(-1)^{k+1} \theta^{2k}}{(2k+3)!} = -\frac{1}{\theta^3}\sum_{k=0}^{\infty} \frac{(-1)^{k} \theta^{2k+3}}{(2k+3)!} = -\frac{1}{\theta^3}\sin\theta + \frac{1}{\theta^2} \]

仔细化简后得到系数为 \(\frac{\theta - \sin\theta}{\theta^3}\)。

最终结果——SO(3) 右雅可比的闭合形式：

\[ \boxed{\mathbf{J}_r(\boldsymbol{\phi}) = \mathbf{I} - \frac{1 - \cos\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^2} [\boldsymbol{\phi}]_{\times} + \frac{\|\boldsymbol{\phi}\| - \sin\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^3} [\boldsymbol{\phi}]_{\times}^2} \]

左右雅可比的关系

\[ \mathbf{J}_l(\boldsymbol{\phi}) = \mathbf{J}_r(-\boldsymbol{\phi}) \]

直观理解：左扰动和右扰动的区别在于"从哪边乘"。把 \(\boldsymbol{\phi}\) 取负号，就把左右互换了。

展开验证：将 \(-\boldsymbol{\phi}\) 代入右雅可比公式。注意 \([-\boldsymbol{\phi}]_{\times} = -[\boldsymbol{\phi}]_{\times}\) 且 \((-[\boldsymbol{\phi}]_{\times})^2 = [\boldsymbol{\phi}]_{\times}^2\)，所以：

\[ \mathbf{J}_l(\boldsymbol{\phi}) = \mathbf{I} + \frac{1-\cos\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^2} [\boldsymbol{\phi}]_{\times} + \frac{\|\boldsymbol{\phi}\|-\sin\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^3} [\boldsymbol{\phi}]_{\times}^2 \]

与 \(\mathbf{J}_r\) 相比，唯一的区别是 \([\boldsymbol{\phi}]_{\times}\) 项的符号**从负变为正**。

另一个重要的等式是 \(\mathbf{J}_l(\boldsymbol{\phi}) = R \cdot \mathbf{J}_r(\boldsymbol{\phi})\)，其中 \(R = \exp(\boldsymbol{\phi}^{\wedge})\)。由于 \(R\) 是正交矩阵（\(R^T = R^{-1}\)），这也可以写为 \(\mathbf{J}_l = \mathbf{J}_r^T\)。注意：这里是转置关系，不是逆转置关系。

右雅可比的逆

在 BCH 近似中直接出现的是 \(\mathbf{J}_r^{-1}\) 而非 \(\mathbf{J}_r\)。其闭合形式为：

\[ \boxed{\mathbf{J}_r^{-1}(\boldsymbol{\phi}) = \mathbf{I} + \frac{1}{2}[\boldsymbol{\phi}]_{\times} + \left(\frac{1}{\|\boldsymbol{\phi}\|^2} - \frac{1+\cos\|\boldsymbol{\phi}\|}{2\|\boldsymbol{\phi}\|\sin\|\boldsymbol{\phi}\|}\right)[\boldsymbol{\phi}]_{\times}^2} \]

验证：当 \(\boldsymbol{\phi} \to \mathbf{0}\) 时，\(\mathbf{J}_r \to \mathbf{I}\)，\(\mathbf{J}_r^{-1} \to \mathbf{I}\)——在零点附近，李群和李代数"几乎一样"，修正因子趋近于单位矩阵。

数值注意：当 \(\|\boldsymbol{\phi}\| \approx 0\) 时，上述公式有 \(0/0\) 型奇异。实际编程中需要用 Taylor 展开处理小角度情况：

\[ \mathbf{J}_r(\boldsymbol{\phi}) \approx \mathbf{I} - \frac{1}{2}[\boldsymbol{\phi}]_{\times} + \frac{1}{6}[\boldsymbol{\phi}]_{\times}^2, \quad \text{当 } \|\boldsymbol{\phi}\| < \epsilon \]

伴随表示 (Adjoint Representation) ⭐⭐⭐

动机：左扰动与右扰动的转换

在 SLAM 优化中，有时你用右扰动模型推导了雅可比，但需要的是左扰动模型（或反过来）。伴随表示就是在两者之间做转换的工具。

核心性质：

\[ T \cdot \exp(\boldsymbol{\xi}^{\wedge}) \cdot T^{-1} = \exp\!\left((\text{Ad}_T \cdot \boldsymbol{\xi})^{\wedge}\right) \]

其中 \(\text{Ad}_T\) 是群元素 \(T\) 的**伴随矩阵**。这个等式的含义是："先扰动再变换"等价于"先变换再做一个经过 Adjoint 修正的扰动"。

直观类比：想象你站在世界坐标系中，往前走一步（扰动 \(\boldsymbol{\xi}\)），然后旋转（变换 \(T\)）。这等价于先旋转，然后在旋转后的坐标系中走"相应的一步"——但"相应的一步"不再是原来的 \(\boldsymbol{\xi}\)，而是经过 \(\text{Ad}_T\) 变换后的 \(\text{Ad}_T \cdot \boldsymbol{\xi}\)。

SO(3) 的伴随

对于 \(SO(3)\)，伴随表示特别简单：

\[ \text{Ad}_R = R \]

即：\(R \cdot \exp(\boldsymbol{\phi}^{\wedge}) \cdot R^T = \exp((R\boldsymbol{\phi})^{\wedge})\)。

推导：利用反对称矩阵的核心性质 \(R[\boldsymbol{\phi}]_{\times}R^T = [R\boldsymbol{\phi}]_{\times}\)（这个性质可以通过验证两边对任意向量 \(\mathbf{v}\) 的作用相等来证明），逐项展开指数映射的 Taylor 级数：

\[ R \exp(\boldsymbol{\phi}^{\wedge}) R^T = R \left(\mathbf{I} + [\boldsymbol{\phi}]_{\times} + \frac{1}{2}[\boldsymbol{\phi}]_{\times}^2 + \cdots\right) R^T = \mathbf{I} + [R\boldsymbol{\phi}]_{\times} + \frac{1}{2}[R\boldsymbol{\phi}]_{\times}^2 + \cdots = \exp((R\boldsymbol{\phi})^{\wedge}) \]

几何含义：旋转一个"小旋转"的轴，等价于在旋转后的坐标系中做同样角度的旋转。

SE(3) 的伴随

\(SE(3)\) 的李代数 \(\mathfrak{se}(3)\) 是 6 维的：\(\boldsymbol{\xi} = \begin{pmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{pmatrix}\)，其中 \(\boldsymbol{\rho}\) 是平移部分，\(\boldsymbol{\phi}\) 是旋转部分。

对于 \(T = \begin{pmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix} \in SE(3)\)，伴随矩阵是一个 \(6 \times 6\) 矩阵：

\[ \boxed{\text{Ad}_T = \begin{pmatrix} R & [\mathbf{t}]_{\times} R \\ \mathbf{0}_{3\times3} & R \end{pmatrix} \in \mathbb{R}^{6 \times 6}} \]

推导：

Step 1：写出 SE(3) 的逆：\(T^{-1} = \begin{pmatrix} R^T & -R^T\mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix}\)

Step 2：对于小的 \(\boldsymbol{\xi}\)，\(\exp(\boldsymbol{\xi}^{\wedge}) \approx \mathbf{I} + \boldsymbol{\xi}^{\wedge} = \begin{pmatrix} \mathbf{I} + [\boldsymbol{\phi}]_{\times} & \boldsymbol{\rho} \\ \mathbf{0}^T & 1 \end{pmatrix}\)

Step 3：计算 \(T \cdot (\mathbf{I} + \boldsymbol{\xi}^{\wedge}) \cdot T^{-1}\)：

\[ = \begin{pmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix} \begin{pmatrix} \mathbf{I} + [\boldsymbol{\phi}]_{\times} & \boldsymbol{\rho} \\ \mathbf{0}^T & 1 \end{pmatrix} \begin{pmatrix} R^T & -R^T\mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix} \]

Step 4：展开矩阵乘法（只保留一阶项），提取旋转和平移分量，可以验证结果等于 \(\mathbf{I} + (\text{Ad}_T \boldsymbol{\xi})^{\wedge}\)，其中 \(\text{Ad}_T\) 正是上述 \(6 \times 6\) 矩阵。

SE(3) 伴随的分块结构解读：

\[ \text{Ad}_T \begin{pmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{pmatrix} = \begin{pmatrix} R\boldsymbol{\rho} + [\mathbf{t}]_{\times}R\boldsymbol{\phi} \\ R\boldsymbol{\phi} \end{pmatrix} \]

• 旋转部分 \(R\boldsymbol{\phi}\)：和 \(SO(3)\) 一样，旋转轴被 \(R\) 旋转
• 平移部分 \(R\boldsymbol{\rho} + [\mathbf{t}]_{\times}R\boldsymbol{\phi}\)：不仅平移向量被旋转（\(R\boldsymbol{\rho}\)），还多了一个**耦合项** \([\mathbf{t}]_{\times}R\boldsymbol{\phi}\)——旋转会通过力臂 \(\mathbf{t}\) 产生额外的平移效果（类似于"扭矩 = 力臂 \(\times\) 力"）

伴随的关键应用：左右扰动转换

利用伴随性质，可以在左右扰动之间自由转换：

\[ \underbrace{T \cdot \exp(\boldsymbol{\xi}^{\wedge})}_{\text{右扰动}} = \underbrace{\exp\!\left((\text{Ad}_T \cdot \boldsymbol{\xi})^{\wedge}\right) \cdot T}_{\text{左扰动，但扰动量变为 } \text{Ad}_T \boldsymbol{\xi}} \]

这在推导雅可比时极其有用——如果右扰动的雅可比难算，可以先算左扰动的，再用伴随转换。

流形上的不确定性传播 ⭐⭐⭐⭐

李群上的高斯分布

欧几里得空间中的高斯分布 \(\mathbf{x} \sim \mathcal{N}(\bar{\mathbf{x}}, \Sigma)\) 有明确的定义。但旋转矩阵 \(R \in SO(3)\) 不在欧几里得空间中——你不能简单地说"\(R \sim \mathcal{N}(\bar{R}, \Sigma)\)"，因为 \(\bar{R} + \epsilon\) 不再是旋转矩阵。

解决方案：在切空间（李代数）上定义高斯分布，通过指数映射投射到李群上。

右扰动模型（实际系统最常用）：

\[ X = \bar{X} \cdot \text{Exp}(\boldsymbol{\epsilon}), \quad \boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0}, \Sigma) \]

含义：真实位姿 \(X\) 是均值 \(\bar{X}\) 加上一个李代数中的小随机扰动 \(\boldsymbol{\epsilon}\)，扰动从右边乘进去。\(\Sigma\) 是 \(6 \times 6\)（对于 \(SE(3)\)）或 \(3 \times 3\)（对于 \(SO(3)\)）的协方差矩阵，定义在李代数上。

左扰动模型：

\[ X = \text{Exp}(\boldsymbol{\epsilon}') \cdot \bar{X}, \quad \boldsymbol{\epsilon}' \sim \mathcal{N}(\mathbf{0}, \Sigma') \]

两种模型的协方差之间的关系正是通过伴随给出的：

\[ \Sigma' = \text{Ad}_{\bar{X}} \cdot \Sigma \cdot \text{Ad}_{\bar{X}}^T \]

这是因为 \(\bar{X} \cdot \text{Exp}(\boldsymbol{\epsilon}) = \text{Exp}(\text{Ad}_{\bar{X}} \boldsymbol{\epsilon}) \cdot \bar{X}\)，所以 \(\boldsymbol{\epsilon}' = \text{Ad}_{\bar{X}} \boldsymbol{\epsilon}\)，对其取协方差即得上式。

复合运算的不确定性传播

问题：已知两个带不确定性的位姿：

\[ X_1 = \bar{X}_1 \cdot \text{Exp}(\boldsymbol{\epsilon}_1), \quad X_2 = \bar{X}_2 \cdot \text{Exp}(\boldsymbol{\epsilon}_2) \]

其中 \(\boldsymbol{\epsilon}_1, \boldsymbol{\epsilon}_2\) 独立。它们的复合 \(Y = X_1 \cdot X_2\) 的不确定性是什么？

推导：

Step 1：展开

\[ Y = \bar{X}_1 \text{Exp}(\boldsymbol{\epsilon}_1) \cdot \bar{X}_2 \text{Exp}(\boldsymbol{\epsilon}_2) \]

Step 2：利用伴随性质将 \(\text{Exp}(\boldsymbol{\epsilon}_1)\) "搬过" \(\bar{X}_2\)。关键步骤：

\[ \text{Exp}(\boldsymbol{\epsilon}_1) \cdot \bar{X}_2 = \bar{X}_2 \cdot \text{Exp}(\text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1) \]

这是因为 \(\text{Exp}(\boldsymbol{\epsilon}_1) = \bar{X}_2 \cdot \bar{X}_2^{-1} \cdot \text{Exp}(\boldsymbol{\epsilon}_1) \cdot \bar{X}_2 \cdot \bar{X}_2^{-1} = \bar{X}_2 \cdot \text{Exp}(\text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1) \cdot \bar{X}_2^{-1}\)，但这样右边还多了一个 \(\bar{X}_2^{-1}\)——实际上正确的推导是利用 \(A \cdot \text{Exp}(\boldsymbol{\xi}) = \text{Exp}(\text{Ad}_A \boldsymbol{\xi}) \cdot A\)，取 \(A = \bar{X}_2^{-1}\) 并从左乘 \(\bar{X}_2\)，得到上述结果。

Step 3：代入得

\[ Y = \bar{X}_1 \bar{X}_2 \cdot \text{Exp}(\text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1) \cdot \text{Exp}(\boldsymbol{\epsilon}_2) \]

Step 4：利用 BCH 一阶近似合并两个小扰动（因为 \(\boldsymbol{\epsilon}_1, \boldsymbol{\epsilon}_2\) 都是小量）：

\[ \text{Exp}(\text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1) \cdot \text{Exp}(\boldsymbol{\epsilon}_2) \approx \text{Exp}(\text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1 + \boldsymbol{\epsilon}_2) \]

Step 5：因此

\[ Y \approx \bar{Y} \cdot \text{Exp}(\boldsymbol{\epsilon}_Y) \]

其中 \(\bar{Y} = \bar{X}_1 \bar{X}_2\)，\(\boldsymbol{\epsilon}_Y = \text{Ad}_{\bar{X}_2^{-1}} \boldsymbol{\epsilon}_1 + \boldsymbol{\epsilon}_2\)。

Step 6：由于 \(\boldsymbol{\epsilon}_1\) 和 \(\boldsymbol{\epsilon}_2\) 独立，线性组合的协方差为：

\[ \boxed{\Sigma_Y = \text{Ad}_{\bar{X}_2^{-1}} \Sigma_1 \text{Ad}_{\bar{X}_2^{-1}}^T + \Sigma_2} \]

这个公式在 SLAM 中无处不在——每次你把两个帧的位姿"串联"起来时（例如 \(T_{0 \to 2} = T_{0 \to 1} \cdot T_{1 \to 2}\)），都需要用这个公式传播不确定性。它是位姿图优化、IMU 预积分、多传感器融合的数学基础。

与欧几里得情况的对比：在普通向量空间中，\(\mathbf{y} = \mathbf{x}_1 + \mathbf{x}_2\) 的协方差直接是 \(\Sigma_y = \Sigma_1 + \Sigma_2\)。李群上的公式多了 \(\text{Ad}_{\bar{X}_2^{-1}}\) 这个"旋转"——它的作用是把 \(X_1\) 的不确定性从 \(X_1\) 的局部坐标系"搬运"到 \(X_2\) 的局部坐标系，然后才能和 \(\Sigma_2\) 相加。

⚠️ 常见陷阱

概念误区：混淆 \(\mathbf{J}_r\) 和 \(\mathbf{J}_l\)

新手想法："反正都是雅可比，用哪个都一样"

实际上：\(\mathbf{J}_r\) 对应右扰动模型 \(R \cdot \text{Exp}(\delta\boldsymbol{\phi})\)，\(\mathbf{J}_l\) 对应左扰动模型 \(\text{Exp}(\delta\boldsymbol{\phi}) \cdot R\)。用错了会导致雅可比差一个旋转矩阵 \(R\)（因为 \(\mathbf{J}_l = R \mathbf{J}_r\)），优化方向完全错误。

后果：优化器不收敛，或收敛到错误的解。由于差的是一个旋转，误差可能在数值上"看起来差不多"（尤其是小角度时 \(R \approx \mathbf{I}\)），非常难调试。

正确做法：在推导的一开始就明确声明"本文使用右扰动模型"，然后全程保持一致。ORB-SLAM 系列使用右扰动，GTSAM 使用右扰动，VINS-Mono 使用右扰动——当前 SLAM 社区的主流是右扰动。

自检方法：小角度时 \(\mathbf{J}_r \approx \mathbf{J}_l \approx \mathbf{I}\)，两者近乎相同。如果你的算法在小角度时正确但大角度时发散，很可能就是 \(\mathbf{J}_r\)/\(\mathbf{J}_l\) 用错了。

编程陷阱：小角度时的数值奇异

错误做法：直接实现 \(\mathbf{J}_r\) 的闭合公式而不处理 \(\|\boldsymbol{\phi}\| \approx 0\) 的情况

现象：当旋转接近零时（如静止或匀速直线运动），出现 NaN 或 Inf，导致优化器崩溃

根本原因：闭合公式中有 \(1/\|\boldsymbol{\phi}\|^2\) 和 \(1/\|\boldsymbol{\phi}\|^3\) 项，当 \(\|\boldsymbol{\phi}\| \to 0\) 时分母趋零。虽然整个表达式有极限值（\(\mathbf{J}_r \to \mathbf{I}\)），但浮点运算会先算分母导致溢出。

正确做法：设置阈值 \(\epsilon\)（通常 \(10^{-6}\) 到 \(10^{-10}\)），当 \(\|\boldsymbol{\phi}\| < \epsilon\) 时用 Taylor 展开代替闭合公式：

if (theta < 1e-8) {
    // Taylor expansion: J_r ≈ I - 0.5*[phi]x + (1/6)*[phi]x^2
    Jr = Eigen::Matrix3d::Identity() 
         - 0.5 * phi_hat 
         + (1.0/6.0) * phi_hat * phi_hat;
} else {
    double c1 = (1.0 - cos(theta)) / (theta * theta);
    double c2 = (theta - sin(theta)) / (theta * theta * theta);
    Jr = Eigen::Matrix3d::Identity() 
         - c1 * phi_hat 
         + c2 * phi_hat * phi_hat;
}

思维陷阱：忘记 Adjoint 在切换扰动侧时的作用

新手想法："左扰动和右扰动只是约定不同，推导完一边，另一边把符号换一下就行"

实际上：从右扰动切换到左扰动（或反过来）时，协方差矩阵必须经过 Adjoint 变换：\(\Sigma_{\text{left}} = \text{Ad}_T \Sigma_{\text{right}} \text{Ad}_T^T\)。如果你忽略了这个变换，直接用同一个 \(\Sigma\)，得到的不确定性估计会是错的。

后果：在多传感器融合中，不同传感器的协方差可能在不同参考系下表示。如果不用 Adjoint 统一到同一参考系就直接融合，协方差矩阵的物理意义不一致，融合结果会出错。

正确做法：始终明确每个协方差矩阵的"定义侧"——它是右扰动意义下的还是左扰动意义下的？在融合前用 Adjoint 统一。GTSAM 库内部统一使用右扰动模型，这也是推荐的做法。

练习

练习 1（手推，⭐⭐⭐）：令 \(\boldsymbol{\phi} = (\pi/2, 0, 0)^T\)（绕 \(x\) 轴旋转 90 度），手工计算 \(\mathbf{J}_r(\boldsymbol{\phi})\)。

提示：先算 \([\boldsymbol{\phi}]_{\times}\) 和 \([\boldsymbol{\phi}]_{\times}^2\)，代入 \(\theta = \pi/2\)，\(\cos\theta = 0\)，\(\sin\theta = 1\) 化简。最后验证 \(\mathbf{J}_r \cdot \mathbf{J}_r^{-1} = \mathbf{I}\)。

练习 2（编程，⭐⭐⭐）：用 C++ (Eigen) 实现右雅可比 rightJacobian(phi) 及其逆 rightJacobianInverse(phi) 的函数，包含小角度的 Taylor 展开处理。然后编写测试：对 100 个随机旋转向量（\(\|\boldsymbol{\phi}\| \in [0, \pi]\)），验证以下两个性质（误差 \(< 10^{-10}\)）：

1. \(\mathbf{J}_r(\boldsymbol{\phi}) \cdot \mathbf{J}_r^{-1}(\boldsymbol{\phi}) = \mathbf{I}\)
2. \(\mathbf{J}_l(\boldsymbol{\phi}) = \mathbf{J}_r(-\boldsymbol{\phi})\)

练习 3（推导，⭐⭐⭐⭐）：利用本节推导的复合运算不确定性传播公式，推导"三帧串联"的情况：\(Y = X_1 \cdot X_2 \cdot X_3\)，写出 \(\Sigma_Y\) 关于 \(\Sigma_1, \Sigma_2, \Sigma_3\) 和各均值 \(\bar{X}_i\) 的表达式。

提示：先把 \(X_1 \cdot X_2\) 看作一个整体，用两帧公式求出 \(\Sigma_{12}\)，然后再与 \(X_3\) 复合。验证当所有 \(\text{Ad}\) 为单位矩阵时（即退化为欧几里得情况），公式退化为简单的协方差加法 \(\Sigma_Y = \Sigma_1 + \Sigma_2 + \Sigma_3\)。

SLAM 理论深入：PnP 与点云配准

---

3.1 PnP 问题深入 ⭐⭐

动机

PnP（Perspective-n-Point）是视觉 SLAM 中 Tracking 阶段的核心——每一帧都在解 PnP。

想象你正在运行 ORB-SLAM3：摄像头拍到一帧新图像，系统从中提取了 ORB 特征点，并与地图中已知的 3D 地图点建立了匹配关系。现在的问题是：已知一组 3D 点在世界坐标系中的位置 \(P_i\)，以及它们在当前图像上的投影 \(p_i\)，如何求出当前相机的位姿 \(T = [R|t]\)？ 这就是 PnP 问题。

ORB-SLAM3 的 Tracking 线程每收到一帧图像，都要执行以下流程：

1. 提取 ORB 特征 → 与地图点匹配 → 解 PnP 求位姿 → 局部优化

PnP 的效率和精度直接决定了 SLAM 系统的实时性和定位精度。如果 PnP 解得慢，系统就无法实时运行；如果 PnP 解得不准，后续的建图和回环检测都会受到累积误差的影响。

问题定义

输入：\(n\) 组 3D-2D 对应关系 \(\{(P_i, p_i)\}_{i=1}^{n}\)，其中： - \(P_i = [X_i, Y_i, Z_i]^T \in \mathbb{R}^3\)：3D 点在世界坐标系中的坐标（已知） - \(p_i = [u_i, v_i]^T \in \mathbb{R}^2\)：对应的 2D 图像像素坐标（观测值） - 相机内参矩阵 \(K\)（对于校准过的相机，视为已知）

输出：相机位姿 \(T = [R|t]\)，其中 \(R \in SO(3)\)（旋转矩阵），\(t \in \mathbb{R}^3\)（平移向量）

投影模型：

\[ s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = K [R|t] \begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix} \]

其中 \(s\) 是深度因子（投影时的齐次化系数）。

自由度分析

相机位姿有 6 个自由度（6 DOF），不是 12 个未知数。 这是一个极其重要且常被搞混的概念。

旋转 \(R\) 是 \(3 \times 3\) 矩阵，看起来有 9 个元素，但 \(R \in SO(3)\) 的约束 \(R^T R = I, \det(R) = 1\) 使得它只有 3 个自由度（可用轴角、欧拉角或四元数的 3 个独立分量来参数化）。加上平移 \(t\) 的 3 个分量，总共 6 DOF。

\[ \underbrace{R}_{3 \text{ DOF (旋转)}} + \underbrace{t}_{3 \text{ DOF (平移)}} = 6 \text{ DOF} \]

为什么有人说"12 个未知数"？ 这是将 DLT 方法中 \(3 \times 4\) 投影矩阵 \(P = K[R|t]\) 的 12 个元素误认为是 PnP 的未知数。\(P\) 矩阵确实有 12 个元素（齐次化后 11 DOF），但其中包含了内参 \(K\) 的 5 个自由度。对于已标定的相机（\(K\) 已知），PnP 只需求解 6 DOF 的位姿。

P3P（Perspective-3-Point）⭐⭐

核心思想：3 组对应点是求解 PnP 的最小情况（minimal case），产生最多 4 组解。

几何构造

P3P 的关键在于将问题转化为求解三角形的边长。设 3 个 3D 点为 \(A, B, C\)，相机光心为 \(O\)，我们需要求出 \(|OA| = s_1, |OB| = s_2, |OC| = s_3\)。

Step 1: 从 2D 观测计算夹角

已知内参 \(K\) 后，可以将像素坐标 \(p_i\) 反投影为归一化相机坐标系下的方向向量 \(\hat{v}_i = K^{-1} \tilde{p}_i / \|K^{-1} \tilde{p}_i\|\)（\(\tilde{p}_i\) 是齐次坐标）。由此计算光线之间的夹角：

\[ \cos \alpha = \hat{v}_A \cdot \hat{v}_B, \quad \cos \beta = \hat{v}_B \cdot \hat{v}_C, \quad \cos \gamma = \hat{v}_A \cdot \hat{v}_C \]

Step 2: 用余弦定理建立方程

三个 3D 点之间的距离 \(d_{AB} = |AB|, d_{BC} = |BC|, d_{AC} = |AC|\) 是已知的（它们是地图点）。在三角形 \(OAB\)、\(OBC\)、\(OAC\) 中分别应用余弦定理：

\[ d_{AB}^2 = s_1^2 + s_2^2 - 2 s_1 s_2 \cos \alpha $$ $$ d_{BC}^2 = s_2^2 + s_3^2 - 2 s_2 s_3 \cos \beta $$ $$ d_{AC}^2 = s_1^2 + s_3^2 - 2 s_1 s_3 \cos \gamma \]

这是关于 \((s_1, s_2, s_3)\) 的三个二次方程。

Step 3: 消元求解

令 \(u = s_1 / s_3, v = s_2 / s_3\)，代入上述方程可以消去 \(s_3\)，最终化简为关于 \(u\)（或 \(v\)）的一元四次多项式。四次方程最多有 4 个正实根，对应 最多 4 组解。

近年来 Ding et al. (CVPR 2023) 提出将 P3P 化简为三次方程而非四次方程，但经典方法仍然基于四次多项式。

Step 4: 用第 4 个点消歧义

4 组解中只有 1 组是正确的。在 RANSAC 框架中，通常使用第 4 个点对每组解进行验证（重投影误差最小的那组即为正确解）。这就是为什么 P3P 虽然理论上只需 3 个点，但实际使用中需要第 4 个点来消除歧义。

P3P 在 ORB-SLAM3 中的角色

ORB-SLAM3 在 RANSAC 循环的每次迭代中使用 P3P（或 MLPnP）：

1. 随机采样 3 组匹配（最小集）
2. 用 P3P 求解（得到最多 4 个候选位姿）
3. 用剩余匹配验证每个候选位姿，统计内点数
4. 重复多轮，选择内点最多的位姿作为初值

P3P 的优势在于**每次 RANSAC 迭代只需 3 个点，所需迭代次数远少于 6 点法（DLT）**。假设内点比例为 \(w\)，要以概率 \(p\) 至少找到一次全内点样本，RANSAC 需要的迭代次数为：

\[ k = \frac{\log(1-p)}{\log(1-w^n)} \]

当 \(w = 0.5, p = 0.99\) 时：\(n=3 \Rightarrow k = 35\)；\(n=6 \Rightarrow k = 293\)。P3P 的效率优势非常明显。

EPnP（Efficient PnP, Lepetit et al. 2009）⭐⭐

核心思想：将所有 \(n\) 个 3D 点表示为 4 个控制点的加权和（重心坐标），从而将 \(n\) 个点的问题降维为 12 个未知数（4 个控制点 \(\times\) 3 个坐标）。

算法推导

Step 1: 定义控制点与重心坐标

选择 4 个控制点 \(c_1, c_2, c_3, c_4\)（通常取 3D 点集的质心加上主成分方向上的点）。每个 3D 参考点 \(P_i\) 可以用重心坐标 \((\alpha_{i1}, \alpha_{i2}, \alpha_{i3}, \alpha_{i4})\) 表示：

\[ P_i = \sum_{j=1}^{4} \alpha_{ij} c_j^w, \quad \text{其中} \quad \sum_{j=1}^{4} \alpha_{ij} = 1 \]

上标 \(w\) 表示世界坐标系。重心坐标 \(\alpha_{ij}\) 在世界坐标系中可以直接计算（因为 \(P_i\) 和 \(c_j^w\) 都已知）。

Step 2: 相机坐标系中的表示

由于刚体变换保持重心坐标不变（这是一个关键性质！仿射变换保持重心坐标），同样的 \(\alpha_{ij}\) 在相机坐标系中也成立：

\[ P_i^c = \sum_{j=1}^{4} \alpha_{ij} c_j^c \]

现在，未知量变成了 4 个控制点在相机坐标系中的坐标 \(c_j^c = [x_j^c, y_j^c, z_j^c]^T\)，共 12 个标量。

Step 3: 建立线性约束

利用投影方程（已知内参 \(K = \begin{bmatrix} f_u & 0 & c_u \\ 0 & f_v & c_v \\ 0 & 0 & 1 \end{bmatrix}\)）：

\[ s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = K P_i^c = K \sum_{j=1}^{4} \alpha_{ij} c_j^c \]

展开消去深度 \(s_i\)（通过第三行得到 \(s_i = \sum_j \alpha_{ij} z_j^c\)），得到每个点对应的 2 个线性方程：

\[ \sum_{j=1}^{4} \alpha_{ij} f_u x_j^c + \alpha_{ij}(c_u - u_i) z_j^c = 0 $$ $$ \sum_{j=1}^{4} \alpha_{ij} f_v y_j^c + \alpha_{ij}(c_v - v_i) z_j^c = 0 \]

\(n\) 个点给出 \(2n\) 个方程，关于 12 个未知数 \(\mathbf{x} = [c_1^{c^T}, c_2^{c^T}, c_3^{c^T}, c_4^{c^T}]^T\)。

Step 4: 求解核空间

将线性方程组写成 \(M \mathbf{x} = 0\)（\(M\) 是 \(2n \times 12\) 矩阵）。对 \(M^T M\)（\(12 \times 12\) 矩阵）做特征值分解（或对 \(M\) 做 SVD），解在 \(M^T M\) 最小特征值对应的特征向量所张成的核空间中：

\[ \mathbf{x} = \sum_{k=1}^{N} \beta_k \mathbf{v}_k \]

其中 \(\mathbf{v}_k\) 是 \(M^T M\) 的零特征值（或最小特征值）对应的特征向量，\(N\) 的取值取决于退化程度（通常 \(N = 1, 2, 3, 4\)）。

Step 5: 利用距离约束确定 \(\beta_k\)

控制点之间的距离在刚体变换下不变：\(\|c_i^c - c_j^c\|^2 = \|c_i^w - c_j^w\|^2\)。这给出了关于 \(\beta_k\) 的二次方程组。Lepetit et al. 对 \(N=1,2,3,4\) 的情况分别求解，选择重投影误差最小的解。

复杂度分析：构建 \(M\) 需要 \(O(n)\) 时间，SVD 作用于固定大小的 \(12 \times 12\) 矩阵（\(O(1)\)），因此总复杂度为 \(O(n)\)——这远优于 DLT 对 \(2n \times 12\) 大矩阵做 SVD 的开销，因为 EPnP 的大矩阵运算被限制在 \(12 \times 12\) 的固定尺度上。

EPnP 在 ORB-SLAM3 中的角色

ORB-SLAM3 在 RANSAC 找到内点集合后，使用 EPnP 对所有内点重新估计位姿，作为后续 LM 优化的初始值。EPnP 的 \(O(n)\) 复杂度使得它在处理大量内点时依然高效。

DLT（Direct Linear Transform）⭐⭐

核心思想：直接求解完整的 \(3 \times 4\) 投影矩阵 \(P\)，无需预先知道相机内参。

与 PnP 的本质区别

特征	PnP（P3P/EPnP）	DLT
输入	3D-2D 对应 + 已知内参 \(K\)	3D-2D 对应（\(K\) 未知）
求解目标	位姿 \([R\|t]\)（6 DOF）	投影矩阵 \(P = K[R\|t]\)（11 DOF）
最少点数	3（P3P）或 4（EPnP）	6
典型应用	SLAM 追踪（相机已标定）	SfM、相机标定

推导过程

Step 1: 投影方程的齐次化

\[ s \tilde{p}_i = P \tilde{P}_i \quad \Rightarrow \quad s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{p}_1^T \\ \mathbf{p}_2^T \\ \mathbf{p}_3^T \end{bmatrix} \tilde{P}_i \]

其中 \(\mathbf{p}_k^T\) 是 \(P\) 的第 \(k\) 行（\(1 \times 4\)），\(\tilde{P}_i = [X_i, Y_i, Z_i, 1]^T\)。

Step 2: 消去 \(s\) 并线性化

由 \(s = \mathbf{p}_3^T \tilde{P}_i\)，代入前两行：

\[ u_i (\mathbf{p}_3^T \tilde{P}_i) - \mathbf{p}_1^T \tilde{P}_i = 0 $$ $$ v_i (\mathbf{p}_3^T \tilde{P}_i) - \mathbf{p}_2^T \tilde{P}_i = 0 \]

每个点给出 2 个方程，6 个点给出 12 个方程。将 \(P\) 的 12 个元素拉成向量 \(\mathbf{p} \in \mathbb{R}^{12}\)，得到齐次线性方程组 \(A \mathbf{p} = 0\)（\(A\) 为 \(2n \times 12\)）。

Step 3: SVD 求解

对 \(A\) 做 SVD，\(\mathbf{p}\) 是最小奇异值对应的右奇异向量。重新排列为 \(3 \times 4\) 矩阵即得 \(P\)。

Step 4: 从 \(P\) 提取 \(K, R, t\)

\(P = K [R|t]\)，其中 \(P\) 的前 \(3 \times 3\) 子矩阵 \(M = KR\)。对 \(M\) 做 RQ 分解（\(M = RQ\)，\(R\) 上三角即 \(K\)，\(Q\) 正交即旋转），即可分离内参和外参。平移 \(t = K^{-1} P_{:,4}\)（第 4 列）。

注意：DLT 不要求相机已标定，这是它的优势。但代价是需要更多点（\(\geq 6\)），且不利用 \(R \in SO(3)\) 的约束，精度不如 EPnP。

迭代优化（Levenberg-Marquardt 精化）⭐⭐

核心思想：P3P/EPnP 给出的是代数解（基于线性化或几何关系），而非统计最优解。迭代优化通过最小化重投影误差来精化位姿。

重投影误差

\[ E(\xi) = \sum_{i=1}^{n} \| p_i - \pi(T(\xi) \cdot P_i) \|^2 \]

其中 \(\xi \in \mathfrak{se}(3)\) 是位姿的李代数参数化（6 维），\(\pi(\cdot)\) 是投影函数，\(T(\xi) = \exp(\xi^\wedge) \in SE(3)\)。

LM 算法迭代

\[ (J^T J + \lambda \text{diag}(J^T J)) \Delta \xi = -J^T \mathbf{r} \]

其中 \(J\) 是重投影误差关于 \(\xi\) 的雅可比矩阵（\(2n \times 6\)），\(\mathbf{r}\) 是残差向量。\(\lambda\) 是阻尼因子——\(\lambda\) 大时近似梯度下降（保守步长），\(\lambda\) 小时近似高斯-牛顿（快速收敛）。

这就是 OpenCV cv::solvePnP(method=SOLVEPNP_ITERATIVE) 的实现原理。

RANSAC + PnP 实际流程 ⭐⭐

一个完整的 PnP 求解流程如下：

输入: 3D-2D 匹配集合 {(P_i, p_i)}，其中包含外点（错误匹配）

RANSAC 循环 (k 次迭代):
  1. 随机采样 3 组匹配
  2. P3P 求解 → 最多 4 个候选位姿
  3. 对每个候选位姿，计算所有匹配的重投影误差
  4. 重投影误差 < 阈值的匹配标记为内点
  5. 记录内点最多的候选位姿

RANSAC 后处理:
  6. 用最佳内点集合，EPnP 重新估计位姿
  7. Levenberg-Marquardt 精化（最小化内点的重投影误差）
  8. 可选: 重新分类内点/外点，再次 LM 精化

输出: 精化后的位姿 T = [R|t]，以及内点/外点标记

方法对比总结

方法	最少点数	需要标定？	复杂度	适用场景
P3P	3	是	\(O(1)\) / RANSAC 迭代	ORB-SLAM3 RANSAC 内部
EPnP	\(\geq 4\)	是	\(O(n)\)	ORB-SLAM3 内点精估
DLT	\(\geq 6\)	否	\(O(n)\)（对大矩阵 SVD）	SfM、相机标定
LM	全部内点	是	迭代	任何初值的精化

⚠️ 常见陷阱

💡 概念误区：混淆 DLT 和 PnP

新手想法："DLT 也是用 3D-2D 对应求位姿，和 PnP 不是一样的吗？"

实际上：DLT 求解的是完整的 \(3 \times 4\) 投影矩阵 \(P = K[R|t]\)（11 DOF，包含内参），适用于**未标定**相机；而 P3P/EPnP 求解的是纯位姿 \([R|t]\)（6 DOF），要求相机**已标定**（\(K\) 已知）。在 SLAM 中，相机通常已经离线标定好了，所以直接用 EPnP 更高效更精确。

正确理解：DLT 是更一般的方法（不需要 \(K\)），PnP 是更特化的方法（利用 \(K\) 减少未知数）。特化 = 利用更多先验信息 = 更少的点数 + 更高的精度。

💡 概念误区：认为 ICP 是 PnP 的一种方法

新手想法："PnP 求位姿，ICP 也求位姿，它们应该是同类方法的不同变体。"

实际上：PnP 处理的是 3D-2D 对应（已知 3D 点，观测 2D 投影），而 ICP 处理的是 3D-3D 对应（两组 3D 点云之间的配准）。它们是**完全不同的问题**： - PnP：涉及透视投影（非线性变换），需要相机模型 - ICP：纯刚体变换（线性问题），不涉及投影

混淆这两者通常源于"都是在求 \(R\) 和 \(t\)"这一表面相似性，但问题的数学结构截然不同。

🧠 思维陷阱：P3P 只给一个解

新手想法："3 个方程 3 个未知数，解应该是唯一的。"

实际上：P3P 的方程是**二次的**（余弦定理），消元后得到四次多项式，最多有 4 个正实根。几何上，这对应于 3D 点在相机前方的 4 种不同排列方式。必须用第 4 个点（或更多点）来消除歧义。这就是为什么 RANSAC 中虽然最小采样是 3 个点，但每个候选解都需要在更多点上验证。

⚠️ 编程陷阱：退化配置——所有点共面时 PnP 失效

现象：当所有 3D 点在同一平面上时（如墙面上的特征点），EPnP 的 \(M^T M\) 矩阵会出现额外的零特征值，导致解不稳定或完全错误。

根本原因：共面点的 3D 结构退化，使得控制点无法张成完整的 3D 空间。此时 4 个控制点实际上只有 3 个自由度。

正确做法：检测到共面情况时，改用**单应矩阵（Homography）**来求解。ORB-SLAM3 的初始化阶段同时计算基础矩阵和单应矩阵，根据各自的评分选择合适的模型。

自检方法：计算 3D 点集的协方差矩阵，检查最小特征值是否接近零。如果 \(\lambda_{\min} / \lambda_{\max} < 0.01\)，说明点集近似共面。

练习

练习 1（手推）：给定 3 个 3D 点 \(A = (0,0,0), B = (1,0,0), C = (0,1,0)\)，以及相机光心 \(O\) 到三条光线的夹角 \(\cos \alpha = 0.8, \cos \beta = 0.7, \cos \gamma = 0.9\)，写出 P3P 的三个余弦定理方程，并令 \(u = s_1/s_3, v = s_2/s_3\) 进行消元，推导关于 \(u\) 的多项式。在草稿纸上完成。

练习 2（编程）：使用 OpenCV 的 cv::solvePnP 分别用 SOLVEPNP_P3P、SOLVEPNP_EPNP、SOLVEPNP_DLT 和 SOLVEPNP_ITERATIVE 四种方法求解同一组 3D-2D 对应的位姿。对比四种方法的：(a) 重投影误差，(b) 运行时间，(c) 对外点的鲁棒性。改变外点比例（10%、30%、50%），观察哪种方法退化最快。

练习 3（思考）：ORB-SLAM3 使用 MLPnP（Maximum Likelihood PnP）代替传统 EPnP，使得 PnP 求解与相机模型解耦（支持鱼眼镜头等）。MLPnP 的输入是投影射线（projective rays）而非像素坐标。思考：(a) 为什么传统 EPnP 与相机模型耦合？（提示：看推导中哪里用到了内参 \(K\)）(b) 如果将像素坐标通过逆投影函数转换为射线方向，为什么就解耦了？

---

3.2 点云配准理论 ⭐⭐

动机

点云配准是 LiDAR SLAM 的前端核心——如何对齐两帧点云来估计传感器的运动。

每当 LiDAR 扫描一帧新的点云，我们需要回答一个问题：相对于上一帧（或地图），传感器移动了多少？ 这就是点云配准（Point Cloud Registration）问题。它在 LiDAR SLAM 中的角色，相当于 PnP 在视觉 SLAM 中的角色——都是前端里程计（odometry）的核心求解步骤。

与 PnP（3D-2D 问题）不同，点云配准是一个 3D-3D 问题：两组点云都是三维的，不涉及透视投影。这使得问题的数学结构完全不同——在已知对应关系的情况下，最优刚体变换有**闭式解**（SVD 方法）。

问题定义

输入： - 源点云（Source）\(\mathcal{P} = \{p_i\}_{i=1}^{N_p}\)，\(p_i \in \mathbb{R}^3\) - 目标点云（Target）\(\mathcal{Q} = \{q_j\}_{j=1}^{N_q}\)，\(q_j \in \mathbb{R}^3\)

输出：刚体变换 \(T = (R, t)\)，使得 \(T \cdot \mathcal{P}\) 与 \(\mathcal{Q}\) 尽可能对齐

优化目标：

\[ T^* = \arg\min_{R, t} \sum_{i} \| q_{c(i)} - (R p_i + t) \|^2 \]

其中 \(c(i)\) 是源点 \(p_i\) 在目标点云中的对应点索引。

ICP（Iterative Closest Point）⭐⭐

ICP 由 Besl & McKay (1992) 和 Chen & Medioni (1992) 独立提出，是最经典的点云配准算法。

两步迭代框架

ICP 的核心是交替执行两步：

重复直到收敛:
  Step 1 (对应关系估计): 对每个 p_i，找 Q 中最近邻 q_{c(i)} = argmin_{q∈Q} ||Rp_i + t - q||
  Step 2 (变换估计): 固定对应关系，求解最优 R, t

这是一个 **EM 算法**的实例：Step 1 是 E 步（估计隐变量——对应关系），Step 2 是 M 步（最大化似然——求解变换）。

Point-to-Point ICP：SVD 闭式解 (Arun et al. 1987)

当对应关系已知时，point-to-point 目标函数为：

\[ E(R, t) = \sum_{i=1}^{n} \| q_i - (R p_i + t) \|^2 \]

推导如下（展示每一步）：

Step 1: 中心化消去平移

计算两组点的质心：

\[ \bar{p} = \frac{1}{n}\sum_{i=1}^{n} p_i, \quad \bar{q} = \frac{1}{n}\sum_{i=1}^{n} q_i \]

定义去中心化的点：\(\bar{p}_i = p_i - \bar{p}\)，\(\bar{q}_i = q_i - \bar{q}\)。

将目标函数展开（这里是关键的技巧——加减质心）：

\[ E = \sum_i \| (q_i - \bar{q}) - R(p_i - \bar{p}) - (t + R\bar{p} - \bar{q}) \|^2 $$ $$ = \sum_i \| \bar{q}_i - R\bar{p}_i \|^2 + n \| t - (\bar{q} - R\bar{p}) \|^2 \]

交叉项为零（因为 \(\sum_i \bar{p}_i = 0\)，\(\sum_i \bar{q}_i = 0\)）。

因此，**\(R\) 和 \(t\) 解耦**了！先求 \(R\) 最小化第一项，再由 \(t = \bar{q} - R\bar{p}\) 求 \(t\)。

Step 2: 求旋转矩阵 \(R\)

需要最小化：

\[ \sum_i \| \bar{q}_i - R\bar{p}_i \|^2 = \sum_i (\bar{q}_i^T \bar{q}_i + \bar{p}_i^T \bar{p}_i - 2\bar{q}_i^T R \bar{p}_i) \]

前两项是常数（与 \(R\) 无关），因此等价于**最大化**：

\[ \sum_i \bar{q}_i^T R \bar{p}_i = \text{tr}\left( R \sum_i \bar{p}_i \bar{q}_i^T \right) = \text{tr}(R H^T) = \text{tr}(H^T R) \]

（利用了矩阵迹的循环性质 \(\text{tr}(AB) = \text{tr}(BA)\)）

其中交叉协方差矩阵为：

\[ H = \sum_{i=1}^{n} \bar{q}_i \bar{p}_i^T \quad (3 \times 3) \]

Step 3: SVD 求解

对 \(H\) 做 SVD：\(H = U \Sigma V^T\)。则：

\[ \text{tr}(H^T R) = \text{tr}(V \Sigma U^T R) \]

令 \(M = U^T R V\)（\(M\) 也是正交矩阵），则 \(\text{tr}(H^T R) = \text{tr}(V \Sigma M) = \text{tr}(\Sigma M)\)（再次用迹的循环性）。

\(\text{tr}(\Sigma M) = \sum_k \sigma_k m_{kk}\)，其中 \(\sigma_k \geq 0\) 是奇异值，\(|m_{kk}| \leq 1\)（\(M\) 正交矩阵的对角元素）。当 \(M = I\) 时取最大值，即：

\[ R = U V^T \]

Step 4: 处理反射情况

如果 \(\det(UV^T) = -1\)，说明得到了反射而非旋转。此时需要修正：

\[ R = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} V^T \]

（翻转 \(U\) 的最后一列对应的符号。）

Step 5: 求平移

\[ t = \bar{q} - R \bar{p} \]

Point-to-Plane ICP

Point-to-plane ICP（Chen & Medioni 1992）将目标函数改为残差沿法向量的投影：

\[ E(R, t) = \sum_{i=1}^{n} \left( n_i^T (R p_i + t - q_i) \right)^2 \]

其中 \(n_i\) 是目标点 \(q_i\) 处的表面法向量。

为什么比 point-to-point 好？ 直觉上，对于平坦表面，沿表面方向的滑动不应产生惩罚（因为表面上任何点都同样"正确"）。Point-to-plane 只惩罚法线方向的偏差，允许切线方向自由滑动，因此**收敛速度约快 10 倍**。

代价是 point-to-plane 没有闭式解，需要用 LM 或其他迭代方法求解。实践中通常将 \(R\) 用小角度近似（\(R \approx I + [\omega]_\times\)）线性化后求解。

GICP（Generalized ICP, Segal et al. 2009）⭐⭐⭐

核心思想：将 point-to-point 和 point-to-plane 统一为同一个概率框架的特殊情况。

概率模型

GICP 假设每个点 \(p_i\)、\(q_i\) 都不是精确的，而是服从以其为中心的高斯分布：

\[ p_i \sim \mathcal{N}(\hat{p}_i, C_i^P), \quad q_i \sim \mathcal{N}(\hat{q}_i, C_i^Q) \]

其中 \(C_i^P, C_i^Q\) 是各点的**局部协方差矩阵**，描述了该点邻域的表面几何结构。

优化目标

对于匹配对 \((p_i, q_i)\)，残差 \(d_i = q_i - T \cdot p_i\) 的分布为：

\[ d_i \sim \mathcal{N}(0, C_i^Q + R C_i^P R^T) \]

最大化似然等价于最小化**Mahalanobis 距离**：

\[ E(T) = \sum_i d_i^T \left( C_i^Q + R C_i^P R^T \right)^{-1} d_i \]

特殊情况

当 \(C_i^P = C_i^Q = \sigma^2 I\)（各向同性噪声）时：

\[ E(T) = \sum_i d_i^T (2\sigma^2 I)^{-1} d_i = \frac{1}{2\sigma^2} \sum_i \|d_i\|^2 \]

这就退化为标准的 point-to-point ICP。

当 \(C_i^Q\) 在法线方向协方差为零、切线方向协方差为无穷大时：

设表面法向量为 \(n_i\)，令 \(C_i^Q = \epsilon^{-1}(I - n_i n_i^T)\)（\(\epsilon \to 0\)），则代价函数中只有法向量分量有贡献：

\[ E(T) \to \sum_i (n_i^T d_i)^2 \]

这就退化为 point-to-plane ICP。

GICP 的实际做法（plane-to-plane）：对每个点，根据其 \(k\) 近邻拟合局部平面，协方差矩阵的特征值分解反映了表面结构——最小特征值方向为法线方向（方差小），其余两个方向为切线方向（方差大）。

NDT（Normal Distributions Transform）⭐⭐⭐

核心思想：将目标点云离散化为体素网格，每个体素拟合一个高斯分布 \(\mathcal{N}(\mu_k, \Sigma_k)\)，然后最大化源点云在此概率场中的似然。

算法步骤

Step 1: 体素化目标点云

将目标点云所在空间划分为固定大小的体素。对每个体素 \(V_k\) 中包含的点 \(\{q_j\}_{j \in V_k}\)，计算：

\[ \mu_k = \frac{1}{|V_k|} \sum_{j \in V_k} q_j, \quad \Sigma_k = \frac{1}{|V_k| - 1} \sum_{j \in V_k} (q_j - \mu_k)(q_j - \mu_k)^T \]

Step 2: 构建似然函数

对变换后的源点 \(T \cdot p_i\)，找到其所在的体素 \(V_{k(i)}\)，该点的似然为：

\[ \mathcal{L}_i(T) = \exp\left( -\frac{1}{2} (T \cdot p_i - \mu_{k(i)})^T \Sigma_{k(i)}^{-1} (T \cdot p_i - \mu_{k(i)}) \right) \]

Step 3: 最大化总对数似然

\[ T^* = \arg\max_T \sum_i \log \mathcal{L}_i(T) = \arg\min_T \sum_i (T \cdot p_i - \mu_{k(i)})^T \Sigma_{k(i)}^{-1} (T \cdot p_i - \mu_{k(i)}) \]

这是一个非线性最小二乘问题，用牛顿法或 LM 方法求解。

体素分辨率的权衡

体素大小	优势	劣势
小（如 0.5m）	保留细节，精度高	计算慢，对噪声敏感，需要密集点云
大（如 5m）	计算快，对稀疏点云鲁棒	丢失细节，精度低，易陷入局部最优
典型选择 1-2m	精度与速度的平衡	需要根据场景调参

NDT 相对于 ICP 的优势：NDT 不需要逐点建立对应关系（这是 ICP 中最耗时的步骤），而是直接在连续概率场中优化。当点云规模很大时，NDT 通常比 ICP 更快。Autoware（自动驾驶开源框架）就默认使用 NDT 进行 LiDAR 定位。

方法对比总结

特性	Point-to-Point ICP	Point-to-Plane ICP	GICP	NDT
精度	低	中	高	中-高
收敛速度	慢（~50 次迭代）	快（~5-10 次）	快	快
闭式解	有（SVD）	无	无	无
对应关系	最近邻	最近邻 + 法线	最近邻 + 协方差	体素 + 高斯
初值敏感度	高	中	中	中-低
参数	无	无	\(k\) 近邻数	体素大小
典型应用	教学	通用	高精度场景	自动驾驶

⚠️ 常见陷阱

🧠 思维陷阱：认为 ICP 一定能收敛到全局最优

新手想法："ICP 是迭代优化，只要迭代次数够多就能找到正确解。"

实际上：ICP 是一个非凸优化问题（因为对应关系在迭代中变化）。如果初始位姿估计不好，ICP 极易陷入**局部最小值**。例如，两帧点云的初始偏差超过 30 度旋转或体素尺度的平移时，ICP 几乎必然收敛到错误的解。

正确做法：在 LiDAR SLAM 中，通常用 IMU 预积分或恒速模型提供初始位姿估计，确保两帧之间的初始偏差很小。LOAM/LIO-SAM 等系统都依赖 IMU 来提供好的初值。

自检方法：跑完 ICP 后检查最终残差。如果残差突然变大（比如前 99 帧的残差都在 0.01m 以下，第 100 帧突然跳到 0.5m），几乎可以确定陷入了局部最优。

⚠️ 编程陷阱：NDT 体素分辨率设置不当

现象 1（太小）：voxel_size = 0.1m，在 64 线 LiDAR 上运行 NDT。每个体素只有 1-2 个点，协方差矩阵 \(\Sigma_k\) 退化（不可逆）。程序直接崩溃或给出 NaN。

现象 2（太大）：voxel_size = 10m，NDT 变成"在一个巨大的高斯球中优化"，所有细节被抹平，精度极差。

根本原因：NDT 需要每个体素有**足够多的点**（通常至少 5-10 个）来拟合有意义的高斯分布。体素太小则点数不足，体素太大则分布过于粗糙。

正确做法：体素大小应与点云密度匹配。经验法则：体素大小约为 5-10 倍平均点间距。对于 Velodyne VLP-16，1-2m 是常见选择；对于 Livox Mid-360 等半固态 LiDAR，0.5-1m 可能更合适。

💡 概念误区：GICP 的协方差矩阵在稀疏点云上不可靠

新手想法："GICP 比 ICP 好，我无脑换 GICP 就行。"

实际上：GICP 需要用 \(k\) 近邻估计每个点的局部协方差矩阵。在**稀疏点云**（如远处的 LiDAR 点）中，近邻点可能跨越不同表面，导致协方差估计完全错误。此时 GICP 的精度可能反而不如简单的 point-to-point ICP。

正确做法：对稀疏区域使用更大的 \(k\) 值或降低这些点的权重。实践中可以根据点到传感器的距离自适应调整 \(k\)。

练习

练习 1（手推 + 编程）：给定两组对应点（3D-3D），手推 SVD 闭式解的完整推导（从 \(E(R,t)\) 开始到最终的 \(R = UV^T, t = \bar{q} - R\bar{p}\)），然后用 Eigen 库实现 point-to-point ICP（包含最近邻搜索和 SVD 求解），测试在已知刚体变换 + 高斯噪声下的配准精度。

练习 2（对比实验）：使用 PCL 库分别运行 ICP、GICP 和 NDT 对同一对点云进行配准。改变初始位姿偏差（5 度、15 度、30 度、45 度），记录每种方法的：(a) 最终旋转误差、(b) 最终平移误差、(c) 收敛所需迭代次数、(d) 运行时间。画出"初始偏差 vs 成功率"曲线，验证 ICP 对初值敏感的结论。