滨海网站建设,无锡网站制作哪些,福州网站建设推广平台,深圳市大鹏建设局网站多传感器融合定位十四-基于图优化的定位方法1. 基于图优化的定位简介1.1 核心思路1.2 定位流程2. 边缘化原理及应用2.1 边缘化原理2.2 从滤波角度理解边缘化3. 基于kitti的实现原理3.1 基于地图定位的滑动窗口模型3.2 边缘化过程4. lio-mapping 介绍4.1 核心思想4.2 具体流程4.…
多传感器融合定位十四-基于图优化的定位方法1. 基于图优化的定位简介1.1 核心思路1.2 定位流程2. 边缘化原理及应用2.1 边缘化原理2.2 从滤波角度理解边缘化3. 基于kitti的实现原理3.1 基于地图定位的滑动窗口模型3.2 边缘化过程4. lio-mapping 介绍4.1 核心思想4.2 具体流程4.2.1 各类因子4.2.2 滑窗模型4.2.3 边缘化4.2.4 添加新帧Reference:
深蓝学院-多传感器融合多传感器融合定位理论基础
文章跳转
多传感器融合定位一-3D激光里程计其一ICP多传感器融合定位二-3D激光里程计其二NDT多传感器融合定位三-3D激光里程计其三点云畸变补偿多传感器融合定位四-3D激光里程计其四点云线面特征提取多传感器融合定位五-点云地图构建及定位多传感器融合定位六-惯性导航原理及误差分析多传感器融合定位七-惯性导航解算及误差分析其一多传感器融合定位八-惯性导航解算及误差分析其二多传感器融合定位九-基于滤波的融合方法Ⅰ其一多传感器融合定位十-基于滤波的融合方法Ⅰ其二多传感器融合定位十一-基于滤波的融合方法Ⅱ多传感器融合定位十二-基于图优化的建图方法其一多传感器融合定位十三-基于图优化的建图方法其二多传感器融合定位十四-基于图优化的定位方法多传感器融合定位十五-多传感器时空标定(综述)
1. 基于图优化的定位简介
1.1 核心思路
核心思路是把融合方法从滤波换成图优化其元素不再是简单的惯性解算而是预积分。 一个暴力的模型可以设计为 缺陷随着时间的进行图模型会越来越大导致无法达到实时性。
解决方法不断删除旧的帧只优化最新的几帧即维持一个滑动窗口。 模型如下 问题直接从模型中删除等于损失了信息。 解法通过模型把旧帧的约束传递下来即边缘化(后面讲具体细节)。
1.2 定位流程 整个流程不断往滑窗里添加新信息并边缘化旧信息。 需要注意的是
正常行驶时不必像建图那样提取稀疏的关键帧停车时需要按一定策略提取关键帧但删除的是次新帧因此不需要边缘化。
2. 边缘化原理及应用
2.1 边缘化原理
优化问题具有如下通用形式: HXbH Xb HXb并可拆解成如下形式: [HmmHmrHrmHrr][XmXr][bmbr]\left[\begin{array}{cc} H_{m m} H_{m r} \\ H_{r m} H_{r r} \end{array}\right]\left[\begin{array}{c} X_m \\ X_r \end{array}\right]\left[\begin{array}{l} b_m \\ b_r \end{array}\right] [HmmHrmHmrHrr][XmXr][bmbr]拆解的目的是通过一系列操作把 XmX_mXm 从状态量 里删除掉并把它的约束保留下来。 在滑窗模式里这个 XmX_mXm 即为要边缘化掉的量。 回顾舒尔补 给定矩阵 M[ABCD]M\left[\begin{array}{ll} \mathrm{A} \mathrm{B} \\ \mathrm{C} \mathrm{D} \end{array}\right] M[ACBD]它可以通过如下变换变成上三角矩阵即 [I0−CA−1I][ABCD][AB0ΔA]\left[\begin{array}{cc} \mathrm{I} 0 \\ -\mathrm{CA}^{-1} \mathrm{I} \end{array}\right]\left[\begin{array}{cc} \mathrm{A} \mathrm{B} \\ \mathrm{C} \mathrm{D} \end{array}\right]\left[\begin{array}{cc} \mathrm{A} \mathrm{B} \\ 0 \Delta \mathrm{A} \end{array}\right] [I−CA−10I][ACBD][A0BΔA]其中 ΔAD−CA−1B\Delta \mathrm{A}\mathrm{D}-\mathrm{CA}^{-1} \mathrm{~B}ΔAD−CA−1 B 称为 A\mathrm{A}A 关于 M\mathrm{M}M 的舒尔补。
拆解后的优化问题可通过舒尔补对矩阵三角化 即 [I0−HrmHmm−1I][HmmHmrHrmHrr][XmXr][I0−HrmHmm−1I][bmbr]\begin{aligned} {\left[\begin{array}{cc} I 0 \\ -H_{r m} H_{m m}^{-1} I \end{array}\right]\left[\begin{array}{cc} H_{m m} H_{m r} \\ H_{r m} H_{r r} \end{array}\right]\left[\begin{array}{c} X_m \\ X_r \end{array}\right] } \\ {\left[\begin{array}{cc} I 0 \\ -H_{r m} H_{m m}^{-1} I \end{array}\right]\left[\begin{array}{l} b_m \\ b_r \end{array}\right] } \end{aligned} [I−HrmHmm−10I][HmmHrmHmrHrr][XmXr][I−HrmHmm−10I][bmbr] 进一步化简得 [HmmHmr0Hrr−HrmHmm−1Hmr][XmXr][bmbr−HrmHmm−1bm]\begin{aligned} {\left[\begin{array}{cc} H_{m m} H_{m r} \\ 0 H_{r r}-H_{r m} H_{m m}^{-1} H_{m r} \end{array}\right]\left[\begin{array}{c} X_m \\ X_r \end{array}\right] } \\ {\left[\begin{array}{c} b_m \\ b_r-H_{r m} H_{m m}^{-1} b_m \end{array}\right] } \end{aligned} [Hmm0HmrHrr−HrmHmm−1Hmr][XmXr][bmbr−HrmHmm−1bm]此时可以利用等式第2行直接得到: (Hrr−HrmHmm−1Hmr)Xrbr−HrmHmm−1bm\left(H_{r r}-H_{r m} H_{m m}^{-1} H_{m r}\right) X_rb_r-H_{r m} H_{m m}^{-1} b_m (Hrr−HrmHmm−1Hmr)Xrbr−HrmHmm−1bm 其含义为此时可以不依赖 XmX_mXm 求解出 XrX_rXr若我们只关心 XrX_rXr 的值则可以把 XmX_mXm 从模型里删除。
2.2 从滤波角度理解边缘化
kalman滤波是此前已经熟悉的从边缘化的 角度重新看一遍滤波器的推导更有利于深入 理解。 运动模型与观测模型分别为: xkAk−1xk−1vkwkykCkxknk\begin{aligned} \mathbf{x}_k\mathbf{A}_{k-1} \mathbf{x}_{k-1}\mathbf{v}_k\mathbf{w}_k \\ \mathbf{y}_k\mathbf{C}_k \mathbf{x}_k\mathbf{n}_k \end{aligned} xkAk−1xk−1vkwkykCkxknk 其中 k1…Kk1 \ldots Kk1…K状态量的求解可以等效为如下模型 x^argminxJ(x)\hat{\mathbf{x}}\arg \min _{\mathbf{x}} J(\mathbf{x}) x^argxminJ(x) 其中 J(x)∑k0K(Jv,k(x)Jy,k(x))Jv,k(x){12(x0−xˇ0)TPˇ0−1(x0−xˇ0),k012(xk−Ak−1xk−1−vk)T×Qk−1(xk−Ak−1xk−1−vk),k1…KJy,k(x)12(yk−Ckxk)TRk−1(yk−Ckxk),k0…K\begin{aligned} J(\mathbf{x})\sum_{k0}^K\left(J_{v, k}(\mathbf{x})J_{y, k}(\mathbf{x})\right) \\ J_{v, k}(\mathbf{x})\left\{\begin{array}{c} \frac{1}{2}\left(\mathbf{x}_0-\check{\mathbf{x}}_0\right)^T \check{\mathbf{P}}_0^{-1}\left(\mathbf{x}_0-\check{\mathbf{x}}_0\right), k0 \\ \frac{1}{2}\left(\mathbf{x}_k-\mathbf{A}_{k-1} \mathbf{x}_{k-1}-\mathbf{v}_k\right)^T \\ \times \mathbf{Q}_k^{-1}\left(\mathbf{x}_k-\mathbf{A}_{k-1} \mathbf{x}_{k-1}-\mathbf{v}_k\right), k1 \ldots K \end{array}\right. \\ J_{y, k}(\mathbf{x})\frac{1}{2}\left(\mathbf{y}_k-\mathbf{C}_k \mathbf{x}_k\right)^T \mathbf{R}_k^{-1}\left(\mathbf{y}_k-\mathbf{C}_k \mathbf{x}_k\right), \quad k0 \ldots K \end{aligned} J(x)k0∑K(Jv,k(x)Jy,k(x))Jv,k(x)⎩⎨⎧21(x0−xˇ0)TPˇ0−1(x0−xˇ0),k021(xk−Ak−1xk−1−vk)T×Qk−1(xk−Ak−1xk−1−vk),k1…KJy,k(x)21(yk−Ckxk)TRk−1(yk−Ckxk),k0…K将上述模型整理为更简洁的形式令 z[xˇ0v1⋮vKy0y1⋮yK],x[x0⋮xK]\mathbf{z}\left[\begin{array}{c} \check{\mathbf{x}}_0 \\ \mathbf{v}_1 \\ \vdots \\ \mathbf{v}_K \\ \hline \mathbf{y}_0 \\ \mathbf{y}_1 \\ \vdots \\ \mathbf{y}_K \end{array}\right], \quad \mathbf{x}\left[\begin{array}{c} \mathbf{x}_0 \\ \vdots \\ \mathbf{x}_K \end{array}\right] zxˇ0v1⋮vKy0y1⋮yK,xx0⋮xKH[1−A01⋱⋱−AK−11C0C1⋱CK]W[Pˇ0Q1⋱QKR0R1RK]\begin{aligned} \mathbf{H}\left[\begin{array}{cccc} \mathbf{1} \\ -\mathbf{A}_0 \mathbf{1} \\ \ddots \ddots \\ -\mathbf{A}_{K-1} \mathbf{1} \\ \hline \mathbf{C}_0 \mathbf{C}_1 \\ \ddots \\ \mathbf{C}_K \end{array}\right] \\ \mathbf{W}\left[\begin{array}{llll|lll} \check{\mathbf{P}}_0 \\ \mathbf{Q}_1 \\ \ddots \\ \mathbf{Q}_K \\ \hline \mathbf{R}_0 \\ \mathbf{R}_1 \\ \mathbf{R}_K \end{array}\right] \\ \end{aligned} H1−A0C01⋱C1⋱−AK−1⋱1CKWPˇ0Q1⋱QKR0R1RK此时目标函数可以重新表示为 J(x)12(z−Hx)TW−1(z−Hx)J(\mathbf{x})\frac{1}{2}(\mathbf{z}-\mathbf{H x})^T \mathbf{W}^{-1}(\mathbf{z}-\mathbf{H x}) J(x)21(z−Hx)TW−1(z−Hx)求解其最小值即令其一阶导为零 ∂J(x)∂xT∣x^−HTW−1(z−Hx^)0\left.\frac{\partial J(\mathbf{x})}{\partial \mathbf{x}^T}\right|_{\hat{\mathbf{x}}}-\mathbf{H}^T \mathbf{W}^{-1}(\mathbf{z}-\mathbf{H} \hat{\mathbf{x}})\mathbf{0} ∂xT∂J(x)x^−HTW−1(z−Hx^)0即 (HTW−1H)x^HTW−1z\left(\mathbf{H}^T \mathbf{W}^{-1} \mathbf{H}\right) \hat{\mathbf{x}}\mathbf{H}^T \mathbf{W}^{-1} \mathbf{z} (HTW−1H)x^HTW−1z然而这是批量求解模型当只关心当前时 刻(k时刻)状态时应改为滤波模型。 假设上一时刻后验为 {x^k−1,P^k−1}\left\{\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{P}}_{k-1}\right\} {x^k−1,P^k−1} 目标是得到当前时刻后验 {x^k−1,P^k−1,vk,yk}↦{x^k,P^k}\left\{\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{P}}_{k-1}, \mathbf{v}_k, \mathbf{y}_k\right\} \mapsto\left\{\hat{\mathbf{x}}_k, \hat{\mathbf{P}}_k\right\} {x^k−1,P^k−1,vk,yk}↦{x^k,P^k}由于马尔可夫性仅与前一时刻有关因此令 zk[x^k−1vkyk]Hk[1−Ak−11Ck]Wk[P^k−1QkRk]\begin{aligned} \mathbf{z}_k \left[\begin{array}{c} \hat{\mathbf{x}}_{k-1} \\ \mathbf{v}_k \\ \mathbf{y}_k \end{array}\right] \\ \mathbf{H}_k \left[\begin{array}{ccc} \mathbf{1} \\ -\mathbf{A}_{k-1} 1 \\ \mathbf{C}_k \end{array}\right] \\ \mathbf{W}_k \left[\begin{array}{ccc} \hat{\mathbf{P}}_{k-1} \\ \mathbf{Q}_k \\ \mathbf{R}_k \end{array}\right] \end{aligned} zkHkWkx^k−1vkyk1−Ak−11CkP^k−1QkRk则模型的解为 (HkTWk−1Hk)x^HkTWk−1zk\left(\mathbf{H}_k^T \mathbf{W}_k^{-1} \mathbf{H}_k\right) \hat{\mathbf{x}}\mathbf{H}_k^T \mathbf{W}_k^{-1} \mathbf{z}_k (HkTWk−1Hk)x^HkTWk−1zk 其中 x^[x^k−1′x^k]\hat{\mathbf{x}}\left[\begin{array}{c} \hat{\mathbf{x}}_{k-1}^{\prime} \\ \hat{\mathbf{x}}_k \end{array}\right] x^[x^k−1′x^k]x^k−1\hat{\mathbf{x}}_{k-1}x^k−1 和 x^k−1′\hat{\mathbf{x}}_{k-1}^{\prime}x^k−1′ 有本质区别下图可以明确展示 在此基础上求解模型可以展开为 [P^k−1−1Ak−1TQk−1Ak−1−Ak−1TQk−1−Qk−1Ak−1Qk−1CkTRk−1Ck][x^k−1′x^k][P^k−1−1x^k−1−Ak−1TQk−1vkQk−1vkCkTRk−1yk]\left[\begin{array}{cc} \hat{\mathbf{P}}_{k-1}^{-1}\mathbf{A}_{k-1}^T \mathbf{Q}_k^{-1} \mathbf{A}_{k-1} -\mathbf{A}_{k-1}^T \mathbf{Q}_k^{-1} \\ -\mathbf{Q}_k^{-1} \mathbf{A}_{k-1} \mathbf{Q}_k^{-1}\mathbf{C}_k^T \mathbf{R}_k^{-1} \mathbf{C}_k \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{x}}_{k-1}^{\prime} \\ \hat{\mathbf{x}}_k \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{P}}_{k-1}^{-1} \hat{\mathbf{x}}_{k-1}-\mathbf{A}_{k-1}^T \mathbf{Q}_k^{-1} \mathbf{v}_k \\ \mathbf{Q}_k^{-1} \mathbf{v}_k\mathbf{C}_k^T \mathbf{R}_k^{-1} \mathbf{y}_k \end{array}\right] [P^k−1−1Ak−1TQk−1Ak−1−Qk−1Ak−1−Ak−1TQk−1Qk−1CkTRk−1Ck][x^k−1′x^k][P^k−1−1x^k−1−Ak−1TQk−1vkQk−1vkCkTRk−1yk]利用舒尔补等式两边左乘如下矩阵便可以直接求解出 x^k\hat{\mathbf{x}}_kx^k 且不需求解 x^k−1′\hat{\mathbf{x}}_{k-1}^{\prime}x^k−1′ [10Qk−1Ak−1(P^k−1−1Ak−1TQk−1Ak−1)−11]\left[\begin{array}{cc} \mathbf{1} \mathbf{0} \\ \mathbf{Q}_k^{-1} \mathbf{A}_{k-1}\left(\hat{\mathbf{P}}_{k-1}^{-1}\mathbf{A}_{k-1}^T \mathbf{Q}_k^{-1} \mathbf{A}_{k-1}\right)^{-1} \mathbf{1} \end{array}\right] [1Qk−1Ak−1(P^k−1−1Ak−1TQk−1Ak−1)−101]可得: P^k−1x^kPˇk−1(Ak−1x^k−1vk)CkTRk−1yk\hat{\mathbf{P}}_k^{-1} \hat{\mathbf{x}}_k\check{\mathbf{P}}_k^{-1}\left(\mathbf{A}_{k-1} \hat{\mathbf{x}}_{k-1}\mathbf{v}_k\right)\mathbf{C}_k^T \mathbf{R}_k^{-1} \mathbf{y}_k P^k−1x^kPˇk−1(Ak−1x^k−1vk)CkTRk−1yk其中 PˇkQkAk−1P^k−1Ak−1TP^k(Pˇk−1CkTRk−1Ck)−1\begin{aligned} \check{\mathbf{P}}_k\mathbf{Q}_k\mathbf{A}_{k-1} \hat{\mathbf{P}}_{k-1} \mathbf{A}_{k-1}^T \\ \hat{\mathbf{P}}_k\left(\check{\mathbf{P}}_k^{-1}\mathbf{C}_k^T \mathbf{R}_k^{-1} \mathbf{C}_k\right)^{-1} \end{aligned} PˇkQkAk−1P^k−1Ak−1TP^k(Pˇk−1CkTRk−1Ck)−1以上过程核心即为边缘化因此滤波(IEKF)可以看做长度为1的滑动窗口。
3. 基于kitti的实现原理
3.1 基于地图定位的滑动窗口模型 窗口优化模型构成 在图优化模型中优化模型也可写成如下形式 J⊤ΣJδx−J⊤Σr\mathbf{J}^{\top} \boldsymbol{\Sigma} \mathbf{J} \delta \boldsymbol{x}-\mathbf{J}^{\top} \boldsymbol{\Sigma} \mathbf{r} J⊤ΣJδx−J⊤Σr其中 rrr 是残差 JJJ 是残差关于状态量的雅可比 ∑\sum∑ 是信息矩阵。 在kitti工程中基于地图定位的滑动窗口其残差包括: 地图匹配位姿和优化变量的残差激光里程计相对位姿和优化变量的残差IMU预积分和优化变量的残差边缘化形成的先验因子对应的残差 此处先介绍前3项第4项待边缘化后介绍。 地图匹配位姿和优化变量的残差 该残差对应的因子为地图先验因子。 一个因子仅约束一个位姿其模型如下 残差关于优化变量的雅可比可视化如下 因此对应的Hessian矩阵的可视化为 激光里程计相对位姿和优化变量的残差 该残差对应的因子为激光里程计因子。 一个因子约束两个位姿其模型如下 残差关于优化变量的雅可比可视化如下 因此对应的Hessian矩阵可视化为 IMU预积分和优化变量的残差 该残差对应的因子为IMU因子。 一个因子约束两个位姿并约束两个时刻 IMU 的速度与 bias。 残差关于优化变量的雅可比可视化如下 因此对应的Hessian矩阵可视化为 完整模型 完整Hessian矩阵即为以上各因子对应矩阵的累加。 上述过程用公式可表示为: J⊤ΣJ⏟Hδx−J⊤Σr⏟b \underbrace{\mathbf{J}^{\top} \boldsymbol{\Sigma} \mathbf{J}}_{\mathbf{H}} \delta \boldsymbol{x}\underbrace{-\mathbf{J}^{\top} \boldsymbol{\Sigma} \boldsymbol{r}}_{\text {b }} HJ⊤ΣJδxb −J⊤Σr其中 r[rY0rY1rY2rL0rL1rM0rM1]\boldsymbol{r}\left[\begin{array}{l} \boldsymbol{r}_{Y 0} \\ \boldsymbol{r}_{Y 1} \\ \boldsymbol{r}_{Y 2} \\ \boldsymbol{r}_{L 0} \\ \boldsymbol{r}_{L 1} \\ \boldsymbol{r}_{M 0} \\ \boldsymbol{r}_{M 1} \end{array}\right] rrY0rY1rY2rL0rL1rM0rM1 J∂r∂δx[∂r0∂δ0∂r1∂δx∂r2∂δx∂rL0∂δx∂rL1∂δx∂rM0∂δx∂rM1∂δx][J1J2J3J4J5J6J7]J⊤[J1⊤J2⊤J3⊤J4⊤J5⊤J6⊤J7⊤]\begin{aligned} \mathbf{J}\frac{\partial \mathbf{r}}{\partial \delta x}\left[\begin{array}{c} \frac{\partial \mathbf{r}_0}{\partial \delta_0} \\ \frac{\partial \mathbf{r}_1}{\partial \delta x} \\ \frac{\partial \mathbf{r}_2}{\partial \delta x} \\ \frac{\partial \mathbf{r}_{L 0}}{\partial \delta x} \\ \frac{\partial \mathbf{r}_{L 1}}{\partial \delta x} \\ \frac{\partial \mathbf{r}_{M 0}}{\partial \delta x} \\ \frac{\partial \mathbf{r}_{M 1}}{\partial \delta x} \end{array}\right]\left[\begin{array}{l} \mathbf{J}_1 \\ \mathbf{J}_2 \\ \mathbf{J}_3 \\ \mathbf{J}_4 \\ \mathbf{J}_5 \\ \mathbf{J}_6 \\ \mathbf{J}_7 \end{array}\right] \\ \mathbf{J}^{\top}\left[\begin{array}{lllllll} \mathbf{J}_1^{\top} \mathbf{J}_2^{\top} \mathbf{J}_3^{\top} \mathbf{J}_4^{\top} \mathbf{J}_5^{\top} \mathbf{J}_6^{\top} \mathbf{J}_7^{\top} \end{array}\right] \\ \end{aligned} J∂δx∂r∂δ0∂r0∂δx∂r1∂δx∂r2∂δx∂rL0∂δx∂rL1∂δx∂rM0∂δx∂rM1J1J2J3J4J5J6J7J⊤[J1⊤J2⊤J3⊤J4⊤J5⊤J6⊤J7⊤]矩阵乘法写成累加形式为: ∑i17Ji⊤ΣiJiδx−∑i17Ji⊤Σiri\sum_{i1}^7 \mathbf{J}_i^{\top} \boldsymbol{\Sigma}_i \mathbf{J}_i \delta \boldsymbol{x}-\sum_{i1}^7 \mathbf{J}_i^{\top} \boldsymbol{\Sigma}_i \mathbf{r}_i i1∑7Ji⊤ΣiJiδx−i1∑7Ji⊤Σiri此累加过程即对应前面可视化时各矩阵叠加。
3.2 边缘化过程 移除老的帧 假设窗口长度为3在加入新的帧之前需要先边缘化掉老的帧即下图方框中的变量。 用前述公式可以表示为 (Hrr−HrmHmm−1Hmr)δxrbr−HrmHmm−1bm\left(H_{r r}-H_{r m} H_{m m}^{-1} H_{m r}\right) \delta x_rb_r-H_{r m} H_{m m}^{-1} b_m (Hrr−HrmHmm−1Hmr)δxrbr−HrmHmm−1bm但是在实际代码中会把它拆成两步实现。 第一步使用和要边缘化掉的量无关的因子构建剩余变量对应的Hessian矩阵。 上标 aaa 代表是第一步中的变量包含Hessian矩阵的完整表达式为 HrraδxrbraH_{r r}^a \delta x_rb_r^a Hrraδxrbra第二步挑出和要边缘化掉的量相关的因子构建待边缘化的Hessian矩阵并使用舒尔补做边缘化。 上标 bbb 代表是第二步中的变量包含Hessian矩阵的完整表达式为 [Hrrb−Hrmb(Hmmb)−1Hmrb]δxrbrb−Hrmb(Hmmb)−1bmb\left[H_{r r}^b-H_{r m}^b\left(H_{m m}^b\right)^{-1} H_{m r}^b\right] \delta x_rb_r^b-H_{r m}^b\left(H_{m m}^b\right)^{-1} b_m^b [Hrrb−Hrmb(Hmmb)−1Hmrb]δxrbrb−Hrmb(Hmmb)−1bmb这一步形成的约束(上式)就叫先验因子它包含了边缘化掉的量对剩余变量的约束关系。 最终使用的是两步的叠加Hessian矩阵叠加的可视化如下 对应的完整公式为 HrrδxrbrH_{r r} \delta x_rb_r Hrrδxrbr其中 HrrHrraHrrb−Hrmb(Hmmb)−1Hmrbbrbrabrb−Hrmb(Hmmb)−1bmb\begin{gathered} H_{r r}H_{r r}^aH_{r r}^b-H_{r m}^b\left(H_{m m}^b\right)^{-1} H_{m r}^b \\ b_rb_r^ab_r^b-H_{r m}^b\left(H_{m m}^b\right)^{-1} b_m^b \end{gathered} HrrHrraHrrb−Hrmb(Hmmb)−1Hmrbbrbrabrb−Hrmb(Hmmb)−1bmb边缘化之后模型如下 注意边缘化先验因子只有在第一次边缘化之前是不存在的完成第一次边缘化之后就一直存在并且随着后续新的边缘化进行其内容不断更新。 添加新的帧 添加新的帧之后模型如下 此处直接给出新的Hessian矩阵可视化结果 此后随着定位过程的进行便不断循环“边缘化老帧-添加新帧”的过程从而维持窗口长度不变。 该过程的代码实现可参考后面lio-mapping的实现理解后者便很容易实现前者。
4. lio-mapping 介绍
4.1 核心思想 基于滑动窗口方法把雷达线/面特征、IMU预积分等的约束放在一起进行优化。
o 到 i 是滑窗只有 p 到 i 的位姿在滑窗中优化o 到 p 是为了构建局部地图防止地图过于稀疏局部地图都投影到 p 的位姿处滑窗中点云约束是当前优化帧和局部地图特征匹配因此特征对应的因子约束的是 p 帧和 k 帧(pkj)。
其优化模型为 minX12{∥rP(X)∥2∑m∈mLαα∈{p1,⋯,j}∥rL(m,X)∥CLαm2∑β∈{p,⋯,j−1}∥rB(zβ1β,X)∥CBβ1Bβ2}\begin{aligned} \min _{\mathbf{X}} \frac{1}{2}\left\{\left\|\mathbf{r}_{\mathcal{P}}(\mathbf{X})\right\|^2\sum_{\substack{m \in \mathbf{m}_{L_\alpha} \\ \alpha \in\{p1, \cdots, j\}}}\left\|\mathbf{r}_{\mathcal{L}}(m, \mathbf{X})\right\|_{\mathbf{C}_{L_\alpha}^m}^2\right. \\ \left.\sum_{\beta \in\{p, \cdots, j-1\}}\left\|\mathbf{r}_{\mathcal{B}}\left(z_{\beta1}^\beta, \mathbf{X}\right)\right\|_{\mathbf{C}_{B_{\beta1}}^{B_\beta}}^2\right\} \end{aligned} Xmin21⎩⎨⎧∥rP(X)∥2m∈mLαα∈{p1,⋯,j}∑∥rL(m,X)∥CLαm2β∈{p,⋯,j−1}∑rB(zβ1β,X)CBβ1Bβ2⎭⎬⎫其中 rP(X)\mathbf{r}_{\mathcal{P}}(\mathbf{X})rP(X) 是边缘化产生的先验因子对应的残差 rL(m,X)\mathbf{r}_{\mathcal{L}}(m, \mathbf{X})rL(m,X) 是点云特征匹配对应的残差 rB(zβ1β,X)\mathbf{r}_{\mathcal{B}}\left(z_{\beta1}^\beta, \mathbf{X}\right)rB(zβ1β,X) 是IMU约束对应的残差。
4.2 具体流程
流程讲解思路 • 以前述kitti中实现原理为基础此处只是多了点云特征的约束 • 只介绍可借鉴的内容因此不介绍bias、外参初始化和外参优化等内容。
4.2.1 各类因子 定义IMU 因子 添加imu因子 定义点云面特征因子 添加点云面特征 定义边缘化先验因子 由于不确定边缘化后会和哪些量产生关联因此没有固定size。 其详细内容待讲解边缘化实现时再展开。 添加边缘化先验因子
4.2.2 滑窗模型 • 帧与帧之间通过特征约束因此没有了激光里程计因子。 • 当前模型中没有使用点云的线特征。 IMU预积分和优化变量的残差 一个因子约束两个位姿并约束两个时刻 IMU 的速度与 bias 残差关于优化变量的雅可比可视化如下 因此对应的Hessian矩阵的可视化为 点云面特征对应的残差 一个因子约束两个位姿 残差关于优化变量的雅可比可视化如下 因此对应的Hessian矩阵的可视化为 完整模型 以上工程就是前述代码中添加各类因子到模型的过程。
4.2.3 边缘化 边缘化模型 需要边缘化掉的为方框中的变量 边缘化可视化 第一步使用和要边缘化掉的量无关的因子构建剩余变量对应的Hessian矩阵。 第二步挑出和要边缘化掉的量相关的因子构建待边缘化的Hessian矩阵并使用舒尔补做边缘化。 对应的完整Hessian矩阵就是他们合在一起的结果。 边缘化实现 核心思路是把要边缘化掉的变量以及跟这些变量被同一个因子约束的变量汇总在一起。 在该例中把和模型无关的量去除剩余部分如下(设每帧有20个面特征) 因此放入MarginalizationInfo中的信息包括: 5个变量: T0M0T1M1T2T_0 \quad M_0 \quad T_1 \quad M_1 T_2T0M0T1M1T2 41个因子 40 个面特征因子、1个IMU因子 此处假设的是第一次进行边缘化若不是第 一次因子中还应该有边缘化先验因子) 找出所有变量后需要知道哪些是应该边缘化的哪些是应该保留的。 右图代码中形参里_parameter_blocks包含所有相关参数而_drop_set即为这些参数中要边缘化掉的参数的id。 添加和IMU因子相关的ResidualBlockInfo 添加和面特征因子相关的ResidualBlockInfo 在添加以上ResidualBlockInfo的同时核心变量parameter_block_size 就被赋值。 第二个核心函数的作用是计算每个因子对应的变量(parameter_blocks)、误差项(residuals)、雅可比矩阵(jacobians)并把变量数值放到parameter_block_data中。 第三个核心函数的作用构建Hessian矩阵Schur掉需要marg的变量得到对剩余变量的约束即为边缘化约束先验约束。 函数的前半部分对m、n和parameter_block_idx 这三个核心变量进行了赋值。 函数的中间部分开始构建Hessian 矩阵由于使用多线程因此要给不同的线程平均分配因子。 函数的最后便是执行边缘化得到边缘化先验因子。 上述过程是假设第一次执行边缘化当不是第一次时步骤上只是多了在AddResidualBlockInfo时把边缘化先验因子也加入进来剩余过程不变。
4.2.4 添加新帧
边缘化之后模型如下 注意由于面特征因子都是和第一帧(T0T_0T0)关联当它边缘化掉之后该因子便不存在因此在加入新的帧的同时还需要构建所有帧和当前第一帧(T1T_1T1)的面特征关联。
添加新的帧以后模型如下 Hessian矩阵可视化如下 此后不断循环该过程便可以实现lio功能。
