激光雷达的运动补偿投影矩阵

wzf@robotics_notes

已于 2023-10-17 12:47:21 修改

阅读量241

点赞数 1

分类专栏：机器人感知文章标签：机器人算法自动驾驶矩阵线性代数

于 2023-10-12 22:21:33 首次发布

本文链接：https://blog.csdn.net/woyaomaishu2/article/details/133800227

版权

机器人感知专栏收录该内容

21 篇文章 3 订阅

订阅专栏

Title: 激光雷达的运动补偿投影矩阵

文章目录

前言
I. 符号定义
II. 问题由来
III. 简单思路
IV. 问题条件
V. 向帧尾投影的投影矩阵
VI. 向帧头投影的投影矩阵
VII. 结论
参考文献

前言

扫描式的激光雷达, 没有 “全局快门”, 同一帧内的激光数据是不同时刻的测量值. 这样存在激光雷达运动导致的激光点云变形, 或者被测物体运动导致的点云变形.

机器人 SLAM 应用中, 一般是为了对固态环境建模, 动态被测物体多数情况要被滤除. 故我们先不考虑 “被测物体运动导致的点云变形”.

这里我们简单记录 LOAM^[1] 及相关演变算法 Fast_LIO^[2] 的运动补偿投影矩阵的推导计算.

I. 符号定义

符号	含义
j	一帧激光点云中包含的第 i 个激光点, 也可视作被测物体点如果激光雷达每一帧共扫描 $n$ 个点, 则 $j\in [1,\cdots, n]$
{G}	全局坐标系
{K}	开始一帧扫描时 (时间为 $t_{K}$ ), 激光雷达上的瞬时局部坐标系, 称作帧头坐标系 (该坐标系形成后就不再变化, 不会继续随着激光雷达一起运动)
{K+1}	结束一帧扫描时 (时间为 $t_{K+1}$ ), 激光雷达上的瞬时局部坐标系, 称作帧尾坐标系 (帧尾也是下一帧的帧头. 该坐标系形成后就不再变化, 不会继续随着雷达一起运动）
{j}	激光雷达获得扫描点 j 时, 其上的瞬时局部坐标系, 称作帧中坐标系, 此时的时间为 $t_{j}$ (该坐标系形成后就不再变化, 不会继续随着激光雷达一起运动)
$\left({^{\{G\}}\mathbf{R}_{\{K\}}},\, {^{\{G\}}\mathbf{P}_{\{K\}}}\right)$	帧头坐标系 {K} 在全局坐标系 {G} 中的位姿 (或全局坐标系 {G} 转移到帧头坐标系 {K} 的姿态和位置变换)
$\left({^{\{G\}}\mathbf{R}_{\{K+1\}}},\, {^{\{G\}}\mathbf{P}_{\{K+1\}}}\right)$	帧尾坐标系 {K+1} 在全局坐标系 {G} 中的位姿 (或全局坐标系 {G} 转移到帧尾坐标系 {K+1} 的姿态和位置变换)
$\left({^{\{G\}}\mathbf{R}_{\{j\}}},\, {^{\{G\}}\mathbf{P}_{\{j\}}}\right)$	帧中坐标系 {j} 在全局坐标系 {G} 中的位姿 (或全局坐标系 {G} 转移到帧中坐标系 {j} 的姿态和位置变换)
$\left({^{\{j\}}\mathbf{R}_{\{K\}}},\, {^{\{j\}}\mathbf{P}_{\{K\}}}\right)$	帧头坐标系 {K} 在帧中坐标系 {j} 中的位姿 (或帧中坐标系 {j} 转移到帧头坐标系 {K} 的姿态和位置变换)
$\left({^{\{j\}}\mathbf{R}_{\{K+1\}}},\, {^{\{j\}}\mathbf{P}_{\{K+1\}}}\right)$	帧尾坐标系 {K+1} 在帧中坐标系 {j} 中的位姿 (或帧中坐标系 {j} 转移到帧尾坐标系 {K+1} 的姿态和位置变换)
$\left({^{\{K\}}\mathbf{R}_{\{j\}}},\, {^{\{K\}}\mathbf{P}_{\{j\}}}\right)$	帧中坐标系 {j} 在帧头坐标系 {K} 中的位姿
$\left({^{\{K+1\}}\mathbf{R}_{\{j\}}},\, {^{\{K+1\}}\mathbf{P}_{\{j\}}}\right)$	帧中坐标系 {j} 在帧尾坐标系 {K+1} 中的位姿
$^{\{j\}}\mathbf{q}_j$	激光点 j 在帧中坐标系 {j} 中测得的坐标向量 (原始测量值)
$^{\{K\}}\mathbf{q}_j$	激光点 j 的原始测量值 $^{\{j\}}\mathbf{q}_j$ 向帧头坐标系 {K} 投影获得的修正坐标向量 (假设在帧头坐标系 {K} 中测同一个物体点 j 获得的坐标向量 )
$^{\{K+1\}}\mathbf{q}_j$	激光点 j 的原始测量值 $^{\{j\}}\mathbf{q}_j$ 向帧尾坐标系 {K+1} 投影获得的修正坐标向量 (假设在帧尾坐标系 {K+1} 中测同一个物体点 j 获得的坐标向量 )
$^{\{G\}}\mathbf{q}_j$	激光点 j 在全局坐标系 {G} 中的坐标向量 (不变量)
$^{\{G\}}{\mathbf{v}}_{K+1}$	帧尾 $t_{K+1}$ 时刻, 雷达的 (绝对) 线速度向量在全局坐标系 {G} 中的描述
$^{\{G\}}{\mathbf{v}}_{K}$	帧头 $t_{K}$ 时刻, 雷达的 (绝对) 线速度向量在全局坐标系 {G} 中的描述
$^{\{G\}}{\mathbf{v}}_{j}$	帧中 $t_j$ 时刻, 雷达的 (绝对) 线速度向量在全局坐标系 {G} 中的描述
$^{\{G\}}{\boldsymbol{\omega}}_{K+1}$	帧尾 $t_{K+1}$ 时刻, 雷达的 (绝对) 角速度向量在全局坐标系 {G} 中的描述
$^{\{G\}}{\boldsymbol{\omega}}_{K}$	帧头 $t_{K}$ 时刻, 雷达的 (绝对) 角速度向量在全局坐标系 {G} 中的描述
$^{\{G\}}{\boldsymbol{\omega}}_{j}$	帧中 $t_j$ 时刻, 雷达的 (绝对) 角速度向量在全局坐标系 {G} 中的描述
$^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}$	帧尾 $t_{K+1}$ 时刻, 雷达的 (绝对) 角速度向量在帧尾坐标系 {K+1} 中的描述
$^{\{K\}}{\boldsymbol{\omega}}_{K}$	帧头 $t_{K}$ 时刻, 雷达的 (绝对) 角速度向量在帧头坐标系 {K} 中的描述

Fig. 1 运动补偿投影的坐标关系

II. 问题由来

如前面提到的, 因为机器人在扫描环境时, 自身也在持续运动中, 故扫描式激光雷达即使是同一帧内获得的各个激光点的坐标值的基准坐标系 (激光雷达测量每个点时的位姿) 是不同的.

如 Fig. 1 所示, 第 K 帧初始测量时的基准坐标系是 {K}. 在测量过程中激光雷达在运动, 当测量第 j 点时, 基准坐标系变成了 {j}. 当测量这一帧最后一个点时, 基准坐标系已经运行到了 {K+1} 附近.

每个测量值的基准坐标系不同, 就给后续的特征提取等操作带来困难. 所以一般需要将同一帧内的各个激光点测量值向统一的基准坐标系投影. 这种统一的基准坐标系一般选择帧头坐标系和帧尾坐标系. 这种将帧内激光点坐标向量向帧头或帧尾坐标系投影的操作, 在 SLAM 中也称为运动补偿或点云去畸变^{[1] [2]}.

如果将未处理的一帧点云进行显示的话, 因为测量时的基准坐标系的变动, 这些点云没法直接真实反应外部环境, 故认为存在畸变. 因为是激光雷达测量时自身运动引起的, 故认为需要对激光雷达的运动进行补偿.

III. 简单思路

那么如何实现这种点云去畸变或运动补偿呢？

—— 将测量数据进行坐标系间的投影, 投影到统一的基准坐标系上.

就是同一帧内的各个测量点, 原来是在各自不同的帧内坐标系上表示的坐标向量, 都转换为在统一基准坐标系下描述坐标向量, 这就是投影.

但这种投影需要保持测量点在全局坐标系中的坐标位置. 就是被测量点还是那个点且纹丝不动, 原始测量值是从帧中坐标系 “看过去” 的, 投影后的测量值从帧头坐标系看过去或从帧尾坐标系 “看过去”. (LOAM 的原创者 CMU 的 Zhang Ji 提出^[1])

数值上, 投影操作通过投影矩阵实现.

IV. 问题条件

一. 假设条件

常速假设

假设同一采样帧时间内, 激光雷达的速度不变, 即

- 雷达的绝对线速度向量在全局坐标系中的描述 $^{\{G\}}{\mathbf{v}}_{K+1} = ^{\{G\}}{\mathbf{v}}_{j} = ^{\{G\}}{\mathbf{v}}_{K}$

- 雷达的绝对角速度向量在全局坐标系中的描述 ${^{\{G\}}{\boldsymbol{\omega}}_{K+1}} = {^{\{G\}}{\boldsymbol{\omega}}_{j}} = {^{\{G\}}{\boldsymbol{\omega}}_{K}}$

$\qquad$ 常速假设中的绝对线速度和绝对角速度全部在帧尾坐标系或者帧头坐标系中描述也一样成立, 只是都同时有一个固定的坐标系间转换, 如
$\begin{aligned} {^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}} &= {^{\{K+1\}}{\boldsymbol{\omega}}_{j}}\\ {^{\{K\}}{\boldsymbol{\omega}}_{j}} &= {^{\{K\}}{\boldsymbol{\omega}}_{K}} \end{aligned}\tag{IV-1-1}$

二. 可知条件

- 帧头激光雷达位姿 $\left({^{\{G\}}\mathbf{R}_{\{K\}}},\, {^{\{G\}}\mathbf{P}_{\{K\}}}\right)$ 及帧头激光雷达的运动 $^{\{G\}}{\mathbf{v}}_{K}$ 、 ${^{\{K\}}{\boldsymbol{\omega}}_{K}}$

- 帧尾激光雷达位姿 $\left({^{\{G\}}\mathbf{R}_{\{K+1\}}},\, {^{\{G\}}\mathbf{P}_{\{K+1\}}}\right)$ 及帧尾激光雷达的运动 $^{\{G\}}{\mathbf{v}}_{K+1}$ 、 ${^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}}$

$\qquad$ 帧头和帧尾的位姿和运动等数据通过激光点云的特征匹配或者 IMU 预测获得, 是 LOAM 及其变体的主要计算内容

- 第 j 个激光点的在帧中坐标系内的测量值 $^{\{j\}}\mathbf{q}_j$

$\qquad$ 为雷达局部坐标系内的原始测量数据

三. 未知条件

- 帧中激光雷达位姿 $\left({^{\{G\}}\mathbf{R}_{\{j\}}},\, {^{\{G\}}\mathbf{P}_{\{j\}}}\right)$

四. 待求值

- 帧中坐标系 {j} 和帧尾坐标系 {K+1} 之间齐次坐标转换关系 $\left({^{\{K+1\}}\mathbf{R}_{\{j\}}},\, {^{\{K+1\}}\mathbf{P}_{\{j\}}}\right)$

- 帧中坐标系 {j} 和帧头坐标系 {K} 之间齐次坐标转换关系 $\left( {^{\{K\}}\mathbf{R}_{\{j\}}},\, {^{\{K\}}\mathbf{P}_{\{j\}}}\right)$

V. 向帧尾投影的投影矩阵

一. 投影不变

我们知道不管如何投影, 都要保证激光点 j 在全局坐标系 {G} 中的坐标向量 $^{\{G\}}\mathbf{q}_j$ 不变.

参照 Fig. 1 并由局部坐标系 {j} 和全局坐标系 {G} 之间齐次坐标转换关系知道
$\begin{bmatrix} ^{\{G\}}\mathbf{q}_j \\ 1 \end{bmatrix}= \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{V-1-1}$

类似地, 由局部坐标系 {K+1} 和全局坐标系 {G} 之间齐次坐标转换关系知道

$\begin{bmatrix} {^{\{G\}}\mathbf{q}_j} \\ 1 \end{bmatrix} = \begin{bmatrix} {^{\{G\}}\mathbf{R}_{\{K+1\}} } & {^{\{G\}}\mathbf{P}_{\{K+1\}}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} {^{\{K+1\}}\mathbf{q}_{j}}\\ 1 \end{bmatrix} \tag{V-1-2}$

两式结合

$\begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}} &^{\{G\}}\mathbf{P}_{\{K+1\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{K+1\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{V-1-3}$

得到

$\begin{bmatrix} ^{\{K+1\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}} &^{\{G\}}\mathbf{P}_{\{K+1\}}\\ \mathbf{0} & 1 \end{bmatrix} ^{-1} \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{V-1-4}$

二. 投影矩阵

另一方面, 局部坐标系 {j} 和局部坐标系 {K+1} 之间齐次坐标转换关系可知
$\begin{bmatrix} ^{\{K+1\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{K+1\}}\mathbf{R}_{\{j\}} &^{\{K+1\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{V-2-1}$

结合式 (V-1-4) 和式 (V-2-1) 得到

$\begin{bmatrix} ^{\{K+1\}}\mathbf{R}_{\{j\}} &^{\{K+1\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}} &^{\{G\}}\mathbf{P}_{\{K+1\}}\\ \mathbf{0} & 1 \end{bmatrix} ^{-1} \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \tag{V-2-2}$

由齐次矩阵的逆

$\begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}} &^{\{G\}}\mathbf{P}_{\{K+1\}}\\ \mathbf{0} & 1 \end{bmatrix} ^{-1} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1} &- {^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1}}\cdot {^{\{G\}}\mathbf{P}_{\{K+1\}}}\\ \mathbf{0} & 1 \end{bmatrix}\tag{V-2-3}$

式 (V-2-3) 代入式 (V-2-2) 得到投影矩阵

$\begin{aligned} \begin{bmatrix} ^{\{K+1\}}\mathbf{R}_{\{j\}} &^{\{K+1\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} &= \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1} &- {^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1}}\cdot {^{\{G\}}\mathbf{P}_{\{K+1\}}}\\ \mathbf{0} & 1\end{bmatrix} \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix}\\ &= \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1} \cdot ^{\{G\}}\mathbf{R}_{\{j\}} & ^{\{G\}}\mathbf{R}_{\{K+1\}}^{-1}\cdot\left( ^{\{G\}}\mathbf{P}_{\{j\}} - {^{\{G\}}\mathbf{P}_{\{K+1\}}}\right)\\ \mathbf{0} & 1 \end{bmatrix}\\ &= \begin{bmatrix} ^{\{K+1\}}\mathbf{R}_{\{j\}} & - {^{\{G\}}\mathbf{R}_{\{K+1\}}^{\rm T}}\cdot\left( {^{\{G\}}\mathbf{P}_{\{K+1\}}} - ^{\{G\}}\mathbf{P}_{\{j\}} \right)\\ \mathbf{0} & 1 \end{bmatrix} \end{aligned} \tag{V-2-4}$

三. 矩阵计算

根据常速假设,

${^{\{K+1\}}\mathbf{R}_{\{j\}}} = {\rm{Exp}}\left(- \,{^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}}\, \Delta t_{K+1-j}\right) \tag{V-3-1}$

${^{\{G\}}\mathbf{P}_{\{K+1\}}} - ^{\{G\}}\mathbf{P}_{\{j\}} = {^{\{G\}}{\mathbf{v}}_{K+1}} \, \Delta t_{K+1-j}$

其中 $\Delta t_{K+1-j} = t_{K+1} - t_{j}$ .

这样就利用常速假设条件把帧中的未知条件替换掉了. 最后得到向帧尾投影的投影矩阵为

$\begin{aligned} \begin{bmatrix} {^{\{K+1\}}\mathbf{R}_{\{j\}}} & {^{\{K+1\}}\mathbf{P}_{\{j\}}}\\ \mathbf{0} & 1 \end{bmatrix} = \begin{bmatrix} {\rm{Exp}}\left(- \,{^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}} \cdot \Delta t_{K+1-j}\right) & - {^{\{G\}}\mathbf{R}_{\{K+1\}}^{\rm T}}\cdot {^{\{G\}}{\mathbf{v}}_{K+1}} \cdot \Delta t_{K+1-j} \\ \mathbf{0} & 1 \end{bmatrix} \end{aligned} \tag{V-3-2}$

之所以投影矩阵 (V-3-2) 的计算基于 $^{\{K+1\}}{\boldsymbol{\omega}}_{K+1}$ 和 ${^{\{G\}}{\mathbf{v}}_{K+1}}$ 是由状态向量决定的.

状态向量的选择, 在机器人状态估计中已基本形成一些约定俗成的套路, 此处我们不展开.

如果进一步推导

${^{\{G\}}\mathbf{R}_{\{K+1\}}^{\rm T}}\cdot {^{\{G\}}{\mathbf{v}}_{K+1}} \cdot \Delta t_{K+1-j} } = - {^{\{K+1\}}{\mathbf{v}}_{K+1}} \cdot \Delta t_{K+1-j} \tag{V-3-3}$

看起来更简洁, 但 ${^{\{K+1\}}{\mathbf{v}}_{K+1}}$ 不是状态向量中的元素, 故此处投影矩阵计算的最终形式还是式 (V-3-2).

四. 源码实现

查看一下 https://github.com/hku-mars/LiDAR_IMU_Init ^[2] 中的实现

    /**CV model： un-distort pcl using linear interpolation **/
    if(lidar_type != L515){
        auto it_pcl = pcl_out.points.end() - 1;
        double dt_j = 0.0;
        for(; it_pcl != pcl_out.points.begin(); it_pcl --)
        {
            dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000);
            M3D R_jk(Exp(state_inout.bias_g, - dt_j));
            V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z);
            // Using rotation and translation to un-distort points
            V3D p_jk;
            p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j;

            V3D P_compensate =  R_jk * P_j + p_jk;

            /// save Undistorted points and their rotation
            it_pcl->x = P_compensate(0);
            it_pcl->y = P_compensate(1);
            it_pcl->z = P_compensate(2);
        }
    }

源码中 it_pcl->curvature 存储每个点的时间, 单位 ms. 和上面的公式对应上了.

VI. 向帧头投影的投影矩阵

类似地, 我们简单记录一下. 由投影不变

$\begin{bmatrix} ^{\{G\}}\mathbf{q}_j \\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{K\}} &^{\{G\}}\mathbf{P}_{\{K\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{K\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{G\}}\mathbf{R}_{\{j\}} &^{\{G\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{VI-1}$

由坐标转换

$\begin{bmatrix} ^{\{K\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} = \begin{bmatrix} ^{\{K\}}\mathbf{R}_{\{j\}} &^{\{K\}}\mathbf{P}_{\{j\}}\\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^{\{j\}}\mathbf{q}_{j}\\ 1 \end{bmatrix} \tag{VI-2}$

上述两式结合起来得到

$\begin{aligned} \begin{bmatrix} {^{\{K\}}\mathbf{R}_{\{j\}}} &{^{\{K\}}\mathbf{P}_{\{j\}}}\\ \mathbf{0} & 1 \end{bmatrix} &= {\begin{bmatrix} {^{\{G\}}\mathbf{R}_{\{K\}}} & {^{\{G\}}\mathbf{P}_{\{K\}}}\\ \mathbf{0} & 1 \end{bmatrix}}^{-1} \begin{bmatrix} {^{\{G\}}\mathbf{R}_{\{j\}}} &{^{\{G\}}\mathbf{P}_{\{j\}}}\\ \mathbf{0} & 1 \end{bmatrix}\\ &=\begin{bmatrix} {^{\{K\}}\mathbf{R}_{\{j\}}} & {^{\{G\}}\mathbf{R}_{\{K\}}^{\rm T}} \cdot \left( {^{\{G\}}\mathbf{P}_{\{j\}}} - {^{\{G\}}\mathbf{P}_{\{K\}}} \right)\\ \mathbf{0} & 1 \end{bmatrix}\\ &= \begin{bmatrix} {\rm{Exp}}\left( \,{^{\{K\}}{\boldsymbol{\omega}}_{K}} \cdot \Delta t_{j-K}\right) & {^{\{G\}}\mathbf{R}_{\{K\}}^{\rm T}} \cdot {^{\{G\}}{\mathbf{v}}_{K}} \cdot \Delta t_{j-K} \\ \mathbf{0} & 1 \end{bmatrix} \end{aligned} \tag{VI-3}$

其中 $\Delta t_{j-k} = t_{j} - t_{K}$ .

VII. 结论

这里的运动补偿投影矩阵就是简单的坐标变换.

在坐标变换的展开计算过程中, 要以系统状态为自变量.

也可以通过插值来实现^[1], 此处没有涉及.

(日常记录, 如有问题请指教)

参考文献

[1] Zhang, J., Singh, S. “Low-drift and real-time lidar odometry and mapping”. Auton Robot 41, 401–416 (2017)

[2] F. Zhu, Y. Ren and F. Zhang, “Robust Real-time LiDAR-inertial Initialization,” 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022, pp. 3948-3955