精华内容
下载资源
问答
  • LOAM论文解读
    2020-10-10 10:37:38

    概述

    算法概括

    激光雷达在走动的时候每列点云都具有不同的被扫描位置,因此会带来误差。LOAM通过估计雷达的速度来修正了雷达运动导致的帧间扭曲。
    在这里插入图片描述

    整体算法分为两部分:一部分能够快速计算里程计,并对速度进行估计。另一部分算法做fine matching 并配准点云。如果有IMU信息可用,高频运动也能被捕捉。
    两种算法中的点云匹配都是基于提取陡峭的边和平面特征的。将特征点和边缘线、平面块匹配。在里程计算法里,这些匹配都被快速计算;在建图算法里,匹配通过计算点云的特征值特征向量实现几何特征的匹配。

    软硬件架构

    在这里插入图片描述他用了个40线,40Hz的激光雷达,智能扫到前方180°范围内的东西。

    在这里插入图片描述

    里程计

    特征点

    在这里插入图片描述首先把每一个scan分成四份,通过计算这个c值,每一份最多能够得到四个平面点或者两个边缘点。
    在这里插入图片描述除了c之外,还需要考虑如ab两种情况,左边的情况是patch和beam平行,右边的情况是存在遮挡时,A点就不作为edge了。

    特征匹配

    LOAM考虑了激光雷达螺旋前进的特征,类似于摆线,同一个frame中的不同点实际上在不同的时间被测量,因此需要projection来进行修正。
    在这里插入图片描述他把蓝色的投影到绿色的,时间从 t k + 1 t_{k+1} tk+1开始,橙色的新一帧点逐渐被加入,并不断代更新当前时刻位姿。根据当前估计的最新位姿,把当前时刻扫到的点都投影到 t k + 1 t_{k+1} tk+1,并在上一帧投过来的数据上检索最近邻。
    在这里插入图片描述
    对于边界点和平面点,分别采用不同的方法进行拟合。图里面所有的线都是上一时刻投影到 t k + 1 t_{k+1} tk+1时刻的。黄色的是最近邻所在的直线,然后上下找一找,需要拟合直线找两个,需要拟合平面则找三个。在计算特征点匹配的情况时,分别用到了点到直线的距离和点到平面的距离。
    在这里插入图片描述最终通过非线性优化来优化整体的距离和最小得到一个好的位姿变换。

    位姿估计

    在这里插入图片描述这个算法在P k+1中点数增多时反复进行。不断估计一帧之内的不同时刻的位姿。

    日期2020年10月9日

    更多相关内容
  • loam论文的理解.pptx

    2019-10-18 16:25:22
    对于论文loam的理解,作者提出了使用六自由度的两轴激光雷达来构建实时激光里程计。
  • LOAM 论文

    2021-12-29 22:34:07
    LOAM]1. 参考 1. 参考 LOAM 论文,CMU,2014。 LOAM 改进,sci-hub,2017。

    欢迎访问我的博客首页


    1. LOAM 概述


    1.1 主要贡献


    图1

    图 1 LOAM 系统概述

      LOAM 旨在使用一个运动的三维激光雷达进行运动估计和地图创建。由于采集时刻不同,运动的激光雷达采集到的点存在畸变,如左侧的点云。LOAM 使用两个并行的算法解决这个问题:里程计算法估计激光雷达的速度并校正点云畸变;建图算法匹配并注册点云到地图中。

    1.2 硬件


      北阳 UTM-30LX 是单线二维激光雷达,它的激光发射器沿水平方向转动,每隔 0.25° 发射一个激光束。所以其水平分辨率是 0.25 0.25 0.25°。
    北阳激光雷达

    图 2 北阳 UTM-30LX 激光雷达

      为了实现三维扫描,激光雷达被安装在一个电机上。该电机以 40 转每秒的速度,在竖直方向的 180° 范围内以逐行扫描的方式扫描 40 行。所以其竖直分辨率是 180 ÷ 40 = 4.5 180 \div 40 = 4.5 180÷40=4.5°。

      北阳电机提供激光雷达上位机软件 UrgBenri,可以很方便的进行数据的采集、显示、存储。下面是 UrgBenri 存储的数据内容,第 28 行是激光发射器旋转 270° 采集到的 270 ÷ 0.25 = 1080 270 \div 0.25 = 1080 270÷0.25=1080 个点,第 30 行是采集第一个点时的时间戳。

    [model]
    UTM-30LX(Hokuyo Automatic Co.,Ltd.)
    [frontStep]
    540
    [minDistance]
    23
    [maxDistance]
    60000
    [motorSpeed]
    2400
    [totalSteps]
    1440
    [captureMode]
    GD_Capture_mode
    [scanMsec]
    25
    [startStep]
    0
    [endStep]
    1080
    [serialNumber]
    00904066
    [timestamp]
    793464
    [logtime]
    2013:1:12:15:6:32:312
    [scan]
    392;397;398;400;403;403;407;409;409;409;411;411;422;424;424;422;420;...
    [timestamp]
    793514
    [logtime]
    2013:1:12:15:6:32:351
    [scan]
    

      上面的 1080 个点是一次扫描(scan)采集到的点。电机完成一轮逐行扫描,产生 1080 × 40 = 4.32 1080 \times 40 = 4.32 1080×40=4.32 万个点,称为一次扫描覆盖(sweep)。

    1.3 符号表示与任务描述


      LOAM 的任务是,根据三维雷达采集的点云估计雷达自身的运动,并对遍历过的场景建图。LOAM 假设雷达内参已标定且内部运动学已知。内存已标定,就可以对雷达点进行 3D 投影。还假设雷达的角速度和线速度是平滑且连续的,该假设使用 IMU 实现。

      小写的右下角标 k k k 代表第 k k k 次扫描覆盖。一次扫描覆盖采集到的点云用 P k {\mathcal P}_k Pk 表示。第 k k k 次扫描覆盖的开始时刻用 t k t_k tk 表示。

      大写的右上角标 L L L W W W 代表坐标系。激光雷达坐标系 L L L 是一个三维坐标系,原点在雷达的几何中心, x x x 轴向左, y y y 轴向上, z z z 轴向前,如图 2。激光雷达第 k k k 次扫描覆盖采集到的点 i i i 在雷达坐标系中的坐标用 X ( k , i ) L X_{(k, i)}^L X(k,i)L 表示。 T k L ( t ) T_k^L(t) TkL(t) 表示把雷达在时刻 t t t 采集到的一个点投影到时刻 t k t_k tk 所对应的变换(投影就是坐标变换)。

      世界坐标系 W W W 与激光雷达初始坐标系重合。激光雷达第 k k k 次扫描覆盖采集到的点 i i i 在世界坐标系中的坐标用 X ( k , i ) W X_{(k, i)}^W X(k,i)W 表示。 T k W ( t ) T_k^W(t) TkW(t) 表示把采集于时刻 t t t 的一点投影到世界坐标系所对应的变换。

      LOAM(lidar odometry and mapping)问题可以概况为:求点云 P k {\mathcal P}_k Pk 的位姿,并用这些点云创建地图。

    1.4 软件


      每个扫描过程中,都会把激光雷达采集到的点 P ^ \hat \mathcal P P^ 变换到激光雷达坐标系 L L L 中,得到 P \mathcal P P

      第 k k k 次扫描采集的点云 P k {\mathcal P}_k Pk 会被两个算法处理:里程计算法计算两次连续扫描间的激光雷达运动;运动估计算法对 P \mathcal P P 祛畸变。这两个算法的工作频率是 10 赫兹。

      算法的输出用于创建地图。创建地图时,把祛畸变的点云与地图匹配并加入地图,工作频率是 1 赫兹。

    图 2

    图 3 LOAM 系统框架

    2. 特征点提取


      点云 P k {\mathcal P}_k Pk 在横竖方向的分辨率不一样,即它不是均匀分布的。但是它的每行点云是共面的,40 行点云对应 40 个平面。因此,利用每行点云的共面关系提取特征点。

      特征点包含边缘点和平面点。假设 i i i P k \mathcal P_k Pk 中第 m m m n n n 列的一点,局部点集 S \mathcal S S 是第 m m m 行第 n ± x n \pm x n±x 列点云。点云 i i i 的局部表面平滑性:

    c = 1 ∣ S ∣ ⋅ ∣ ∣ X ( k , i ) L ∣ ∣ ∣ ∣ ∑ j ∈ S , j ≠ i ( X ( k , i ) L − X ( k , j ) L ) ∣ ∣ . (1) c = \frac{1} {| \mathcal S| \cdot || X_{(k, i)}^L ||} || \sum_{j \in \mathcal S, j \neq i}{} (X_{(k, i)}^L - X_{(k, j)}^L) ||. \tag{1} c=SX(k,i)L1jS,j=i(X(k,i)LX(k,j)L).(1)

    为了消除尺度影响,使用点 i i i 到激光雷达中心的距离 ∣ ∣ X ( k , i ) L ∣ ∣ || X_{(k, i)}^L || X(k,i)L 除局部表面平滑性。
    图 3

    图 4 不适合作为特征点的情况
      图 a 的实线代表局部表面,虚线代表激光束。点 A 所在的局部表面与激光束有一定的夹角,可以作为平面点;点 B 所在的局部表面几乎与激光束平行,不可作为平面点。
      图 b 的实线代表可见区域,虚线代表遮挡区域。点 C 在可见区域与遮挡区域的边界,容易被误认为是边缘点,但换个角度就可以看到遮挡区域,因此它不可作为边缘点。点 D 在一个局部平面的边缘,也不被作为边缘点。


      把一次扫描采集到的点按 c c c 的大小排序,再根据 c c c 的大小选择特征点:从大于阈值 5 × 1 0 − 3 5 \times 10^{-3} 5×103 的点中选择较大的点当作边缘点,从小于阈值 5 × 1 0 − 3 5 \times 10^{-3} 5×103 的点中选择较小的点当作平面点。为了使特征点均匀分布,把扫描平面分成四个子区域,在每个子区域中最多提取 2 个边缘点和 4 个平面点。特征点选取的注意事项:

    1. 特征点应该足够稀疏,即,特征点旁边的点不应再被当作特征点。
    2. 特征点所在的局部表面不应该与激光束平行,如图 4(a)。因为这样的点通常不可靠。
    3. 特征点不应该在遮挡区域的边界,如图 4(b)。由于橘色区域被遮挡,点 C C C 会被认为是边缘点。而实际上它是平面点,因为换个角度就能看到遮挡区域。

      为了避免上述三种点被当作特征点:要求采集点 i i i 的激光束与 S \mathcal S S 的表面法向量的夹角必须大于 10°; S \mathcal S S 内的点不能被障碍物阻断,如图 4(b) 的 D 点。
    图4

    图 5 提取到的边缘点和平面点

      图 5 是在一个走廊场景中提取的特征点。黄色代表边缘点,红色代表平面点。激光雷达以 0.5 米每秒的速度向图左侧的墙移动,造成墙上出现运动畸变。

    3. 特征点匹配


      里程计算法用于估计激光雷达在一个扫描内的运动。第 k − 1 k-1 k1 次扫描采集到的点云 P k − 1 \mathcal P_{k-1} Pk1 会被投影到时刻 t k t_k tk 得到 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1,从 P k \mathcal P_k Pk 中提取的特征点也会被投影到时刻 t k t_k tk。投影后的点用于估计雷达的运动。
    图 2

    图 6 把一次扫描采集到的点云投影到扫描结束时刻

      图 6 中,蓝色的线表示的第 k − 1 k-1 k1 次扫描采集到的点云 P k − 1 \mathcal P_{k-1} Pk1。在第 k − 1 k-1 k1 次扫描的结束时刻 t k t_k tk P k − 1 \mathcal P_{k-1} Pk1 被投影到时刻 t k t_k tk,投影后用 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 表示,如图中的绿线。在第 k k k 次扫描过程中, P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 和新采集到的点云 P k \mathcal P_k Pk(黄线)一起用于估计激光雷达的运动。

      特征点匹配时,先使用前面讨论的特征点提取算法从 P k \mathcal P_k Pk 中提取边缘点 E k \mathcal E_k Ek 和平面点 H k \mathcal H_k Hk。再从 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中选择边缘线和平面块作为边缘点和平面点的匹配。

      注意, P k \mathcal P_k Pk 在第 k k k 次扫描开始时是空的,它包含的点云数量随着扫描逐渐增长。里程计在采集过程中递归地估计激光雷达的 6 自由度运动。 E k \mathcal E_k Ek H k \mathcal H_k Hk 被投影到扫描开始时刻,得到 E ~ k \tilde \mathcal E_k E~k H ~ k \tilde \mathcal H_k H~k。我们需要在 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中为 E ~ k \tilde \mathcal E_k E~k H ~ k \tilde \mathcal H_k H~k 中的每一个特征点找到最近点。为了快速查找, P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 以三维 KD 树的形式存储,存储的形式是 L k L_k Lk

      图 7(a) 是为一个边缘点匹配一个边缘线的过程。边缘线由来自 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 的两个点 j j j l l l 表示。假设 i i i E ~ k \tilde \mathcal E_k E~k 中的一点, j j j P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中离 i i i 最近的点, l l l P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中与 j j j 相邻的两行中离 i i i 最近的点。于是,边缘线 ( j , l ) (j, l) (j,l) 就是边缘点 i i i 的匹配。然后使用公式 (1) 验证 j j j l l l 是边缘点,要求 c > 5 × 1 0 − 3 c \gt 5 \times 10^{-3} c>5×103

      之所以要求 j j j l l l 来自不同的扫描行,是考虑到一次扫描不能从同一条边缘线采集到两个及以上的点,除非边缘线在扫描平面上。如果边缘线在扫描平面上,它就退化成一条直线,这条线上的特征点一开始也就不会被当成特征点。

      图 7(b) 是为一个平面点匹配一个平面块的过程。平面块由来自 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 的三个点 j j j l l l m m m 表示。假设 i i i H k \mathcal H_k Hk 中的一点, j j j P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中离 i i i 最近的点, l l l P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中与 j j j 同行且离 i i i 第二近的点, m m m 是来自 j j j 旁边两行离 i i i 最近的点。这样可以保证三个点不共线。然后使用公式 (1) 验证 j j j l l l m m m 都是平面点,要求 c < 5 × 1 0 − 3 c \lt 5 \times 10^{-3} c<5×103
    图 3

    图 7 特征点匹配

      图 7(a) 表示为 E ~ k \tilde \mathcal E_k E~k 中的一个边缘点 i i i 匹配边缘线,图 7(b) 表示为 H ~ k \tilde \mathcal H_k H~k 中的一个平面点 i i i 匹配平面块。 j j j 是从 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中找到的离 i i i 最近的点。橘色线代表的点和 j j j 来自同一行扫描,蓝色线代表橘色线前后的两行扫描。在图 a 中,为了找到边缘线,在蓝色线中寻找点 l l l。在图 b 中,为了找到平面块,分别在橘色线和蓝色线中寻找点 l l l 和点 m m m

      接下来计算特征点到它的匹配对象的距离,然后最小化所有距离求得激光雷达的运动。

       E ~ k \tilde \mathcal E_k E~k 中的一点 i i i 到直线 ( j , l ) (j, l) (j,l) 的距离

    d E = ∣ ( X ~ ( k , i ) L − X ˉ ( k − 1 , j ) L ) × ( X ~ ( k , i ) L − X ˉ ( k − 1 , l ) L ) ∣ ∣ X ˉ ( k − 1 , j ) L − X ˉ ( k − 1 , l ) L ∣ , (2) d_{\mathcal E} = \frac {\left|\begin{array}{cccc} (\tilde X_{(k, i)}^L - \bar X_{(k-1, j)}^L) \times (\tilde X_{(k, i)}^L - \bar X_{(k-1, l)}^L) \end{array}\right|} {\left|\begin{array}{cccc} \bar X_{(k-1, j)}^L - \bar X_{(k-1, l)}^L \end{array}\right|}, \tag{2} dE=Xˉ(k1,j)LXˉ(k1,l)L(X~(k,i)LXˉ(k1,j)L)×(X~(k,i)LXˉ(k1,l)L),(2)

    其中 X ~ ( k , i ) L \tilde X_{(k, i)}^L X~(k,i)L X ˉ ( k − 1 , j ) L \bar X_{(k-1, j)}^L Xˉ(k1,j)L X ˉ ( k − 1 , l ) L \bar X_{(k-1, l)}^L Xˉ(k1,l)L 分别是 i i i j j j l l l 在坐标系 L k L_k Lk 中的坐标。 H ~ k \tilde \mathcal H_k H~k 中的一点 i i i 到平面 ( j , l , m ) (j, l, m) (j,l,m) 的距离

    d H = ∣ X ~ ( k , i ) L − X ˉ ( k − 1 , j ) L ( X ˉ ( k − 1 , j ) L − X ˉ ( k − 1 , l ) L ) × ( X ˉ ( k − 1 , j ) L − X ˉ ( k − 1 , m ) L ) ∣ ∣ ( X ˉ ( k − 1 , j ) L − X ˉ ( k − 1 , l ) L ) × ( X ˉ ( k − 1 , j ) L − X ˉ ( k − 1 , m ) L ) ∣ . (3) d_{\mathcal H} = \frac {\left|\begin{array}{cccc} \tilde X_{(k, i)}^L - \bar X_{(k-1, j)}^L\\ (\bar X_{(k-1, j)}^L - \bar X_{(k-1, l)}^L) \times (\bar X_{(k-1, j)}^L - \bar X_{(k-1, m)}^L) \end{array}\right|} {\left|\begin{array}{cccc} (\bar X_{(k-1, j)}^L - \bar X_{(k-1, l)}^L) \times (\bar X_{(k-1, j)}^L - \bar X_{(k-1, m)}^L) \end{array}\right|}. \tag{3} dH=(Xˉ(k1,j)LXˉ(k1,l)L)×(Xˉ(k1,j)LXˉ(k1,m)L)X~(k,i)LXˉ(k1,j)L(Xˉ(k1,j)LXˉ(k1,l)L)×(Xˉ(k1,j)LXˉ(k1,m)L).(3)

    公式 (2) 和公式 (3) 的推导分别参见附录 8.2 和 8.3。

    4. 运动估计


      激光雷达在一个扫描覆盖内的运动被建模为角速度和线速度都是匀速的运动,这样就可以根据时间戳使用线性插值的方式求得一个扫描内各时刻的位姿。假设 t k t_k tk 是第 k k k 次扫描覆盖的开始时刻, t t t 是当前时刻。 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 是雷达从时刻 t t t 变换回时刻 t k t_k tk 的六自由度位姿变换, T k L ( t ) = [ τ k L ( t ) , θ k L ( t ) ] T \boldsymbol T_k^L(t) = [\tau_k^L(t), \theta_k^L(t)]^T TkL(t)=[τkL(t),θkL(t)]T,其中 τ k L ( t ) = [ t x , t y , t z ] T \tau_k^L(t) = [t_x, t_y, t_z]^T τkL(t)=[tx,ty,tz]T θ k L ( t ) = [ θ x , θ y , θ z ] T \theta_k^L(t) = [\theta_x, \theta_y, \theta_z]^T θkL(t)=[θx,θy,θz]T 分别是在 L k L_k Lk 坐标系中的平移和旋转。给定 θ k L ( t ) \theta_k^L(t) θkL(t),旋转矩阵可以根据罗德里格斯(Rodrigues)公式定义为

    R k L ( t ) = e θ ^ k L ( t ) = I + θ ^ k L ( t ) ∣ ∣ θ k L ( t ) ∣ ∣ s i n ∣ ∣ θ k L ( t ) ∣ ∣ + ( θ ^ k L ( t ) ∣ ∣ θ k L ( t ) ∣ ∣ ) 2 ( 1 − ∣ ∣ c o s θ k L ( t ) ∣ ∣ ) . (4) {\bf R}_k^L(t) = e^{\hat \theta_k^L(t)} = {\bf I} + \frac{\hat \theta_k^L(t)}{|| \theta_k^L(t) ||} sin|| \theta_k^L(t) || + (\frac{\hat \theta_k^L(t)}{|| \theta_k^L(t) ||})^2 (1 - || cos\theta_k^L(t) ||). \tag{4} RkL(t)=eθ^kL(t)=I+θkL(t)θ^kL(t)sinθkL(t)+(θkL(t)θ^kL(t))2(1cosθkL(t)).(4)

    其中, θ ^ k L ( t ) \hat \theta_k^L(t) θ^kL(t) θ k L ( t ) \theta_k^L(t) θkL(t) 的斜对称矩阵。

      假设 i i i P k \mathcal P_k Pk 中的一点, t ( k , i ) t(k, i) t(k,i) 是它的时间戳, T ( k , i ) L \boldsymbol T_{(k, i)}^L T(k,i)L [ t k , t ( k , i ) ] [t_k, t_{(k, i)}] [tk,t(k,i)] 间某个时刻的位姿变换。 T ( k , i ) L \boldsymbol T_{(k, i)}^L T(k,i)L 可以通过对 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 线性插值计算得到:

    T ( k , i ) L = t ( k , i ) − t k t − t k T k L ( t ) . (5) \boldsymbol T_{(k, i)}^L = \frac{t_{(k, i)} - t_k}{t - t_k} \boldsymbol T_k^L(t). \tag{5} T(k,i)L=ttkt(k,i)tkTkL(t).(5)

    注意, T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 随着时间变化,所以上面的插值也随着时间的变化而不同。下面的公式把边缘点 E k \mathcal E_k Ek 和平面点 H k \mathcal H_k Hk 变换到扫描的开始时刻,得到 E ~ k \tilde \mathcal E_k E~k 和平面点 H ~ k \tilde \mathcal H_k H~k

    X ~ ( k , i ) L = R ( k , i ) L X ( k , i ) L + τ ( k , i ) L , (6) {\boldsymbol {\tilde X}}_{(k, i)}^L = {\bf R}_{(k, i)}^L {\boldsymbol X}_{(k, i)}^L + \tau_{(k, i)}^L, \tag{6} X~(k,i)L=R(k,i)LX(k,i)L+τ(k,i)L,(6)

    其中, X ( k , i ) L {\boldsymbol X}_{(k, i)}^L X(k,i)L E k \mathcal E_k Ek H k \mathcal H_k Hk 中的一点, X ~ ( k , i ) L {\boldsymbol {\tilde X}}_{(k, i)}^L X~(k,i)L X ( k , i ) L {\boldsymbol X}_{(k, i)}^L X(k,i)L 变换后的点。 R ( k , i ) L {\bf R}_{(k, i)}^L R(k,i)L τ ( k , i ) L \tau_{(k, i)}^L τ(k,i)L T ( k , i ) L {\boldsymbol T}_{(k, i)}^L T(k,i)L 包含的旋转矩阵和平移向量。

      公式(2)用于计算投影后的点到其匹配对象的距离,公式(6)定义投影操作。把这两个公式合写成一个公式:

    f E ( X ( k , i ) L , T k L ( t ) ) = d E , i ∈ E k . (7) f_{\mathcal E} ({\boldsymbol X}_{(k, i)}^L, {\boldsymbol T}_k^L(t)) = d_{\mathcal E}, \quad i \in {\mathcal E}_k. \tag{7} fE(X(k,i)L,TkL(t))=dE,iEk.(7)

    该公式把一个边缘点 X ( k , i ) L {\boldsymbol X}_{(k, i)}^L X(k,i)L 投影到时刻 t k t_k tk,并求投影后的点到与其匹配的边缘线的距离 d E d_{\mathcal E} dE。同样,由公式 (3) 和公式 (6) 可以得到 H k \mathcal H_k Hk 中的一个平面点到与其匹配的平面块的距离:

    f H ( X ( k , i ) L , T k L ( t ) ) = d H , i ∈ H k . (8) f_{\mathcal H} ({\boldsymbol X}_{(k, i)}^L, {\boldsymbol T}_k^L(t)) = d_{\mathcal H}, \quad i \in {\mathcal H}_k. \tag{8} fH(X(k,i)L,TkL(t))=dH,iHk.(8)

      最后使用莱文贝格-马夸特(Levenberg–Marquardt)算法求解激光雷达的运动。把公式 (7) 和 (8) 写成一个统一的形式,就得到一个非线性函数:

    f ( T k L ( t ) ) = d , (9) \boldsymbol { f(T_k^L(t)) = d }, \tag{9} f(TkL(t))=d,(9)

    其中, f \boldsymbol f f 的每行对应一个特征点, d \boldsymbol d d 是相应的距离。我们计算 f \boldsymbol f f 关于 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 的雅可比矩阵 J \bf J J,其中 J = ∂ f / ∂ T k L ( t ) {\bf J} = \partial f / \partial {\boldsymbol T}_k^L(t) J=f/TkL(t)。然后使用非线性迭代最小化 d \boldsymbol d d 就可以求解公式 (9):

    T k L ( t ) ← T k L ( t ) − ( J T J + λ d i a g ( J T J ) ) − 1 J T d . (10) {\boldsymbol T}_k^L(t) \leftarrow {\boldsymbol T}_k^L(t) - ({\bf J}^T{\bf J} + \lambda diag({\bf J}^T{\bf J}))^{-1} {\bf J}^T{\boldsymbol d}. \tag{10} TkL(t)TkL(t)(JTJ+λdiag(JTJ))1JTd.(10)

    其中 λ \lambda λ 是莱文贝格-马夸特算法需要的一个因数。

    5. 里程计算法


      下面是第 k k k 次扫描覆盖过程中,里程计算法的迭代过程。它的输入有三个:

    1. P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1: 第 k − 1 k-1 k1 次扫描覆盖采集到的点 P k − 1 \mathcal P_{k-1} Pk1 在时刻 t k t_k tk 的投影。
    2. P k \mathcal P_k Pk: 从时刻 t k t_k tk 到时刻 t t t 期间采集到的点,点的数量随着 t t t 的增大而增多。
    3. T k L ( t ) \boldsymbol T_k^L(t) TkL(t): 时刻 t t t 到时刻 t k t_k tk 的变换,其中 t k ≤ t ≤ t k + 1 t_k \le t \le t_{k+1} tkttk+1。该算法在时间段 [ t k , t k + 1 ] [t_k, t_{k+1}] [tk,tk+1] 中迭代: T k L ( t ) \boldsymbol T_k^L(t) TkL(t) t k t_k tk 时刻被赋予初始值 0,然后在每次迭代过程中更新,在 t k + 1 t_{k+1} tk+1 时刻迭代结束即为时刻 t k + 1 t_{k+1} tk+1 到时刻 t k t_k tk 的变换。

    T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 的初始值为 0。从第二次迭代开始, T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 使用上次迭代结束时它的值。 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 的更新方法:

    1. P k \mathcal P_k Pk 中选择特征点 E k \mathcal E_k Ek H k \mathcal H_k Hk
    2. 使用 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 把它们变换到 t k t_k tk 时刻,得到 E ~ k \tilde \mathcal E_k E~k H ~ k \tilde \mathcal H_k H~k
    3. 假设 i i i E ~ k \tilde \mathcal E_k E~k H ~ k \tilde \mathcal H_k H~k 中的一个点。在 P ˉ k − 1 \bar \mathcal P_{k-1} Pˉk1 中找 i i i 的匹配对象边缘线或平面块,计算 i i i 到该匹配对象的距离。调整 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 使距离总和减小,此时的 T k L ( t ) \boldsymbol T_k^L(t) TkL(t) 即为本次迭代的结果。

    当距离总和收敛或达到最大迭代次数时停止迭代。由于里程计的频率是 10 赫兹,所以距离该算法上次被调用的时间达到 100 毫秒时,该算法再次被调用。因为在这 100 毫秒中, P k \mathcal P_k Pk 中又增加了一些点。

      为了区分特征匹配的质量,为每个距离添加一个权重:特征点和它的匹配对象间的距离越大,权重越小;距离大于阈值,权重为 0。权重的定义如下:

    w = { ( 1 − α 2 ) 2 − 1 < α < 1 , 0 o t h e r w i s e , (11) w = \left\{ \begin{matrix} (1 - \alpha^2)^2 & -1 \lt \alpha \lt 1, \\ 0 & otherwise, \end{matrix} \right. \tag{11} w={(1α2)201<α<1,otherwise,(11)

    其中 α = r 6.9459 σ 1 − h \alpha = \frac{r}{6.9459 \sigma \sqrt{1 - h}} α=6.9459σ1h r r r r 是最小二乘对应的残差, σ \sigma σ 是残差的中位数绝对离差, h h h ( J ( J T J ) ) − 1 J T ({\bf J}({\bf J}^{\boldsymbol T} {\bf J}))^{-1}{\bf J}^T (J(JTJ))1JT 对角线上的杠杆值或对应元, J \bf J J 是公式 (10) 中说过的雅可比矩阵。

    6. 地图创建算法


      第 k k k 次扫描覆盖结束时,里程计算法计算出一个位姿变换 T k L ( t ) \boldsymbol T_k^L(t) TkL(t),其中 t = t k + 1 t=t_{k+1} t=tk+1。该变换可以祛除 P k \mathcal P_k Pk 中的畸变得到 P ˉ k \bar \mathcal P_k Pˉk
    图 8

    图 8 地图创建

      蓝色曲线代表雷达在地图中的位姿 T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk),该位姿是地图创建算法在第 k − 1 k-1 k1 次扫描周期内计算出来的。橘色曲线代表雷达在第 k k k 次扫描过程中的运动 T k L ( t k + 1 ) \boldsymbol T_k^L(t_{k+1}) TkL(tk+1),该运动是里程计算法计算出来的。根据 T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk) T k L ( t k + 1 ) \boldsymbol T_k^L(t_{k+1}) TkL(tk+1),里程计算法产生的无畸变点云被投影到地图中成为 Q k \mathcal Q_k Qk(绿线),并与地图中存在的点云 Q k − 1 \mathcal Q_{k-1} Qk1 (黑线)匹配。

      地图创建算法在每个扫描周期内仅被调用一次,它把 P ˉ k \bar \mathcal P_k Pˉk 与地图匹配并注册 P ˉ k \bar \mathcal P_k Pˉk 到世界坐标系 { W } \{W\} {W} 中,如图 8 所示。

      定义 Q k − 1 \mathcal Q_{k-1} Qk1 是截至第 k − 1 k-1 k1 次扫描结束时,地图中积累的点云; T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk) 是第 k − 1 k-1 k1 次扫描结束时刻 t k t_k tk,雷达在地图中的位姿。地图创建算法把 T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk) 扩展到 [ t k , t k + 1 ] [t_k, t_{k+1}] [tk,tk+1] 得到 T k W ( t k + 1 ) \boldsymbol T_k^W(t_{k+1}) TkW(tk+1),把 P ˉ k \bar \mathcal P_k Pˉk 投影到世界坐标系 { W } \{W\} {W} 得到 Q ˉ k \bar \mathcal Q_k Qˉk,然后通过优化雷达位姿 T k W ( t k + 1 ) \boldsymbol T_k^W(t_{k+1}) TkW(tk+1) 匹配 Q ˉ k \bar \mathcal Q_k Qˉk Q k − 1 \mathcal Q_{k-1} Qk1

      为了寻找特征点的匹配对象,把地图 Q k − 1 \mathcal Q_{k-1} Qk1 中的点云储存在一个 10 立方米的区域内。这个区域与 Q ˉ k \bar \mathcal Q_k Qˉk 重叠的部分中的点会按它们在世界坐标系 W {W} W 中的坐标被存储在一个三维 KD 树中。假设 S ′ \mathcal S' S Q k − 1 \mathcal Q_{k-1} Qk1 中,以特征点为中心的一个棱长为 10 厘米的立方体区域。对于一个边缘点,只保留 S ′ \mathcal S' S 中边缘线上的点;对于一个平面点,只保留 S ′ \mathcal S' S 中平面块上的点。然后计算 S ′ \mathcal S' S 的协方差矩阵 M \bf M M M \bf M M 的特征值 V \boldsymbol V V 和特征向量 E \boldsymbol E E。这些值决定点云簇的位姿和点线、点面距离。

      如果 S ′ \mathcal S' S 位于边缘线上, V \boldsymbol V V 的一个特征值会明显大于另外两个,最大的一个特征值对应的特征向量代表着边缘线的方向。如果 S ′ \mathcal S' S 位于平面块上, V \boldsymbol V V 的一个特征值会明显小于另外两个,最小的一个特征值对应的特征向量代表着平面块的方向。这样计算的边缘线和平面块经过 S ′ \mathcal S' S 的中心。

      为了计算特征点到其匹配对象的距离,在边缘线上选择两个点,在平面块上选择三个点,然后使用公式(2)和公式(3)计算距离。然后使用公式(7)和公式(8)计算距离。不同的是, Q ˉ k \bar \mathcal Q_k Qˉk 中的点时间戳都是 t k + 1 t_{k+1} tk+1。再次使用莱文贝格-马夸特算法进行非线性优化,接着 Q ˉ k \bar \mathcal Q_k Qˉk 被注册到地图。

      为了分布均匀,每次把一次扫描合并到地图后,都使用体素网格滤波器对地图点云下采样。体素网格滤波器使各体素中包含相同数量的点。边缘点和平面点的体素尺寸不同,前者的体素棱长为 5 厘米,后者的体素棱长为 10 厘米。为了限制内存占用,地图大小被限制在以传感器为中心,棱长为 500 米的范围内。
    图 9

    图 9 位姿变换

      蓝色区域代表地图创建算法在每个扫描周期内计算出的一个位姿 T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk)。橘色区域代表里程计以 10 赫兹的频率计算出的雷达在当前扫描过程中的运动。雷达的运动估计包含两个变换,频率也是 10 赫兹。

      位姿变换如图 9。蓝色区域代表地图创建算法在每个扫描周期内计算出的一个位姿 T k − 1 W ( t k ) \boldsymbol T_{k-1}^W(t_k) Tk1W(tk)。橘色区域代表里程计以 10 赫兹的频率计算出的变换 T k L ( t ) \boldsymbol T_k^L(t) TkL(t)。雷达相对地图的位姿包含这两个变换,频率也是 10 赫兹。

    7. 附录


    7.1 数量积与向量积


      数量积又称点积、内积,计算方式为:

    a ⃗ ⋅ b ⃗ = { x 1 x 2 + y 1 y 2 ∣ a ⃗ ∣ ∣ b ⃗ ∣ c o s θ . (8.1) \vec{a} \cdot \vec{b} = \begin{cases} x_1x_2 + y_1y_2 \\ |\vec{a}||\vec{b}|cos\theta \end{cases}. \tag{8.1} a b ={x1x2+y1y2a b cosθ.(8.1)

    如果知道平面的法向量和平面上一点,由第二个公式可以求得平面外一点到平面的距离。

      向量积又称叉积、外积,计算方式为:

    { a ⃗ × b ⃗ = ( a y b z − a z b y ) i ⃗ + ( a z b x − a x b z ) j ⃗ + ( a x b y − a y b x ) k ⃗ ∣ a ⃗ × b ⃗ ∣ = ∣ a ⃗ ∣ ∣ b ⃗ ∣ s i n θ . (8.2) \begin{cases} \vec{a} \times \vec{b} = (a_yb_z - a_zb_y)\vec{i} + (a_zb_x - a_xb_z)\vec{j} + (a_xb_y - a_yb_x)\vec{k} \\ |\vec{a} \times \vec{b}| = |\vec{a}| |\vec{b}| sin\theta \end{cases}. \tag{8.2} {a ×b =(aybzazby)i +(azbxaxbz)j +(axbyaybx)k a ×b =a b sinθ.(8.2)

    第一个公式求得的是一个垂直于向量 a ⃗ \vec{a} a b ⃗ \vec{b} b 所在平面的向量。第二个公式求得的是以 a ⃗ \vec{a} a b ⃗ \vec{b} b 为邻边的平行四边形的面积。

    7.2 点到直线的距离


      我们讨论向量形式的点到直线距离公式。如图 8.1(左),点 i i i 是直线 j l jl jl 外一点, i i i j j j l l l 坐标已知,求点 i i i 到直线 j l jl jl 的距离。
    图 8.1

    图 8.1 向量形式的点到直线距离公式及点到平面距离公式

      作平行四边形 i j m l ijml ijml,则其面积是 ∣ i j ⃗ × i l ⃗ ∣ |\vec {ij} \times \vec {il}| ij ×il 。因为直线 j l jl jl 把四边形分成两个全等的三角形,所以四边形的面积也等于 ∣ O i ⃗ ∣ ⋅ ∣ j l ⃗ ∣ |\vec {Oi}| \cdot |\vec {jl}| Oi jl ,其中 O O O 是过点 i i i 的直线与直线 j l jl jl 的垂足。所以点 i i i 到直线 j l jl jl 的距离

    ∣ O i ⃗ ∣ = ∣ i j ⃗ × i l ⃗ ∣ ∣ j l ⃗ ∣ . (8.3) |\vec {Oi}| = \frac{|\vec {ij} \times \vec {il}|}{|\vec {jl}|}. \tag{8.3} Oi =jl ij ×il .(8.3)

    7.3 点到平面的距离


      我们讨论向量形式的点到平面距离公式。如图 7(右),已知平面上的三个点 j j j l l l m m m 和平面外一点 i i i 的坐标,求点 i i i 到平面的距离。

      由平面上的三点可以求得平面的法向量 n ⃗ = j l ⃗ × j m ⃗ \vec{n} = \vec{jl} \times \vec{jm} n =jl ×jm ,向量 i j ⃗ \vec{ij} ij n ⃗ \vec{n} n 夹角的余弦值 c o s θ = ( i j ⃗ ⋅ n ⃗ ) / ( ∣ i j ⃗ ∣ ∣ n ⃗ ∣ ) cos\theta = (\vec{ij} \cdot \vec{n}) / (|\vec{ij}| |\vec{n}|) cosθ=(ij n )/(ij n )。所以点 i i i 到平面的距离

    ∣ O i ⃗ ∣ = ∣ i j ⃗ ∣ ∣ c o s θ ∣ = ∣ i j ⃗ ∣ ∣ i j ⃗ ⋅ n ⃗ ∣ ∣ i j ⃗ ∣ ∣ n ⃗ ∣ = ∣ i j ⃗ ⋅ n ⃗ ∣ ∣ n ⃗ ∣ = ∣ i j ⃗ ⋅ ( j l ⃗ × j m ⃗ ) ∣ ∣ j l ⃗ × j m ⃗ ∣ = ∣ i j ⃗ ∣ ∣ n ⃗ u n i t ∣ (8.4) \begin{aligned} |\vec{Oi}| = |\vec{ij}| |cos \theta| = |\vec{ij}| \frac{|\vec{ij} \cdot \vec{n}|}{|\vec{ij}| |\vec{n}|} &= \frac{|\vec{ij} \cdot \vec{n}|}{|\vec{n}|} \\ &= \frac{|\vec{ij} \cdot (\vec{jl} \times \vec{jm})|}{|\vec{jl} \times \vec{jm}|} \\ &= |\vec {ij}| |\vec n_{unit}| \end{aligned} \tag{8.4} Oi =ij cosθ=ij ij n ij n =n ij n =jl ×jm ij (jl ×jm )=ij n unit(8.4)

    其中 n ⃗ u n i t \vec n_{unit} n unit 是平面的单位法向量。

    7.4 协方差矩阵的特征值与特征向量


      矩阵 M 3 , n M_{3,n} M3,n n n n 个三维点的坐标组成,它的协方差矩阵 C 3 , 3 C_{3,3} C3,3 是一个三阶方阵。假设 C 3 , 3 C_{3,3} C3,3 的三个特征值从大到小排列为 λ 1 \lambda_1 λ1 λ 2 \lambda_2 λ2 λ 3 \lambda_3 λ3,对应的特征向量分别为 v ⃗ 1 \vec v_1 v 1 v ⃗ 2 \vec v_2 v 2 v ⃗ 3 \vec v_3 v 3。假设 n n n 个三维点组成的空间区域为 Z Z Z
    图 8.2

    图 8.2 三阶协方差矩阵的三个特征向量

       v ⃗ 1 \vec v_1 v 1 Z Z Z 中距离最远的两个点所在直线的方向向量, λ 1 \lambda_1 λ1 是这两个点的距离。 v ⃗ 2 \vec v_2 v 2 Z Z Z 中垂直于 v ⃗ 1 \vec v_1 v 1 方向上距离最远的两个点所在直线的方向向量, λ 2 \lambda_2 λ2 是这两个点的距离。 v ⃗ 3 \vec v_3 v 3 Z Z Z 中垂直于 v ⃗ 1 \vec v_1 v 1 v ⃗ 2 \vec v_2 v 2 方向上距离最远的两个点所在直线的方向向量, λ 3 \lambda_3 λ3 是这两个点的距离。

      所以,假如这 n n n 个点分布在一条直线上, λ 1 \lambda_1 λ1 明显大于 0, λ 2 \lambda_2 λ2 λ 3 \lambda_3 λ3 为 0;假如这 n n n 个点分散在一个平面上, λ 1 \lambda_1 λ1 λ 2 \lambda_2 λ2 明显大于 0, λ 3 \lambda_3 λ3 为 0;假如这 n n n 个点不共面, λ 1 \lambda_1 λ1 λ 2 \lambda_2 λ2 λ 3 \lambda_3 λ3 都明显大于 0。下面使用 numpy 计算 n n n 个三维点组成的矩阵的协方差矩阵及其特征值与特征向量。

    import numpy as np
    import matplotlib.pyplot as plt
    import mpl_toolkits.mplot3d
    
    def show_points(x, y, z, eigenvalues, eigenvectors):
        ax = plt.subplot(projection='3d')
        ax.set_title('3D points')
        ax.scatter(x, y, z, c='gray')
        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.set_zlabel('Z')
        ax.quiver(1, 2, 3, eigenvectors[:, 0][0], eigenvectors[:, 0][1], eigenvectors[:, 0][2], length=0.1 * eigenvalues[0],
                  color='r')
        ax.quiver(1, 2, 3, eigenvectors[:, 1][0], eigenvectors[:, 1][1], eigenvectors[:, 1][2], length=0.1 * eigenvalues[1],
                  color='g')
        ax.quiver(1, 2, 3, eigenvectors[:, 2][0], eigenvectors[:, 2][1], eigenvectors[:, 2][2], length=0.1 * eigenvalues[2],
                  color='b')
        plt.show()
    
    def get_points(points='line'):
        x = np.linspace(start=-30, stop=30, num=5000)
        if points == 'line':
            y = np.linspace(start=-20, stop=20, num=5000)
        else:
            y = np.random.randint(-20, 20, 5000)
        z = 3 - (4 * (x - 1) + 5 * (y - 2)) / 6.0
        return x, y, z
    
    if __name__ == '__main__':
        x, y, z = get_points(points='plane')
        matrix = np.vstack((x, y, z))  # shape=(3, n)
        # 协方差矩阵。
        covariance_matrix = np.cov(matrix)  # shape=(3, 3)
        # 协方差矩阵的特征值与特征向量。
        eigenvalues, eigenvectors = np.linalg.eig(covariance_matrix)
        # 最大的特征值。
        max_eigenvalue_idx = np.argmax(eigenvalues)
        # 最大的特征值对应的特征向量。
        max_eigenvector = eigenvectors[:, max_eigenvalue_idx]
        # 绘图。
        show_points(x, y, z, eigenvalues, eigenvectors)
    

    8. 参考


    1. LOAM 论文 1,CMU,2014。
    2. LOAM 论文 2,sci-hub,2017。
    3. 激光雷达笔记系列,CSDN。
    4. A-LOAM,HKUST-Aerial-Robotics,github。
    5. loam_velodyne,github。
    6. LeGO-LOAM,github。
    7. A-LOAM 建图讲解,博客园。
    展开全文
  • loam论文翻译

    2021-12-12 13:41:40
    目录AbstractI. INTRODUCTIONII .Related workIII. Notation and Task Description(符号和任务描述)IV. System OverviewA. Lidar HardwareB. Software System OverviewV. Lidar OdometryA. Feature Point ...

    Abstract

    We propose a real-time method for odometry and mapping using range measurements from a 2-axis lidar moving in 6-DOF . The problem is hard because the range measurements are received at different times, and errors in motion estimation can cause mis-registration of the resulting point cloud. To date, coherent 3D maps can be built by off-line batch methods, often using loop closure to correct for drift over time. Our method achieves both low-drift and low-computational complexity without the need for high accuracy ranging or inertial measurements. The key idea in obtaining this level of performance is the division of the complex problem of simultaneous localization and mapping, which seeks to optimize a large number of variables simultaneously, by two algorithms. One algorithm performs odometry at a high frequency but low fidelity to estimate velocity of the lidar. Another algorithm runs at a frequency of an order of magnitude lower for fine matching and registration of the point cloud. Combination of the two algorithms allows the method to map in real-time. The method has been evaluated by a large set of experiments as well as on the KITTI odometry benchmark. The results indicate that the method can achieve accuracy at the level of state of the art offline batch methods.
    我们提出了一种在6自由度范围内利用2轴激光雷达进行距离测量实现SLAM的方法。这个问题难点在于 测量的距离信息是在不同的时间接收的(因为激光雷达需要通过旋转的方式采集信息),运动估计中的误差可能会导致最终点云的错误配准。迄今为止,通过off-line batch方法可以构建3D地图,并使用loop closure来校正随时间的漂移。我们实现了一种低漂移和低计算复杂度的方法,无需高精度测距或惯性测量。该方法的核心思路是通过两种算法来实现分别定位和建图的问题,我们的方法可以同时优化大量变量。第一个算法以高频但低精度的性能进行里程计测量,以估计激光雷达的速度。第二个算法以较低数量级的频率运行,用于点云的精细匹配和对齐(registation)(类似于SLAM中的前端和后端)。通过两种算法的结合允许该方法实时建图。该方法已通过大量实验以及KITTI里程计基准进行了评估。结果表明,该方法可以达到off-line batch处理方法的精度水平。
    在这里插入图片描述
    Fig. 1. The method aims at motion estimation and mapping using a moving 2-axis lidar. Since the laser points are received at different times, distortion is present in the point cloud due to motion of the lidar (shown in the left lidar cloud). Our proposed method decomposes the problem by two algorithms running in parallel. An odometry algorithm estimates velocity of the lidar and corrects distortion in the point cloud, then, a mapping algorithm matches and registers the point cloud to create a map. Combination of the two algorithms ensures feasibility of the problem to be solved in real-time.
    图1所示。该方法利用移动的2轴激光雷达进行运动估计和建图。由于激光雷达的运动,激光点是在不同的时间接收到的,在点云中会出现失真(如图左激光雷达云所示)。我们提出的方法通过两个并行的算法来分解问题。一种odometry算法估计激光雷达的速度并校正点云中的失真,然后一种mapping算法匹配并对齐(registers)点云来创建地图。两种算法的结合保证了所要解决问题的实时性。

    I. INTRODUCTION

    3D mapping remains a popular technology [1]–[3]. Mapping with lidars is common as lidars can provide high frequency range measurements where errors are relatively constant irrespective of the distances measured. In the case that the only motion of the lidar is to rotate a laser beam, registration of the point cloud is simple. However, if the lidar itself is moving as in many applications of interest, accurate mapping requires knowledge of the lidar pose during continuous laser ranging. One common way to solve the problem is using independent position estimation (e.g. by a GPS/INS) to register the laser points into a fixed coordinate system. Another set of methods use odometry measurements such as from wheel encoders or visual odometry systems [4], [5] to register the laser points. Since odometry integrates small incremental motions over time, it is bound to drift and much attention is devoted to reduction of the drift (e.g. using loop closure).
    3D建图仍然是一种主流的技术[1]–[3]。使用激光雷达进行建图是非常 常见的,因为激光雷达可以提供高频的距离测量信息,无论测量的距离如何,误差都相对恒定。当激光雷达的唯一运动是旋转激光束时,其点云的配准非常简单。然而,如果激光雷达(多自由度运动)in many applications of interest中运动,则精确的测绘需要在连续激光测距期间了解激光雷达的姿态变换。解决该问题的一种常见方法是使用独立的位置估计(例如通过GPS/INS)将激光点对齐到固定的坐标系中。另一组方法使用里程计测量,例如从车轮编码器或视觉里程计系统[4],[5]记录激光点。由于里程计集成了随时间推移的小增量运动,因此必然会发生漂移,并且会将大量注意力放在减小漂移上(例如,使用loop closure)。

    Here we consider the case of creating maps with lowdrift odometry using a 2-axis lidar moving in 6-DOF. A key advantage of using a lidar is its insensitivity to ambient lighting and optical texture in the scene. Recent developments in lidars have reduced their size and weight. The lidars can be held by a person who traverses an environment [6], or even attached to a micro aerial vehicle [7]. Since our method is intended to push issues related to minimizing drift in odometry estimation, it currently does not involve loop closure.
    在这里,我们考虑使用在6自由度运动的2轴激光雷达进行地图创建的场景。使用激光雷达的一个关键优势是它对场景中的环境光明和光学纹理不敏感。激光雷达的最新发展缩小了它们的尺寸和重量。激光雷达可以由人手持[6],甚至可以连接到微型飞行器[7]。由于我们的方法旨在推动里程计估计中的最小化dift的问题,因此目前不涉及loop closure。

    The method achieves both low-drift and low-computational complexity without the need for high accuracy ranging or inertial measurements. The key idea in obtaining this level of performance is the division of the typically complex problem of simultaneous localization and mapping (SLAM) [8], which seeks to optimize a large number of variables simultaneously, by two algorithms. One algorithm performs odometry at a high frequency but low fidelity to estimate velocity of the lidar. Another algorithm runs at a frequency of an order of magnitude lower for fine matching and registration of the point cloud. Although unnecessary, if an IMU is available, a motion prior can be provided to help account for high frequency motion. Specifically, both algorithms extract feature points located on sharp edges and planar surfaces, and match the feature points to edge line segments and planar surface patches, respectively. In the odometry algorithm, correspondences of the feature points are found by ensuring fast computation. In the mapping algorithm, the correspondences are determined by examining geometric distributions of local point clusters, through the associated eigenvalues and eigenvectors.
    该方法实现了低漂移和低计算复杂度,无需高精度测距或惯性测量。获得这一性能水平的关键思想是通过两种算法将SLAM[8]进行划分,该问题寻求(seek)同时优化大量变量。一种算法以高频但低精度的性能进行里程测量,以估计激光雷达的速度。另一种算法以较低数量级的频率运行,用于点云的精细匹配和对齐(registration)。尽管非必要,但如果IMU可用,则可以用于提供运动先验以帮助计算高频运动。具体来说,这两种算法都提取位于锐边和平面上(sharp edges and planar surfaces)的特征点,并分别将特征点与边线段和平面匹配。在里程计算法中,通过足够快速的计算来找到特征点的对应关系。在mapping算法中,通过相关的特征值和特征向量检查局部点簇(local point clusters) 的几何分布来确定对应关系。

    By decomposing the original problem, an easier problem is solved first as online motion estimation. After which, mapping is conducted as batch optimization (similar to iterative closest point (ICP) methods [9]) to produce high-precision motion estimates and maps. The parallel algorithm structure ensures feasibility of the problem to be solved in real-time. Further, since the motion estimation is conducted at a higher frequency, the mapping is given plenty of time to enforce accuracy. When running at a lower frequency, the mapping algorithm is able to incorporate a large number of feature points and use sufficiently many iterations for convergence.
    通过对原始问题进行分解,首先解决了一个简单的问题,即在线运动估计。之后,mapping作为批量优化(类似于迭代最近点(ICP)方法[9])进行,以生成高精度运动估计和maps。并行算法结构保证了问题实时求解的可行性。此外,由于运动估计是在更高的频率下进行的,因此给了mapping足够的时间来提高精度。当以较低的频率运行时,mapping算法能够包含大量的特征点,并使用足够多的迭代进行收敛。

    II .Related work

    III. Notation and Task Description(符号和任务描述)

    The problem addressed in this paper is to perform ego-motion estimation with point cloud perceived by a 3D lidar, and build a map for the traversed environment. We assume that the lidar is pre-calibrated. We also assume that the angular and linear velocities of the lidar are smooth and continuous over time, without abrupt changes. The second assumption will be released by usage of an IMU, in Section VII-B.
    本文解决的问题是利用三维激光雷达感知到的点云进行自运动估计,并建立遍历环境的地图。我们假设激光雷达是预先校准的。我们还假设激光雷达的角速度和线速度是平滑和连续的,没有突变。第二项假设将在Section VII-B中通过使用IMU提出。

    As a convention in this paper, we use right uppercase superscription to indicate the coordinate systems. We define a sweep as the lidar completes one time of scan coverage. We use right subscription k , k ∈ Z + k, k∈Z^+ k,kZ+to indicate the sweeps, and P k P_k Pk to indicate the point cloud perceived during sweep k. Let us define two coordinate systems as follows.
    我们使用右上角的大写上标来表示坐标系。我们将扫描定义为激光雷达完成一次扫描覆盖 。我们使用右下角的小写下标 k , k ∈ Z + k,k∈Z+ k,kZ+表示扫描, P k P_k Pk 表示第 k k k次扫描期间感知到的点云。让我们定义如下两个坐标系。

    •Lidar coordinate system L {L} L is a 3D coordinate system with its origin at the geometric center of the lidar. The x-axis is pointing to the left, the y-axis is pointing upward, and the z-axis is pointing forward. The coordinates of a point i , i ∈ P k i,i∈ P_k i,iPk, in L k {L_k} Lk are denoted as X ( k , i ) L X^L_{(k,i)} X(k,i)L.
    •World coordinate system W {W} W is a 3D coordinate system coinciding with L {L} L at the initial position. The coordinates of a point i , i ∈ P k i,i∈ P_k i,iPk, in W k {W_k} Wkare X ( k , i ) W X^W _{(k,i)} X(k,i)W
    •激光雷达坐标系 L {L} L是一个三维坐标系,其原点位于激光雷达的几何中心。x轴指向左侧,y轴指向上方,z轴指向前方。激光点 i , i ∈ P k i,i∈ P_k iiPk L k {L_k} Lk坐标系下表示为 X ( k , i ) L X^L_{(k,i)} X(k,i)L
    •世界坐标系 W {W} W是一个3D坐标系,在初始位置与 L {L} L重合。激光点 i , i ∈ P k i,i∈ P_k iiPk W k {W_k} Wk 坐标系下表示为 X ( k , i ) W X^W _{(k,i)} X(k,i)W

    With assumptions and notations made, our lidar odometry and mapping problem can be defined as
    通过假设和标记,我们的激光雷达里程测量和mapping问题可以定义为

    Problem: Given a sequence of lidar cloud P k , k ∈ Z + P_k, k∈Z^+ Pk,kZ+, compute the ego-motion of the lidar during each sweepk, and build a map with P k P_k Pk for the traversed environment.
    问题: 给定一个激光点云序列 P k , k ∈ Z + P_k, k∈Z^+ Pk,kZ+,计算每次扫描时激光雷达的自运动,用 P k P_k Pk构建遍历环境的地图。

    IV. System Overview

    A. Lidar Hardware

    The study of this paper is validated on, but not limited to a custom built 3D lidar based on a Hokuyo UTM-30LX laser scanner. Through the paper, we will use data collected from the lidar to illustrate the method. The laser scanner has a field of view of 180◦with 0.25◦ resolution and 40 lines/sec scan rate. The laser scanner is connected to a motor, which is controlled to rotate at an angular speed of180◦/s between −90◦ and 90◦ with the horizontal orientation of the laser scanner as zero. With this particular unit, a sweep is a rotation from −90◦ to 90◦ or in the inverse direction (lasting for 1s). Here, note that for a continuous spinning lidar, a sweep is simply a semispherical rotation. An onboard encoder measures the motor rotation angle with a resolution of 0.25◦, with which, the laser points are projected into the lidar coordinates, L {L} L.
    本文的研究在基于Hokuyo UTM-30LX激光扫描仪的定制3D激光雷达上得到验证,但不限于此。通过本文,我们将使用从激光雷达收集的数据来说明该方法。激光扫描仪的视场为180度,分辨率为0.25度,扫描速率为40行/秒。激光扫描仪与电机相连,电机被控制以180度/s (−90◦到90◦)的角速度旋转,激光扫描仪的水平角度为零。对于此特定单元,扫描是从−90◦到90◦或相反方向进行的(持续1s)。请注意,对于连续旋转的激光雷达,扫描只是一个半球形旋转。车载编码器以0.25度的分辨率测量电机旋转角度, 激光点被投影到激光雷达坐标 L {L} L

    B. Software System Overview

    Fig. 3 shows a diagram of the software system. Let P ^ \hat{P} P^ be the points received in a laser scan. During each sweep, P ^ \hat{P} P^ is registered in{L}. The combined point cloud during sweep k forms P k P_k Pk. Then, P k P_k Pk is processed in two algorithms. The lidar odometry takes the point cloud and computes the motion of the lidar between two consecutive sweeps. The estimated motion is used to correct distortion in P k P_k Pk. The algorithm runs at a frequency around 10Hz. The outputs are further processed by lidar mapping, which matches and registers the undistorted cloud onto a map at a frequency of 1Hz. Finally, the pose transforms published by the two algorithms are integrated to generate a transform output around 10Hz, regarding the lidar pose with respect to the map. Section V and VI present the blocks in the software diagram in detail.
    图3为软件系统框图。定义 P ^ \hat{P} P^ 为激光扫描收到的点。在每次扫描期间, P ^ \hat{P} P^ 在 激光雷达坐标系 L {L} L 中对齐。第k次扫描期间的点云组合形成 P k P_k Pk。然后,用两种算法处理 P k P_k Pk。激光雷达里程计通过点云,计算激光雷达在两次连续扫描之间的运动。估计的运动被用来校正在 P k P_k Pk中的畸变。该算法的运行频率约为10Hz。输出的结果被激光雷达mapping算法施行进一步的处理,它以1Hz的频率将去畸变的点云匹配并对齐到地图上。最后,将两种算法发布的姿态变换进行集成,得到激光雷达姿态相对于地图的约10Hz的变换输出。Section V and VI 对软件框图中的模块进行了详细介绍。
    在这里插入图片描述

    V. Lidar Odometry

    A. Feature Point Extraction

    We start with extraction of feature points from the lidar cloud, P k P_k Pk. The lidar presented in Fig. 2 naturally generates unevenly distributed points inPk. The returns from the laser scanner has a resolution of0.25◦within a scan. These points are located on a scan plane. However, as the laser scanner rotates at an angular speed of 180◦/s and generates scans at 40Hz, the resolution in the perpendicular direction to the scan planes is 180◦/40 = 4.5◦. Considering this fact, the feature points are extracted from P k P_k Pk using only information from individual scans, with co-planar geometric relationship.
    我们首先从激光雷达云 P k P_k Pk中提取特征点。图2所示的激光雷达在 P k P_k Pk中会产生分布不均匀的点。激光扫描仪返回的分辨率为0.25◦。这些点位于一个扫描平面上。但是,由于激光扫描仪以180◦/s的角速度旋转并以40Hz的频率生成扫描,垂直于扫描平面的分辨率为180◦/40 = 4.5◦考虑到这一事实,从 P k P_k Pk中提取的特征点,仅使用单个扫描的信息,具有共面几何关系.
    在这里插入图片描述
    Fig. 2. The 3D lidar used in this study consists of a Hokuyo laser scanner driven by a motor for rotational motion, and an encoder that measures the rotation angle. The laser scanner has a field of view of180◦with a resolution of0.25◦. The scan rate is 40 lines/sec. The motor is controlled to rotate from −90◦to90◦with the horizontal orientation of the laser scanner as zero.
    图2所示。本研究使用的3D激光雷达由一个旋转运动电机驱动的Hokuyo激光扫描仪和一个测量旋转角度的编码器组成。激光扫描仪的视场为180◦分辨率为0.25◦。扫描速度为40行/秒。电机被控制为从−90◦到90◦的旋转,且水平方向上角度为零。

    We select feature points that are on sharp edges and planar surface patches. Let i i i be a point in P k P_k Pk ,i∈ P k P_k Pk, and let S S S be the set of consecutive points of i i i returned by the laser scanner in the same scan. Since the laser scanner generates point returns in CW or CCW order, S S S contains half of its points on each side of i i i and 0.25◦ intervals between two points. Define a term to evaluate the smoothness of the local surface,
    我们选择位于锐边和平面 patches上的特征点(选择面点和角点作为特征点)。定义 i i i为激光雷达点云 P k P_k Pk中的一个点,i∈ P k P_k Pk,设 S S S为激光扫描仪在同一次扫描中返回的 i i i的连续点集。由于激光扫描仪以CW或CCW顺序(顺时针或逆时针)返回生成点, S S S i i i 和 0.25◦间隔的每一侧包含其点的一半。定义一个 term 以评估局部曲面的平滑度(曲率),
    在这里插入图片描述

    j j j 就是在 i i i 周围的点
    曲率 = (当前点到其附近点的距离差 / 当前点的值 ) 的总和再求平均 = 平均的距离差

    The points in a scan are sorted based on the c c c values, then feature points are selected with the maximum c c c’s, namely, edge points, and the minimumc’s, namely planar points. To evenly distribute the feature points within the environment, we separate a scan into four identical subregions. Each subregion can provide maximally 2 edge points and 4 planar points. A point i i i can be selected as an edge or a planar point only if its c c c value is larger or smaller than a threshold, and the number of selected points does not exceed the maximum.
    扫描中的点根据 c c c 值(曲率)进行排序,然后选择具有最大 c c c 值(即边缘点)和最小 c c c 值(即平面点)的特征点。为了在环境中均匀分布特征点,我们将扫描分为四个相同的子区域(代码中是6等分)。每个子区域最多可提供2个边点和4个平面点。仅当点 i i i c c c 值大于或小于阈值且所选点的数量不超过最大值时,才可以将点 i i i 选择为边或平面点。

    While selecting feature points, we want to avoid points whose surrounded points are selected, or points on local planar surfaces that are roughly parallel to the laser beams (pointB in Fig. 4(a)). These points are usually considered as unreliable. Also, we want to avoid points that are on boundary of occluded regions [23]. An example is shown in Fig. 4(b). PointA is an edge point in the lidar cloud because its connected surface (the dotted line segment) is blocked by another object. However, if the lidar moves to another point of view, the occluded region can change and become observable. To avoid the aforementioned points to be selected, we find again the set of points S S S. A point i i i can be selected only if S S S does not form a surface patch that is roughly parallel to the laser beam, and there is no point in S S S that is disconnected from i i i by a gap in the direction of the laser beam and is at the same time closer to the lidar then point i i i (e.g. point B B B cin Fig. 4(b))
    在选择特征点时,我们希望避免选择其周围点已经被选择的点,或局部平面上与激光束大致平行的点(图4(a)中的点B)。这些点通常被认为是不可靠的。此外,我们还希望避免位于遮挡区域边界上的点[23]。图4(b)中示出了一个示例。Point A是激光雷达云中的一个边缘点,因为其连接表面(虚线段)被另一个对象阻挡。但是,如果激光雷达移动到另一个视角,则遮挡区域可能会发生变化并变得可见。为了避免选择上述点,我们再次找到点集 S S S。仅当 S S S未形成大致平行B于激光束的 surface patch,且 S S S中没有任何点通过激光束方向上的间隙与 i i i断开,同时更靠近激光雷达(如图4(b)中的点 B)时,才可选择点 i i i
    在这里插入图片描述
    Fig. 4. (a) The solid line segments represent local surface patches. PointA is on a surface patch that has an angle to the laser beam (the dotted orange line segments). PointBis on a surface patch that is roughly parallel to the laser beam. We treat B as a unreliable laser return and do not select it as a feature point. (b) The solid line segments are observable objects to the laser. Point A is on the boundary of an occluded region (the dotted orange line segment), and can be detected as an edge point. However, if viewed from a different angle, the occluded region can change and become observable. We do not treatAas a salient edge point or select it as a feature point.
    图4所示。(a)实线段表示local surface patches。Point A在一个surface patch上,它与激光束(橙色点线段)有一定的角度。point B位于一个与激光束大致平行的surface patch上。我们将B视为不可靠的激光回波,不选择它作为特征点。(b)实线段是激光的可观测对象。point A位于被遮挡区域(橙色点线段)的边界上,可以被检测为边缘点。然而,如果从不同的角度看,被遮挡的区域可以改变并变得可见。我们不把a作为一个显著的边缘点,也不把它作为一个特征点。

    In summary, the feature points are selected as edge points starting from the maximum c c c value, and planar points starting from the minimum c c c value, and if a point is selected,
    总之,特征点被选择为从最大 c c c值开始的边点和从最小 c c c值开始的平面点,如果一个点要被选择,

    • The number of selected edge points or planar points cannot exceed the maximum of the subregion, and
    • None of its surrounding point is already selected, and
    • It cannot be on a surface patch that is roughly parallel to the laser beam, or on boundary of an occluded region.
    • 选择的边缘点或平面点的数量不能超过子区域的最大值,并且
    • 它的周围点都没有被选择,并且
    • 它不能在大致平行于激光束的surface patch上,或位于封闭区域的边界。

    An example of extracted feature points from a corridor scene is shown in Fig. 5. The edge points and planar points are labeled in yellow and red colors, respectively.
    从走廊场景中提取特征点的实例如图5所示。边缘点和平面点分别用黄色和红色标注。
    在这里插入图片描述
    Fig. 5. An example of extracted edge points (yellow) and planar points (red) from lidar cloud taken in a corridor. Meanwhile, the lidar moves toward the wall on the left side of the figure at a speed of 0.5m/s, this results in motion distortion on the wall.
    图5所示。从一条走廊的激光雷达云中提取边缘点(黄色)和平面点(红色)的例子。同时,激光雷达以0.5m/s的速度向图像左侧墙体移动,导致墙体运动变形。

    B. Finding Feature Point Correspondence

    The odometry algorithm estimates motion of the lidar within a sweep. Let t k t_k tk be the starting time of a sweep k k k. At the end of each sweep, the point cloud perceived during the sweep, P k P_k Pk, is reprojected to time stamp t k + 1 t_{k+1} tk+1, illustrated in Fig. 6. We denote the reprojected point cloud as P ‾ k \overline{P}_k Pk. During the next sweep k + 1 k+1 k+1, P ‾ k \overline{P}_k Pk is used together with the newly received point cloud, P k + 1 P_{k+1} Pk+1, to estimate the motion of the lidar.
    里程计算法估计激光雷达在扫描范围内的运动。让 t k t_k tk作为扫描 k k k的开始时间。在每次扫描结束时,扫描期间感知到的点云 P k P_k Pk被重新投影到时间戳 t k + 1 t_{k+1} tk+1,如图6所示。我们将重新投影的点云表示为 P ‾ k \overline{P}_k Pk。在下一次扫描 k + 1 k+1 k+1期间, P ‾ k \overline{P}_k Pk与新接收的点云 P k + 1 P_{k+1} Pk+1一起使用,以估计激光雷达的运动。
    在这里插入图片描述
    Fig. 6. Reprojecting point cloud to the end of a sweep. The blue colored line segment represents the point cloud perceived during sweep k, P k P_k Pk. At the end of sweep k, P k P_k Pk is reprojected to time stamp t k + 1 t_{k+1} tk+1 to obtain P ‾ k \overline{P}_k Pk (the green colored line segment). Then, during sweep k+ 1, P ‾ k \overline{P}_k Pk and the newly perceived point cloud P k + 1 P_{k+1} Pk+1 (the orange colored line segment) are used together to estimate the lidar motion.
    图6, 将点云重新投影到扫描的末尾。蓝色线段表示第k次扫描期间感知到的点云 P k P_k Pk。在第k次扫描结束时,将 P k P_k Pk重新投影到时间戳 t k + 1 t_{k+1} tk+1以获得 P ‾ k \overline{P}_k Pk(绿色线段)。然后,在第k+1次扫描期间, P ‾ k \overline{P}_k Pk和新感知的点云 P k + 1 P_{k+1} Pk+1(橙色线段)一起用于估计激光雷达运动。

    Let us assume that both P ‾ k \overline{\mathcal{P}}_{k} Pk and P k + 1 \mathcal{P}_{k+1} Pk+1 are available for now, and start with finding correspondences between the two lidar clouds. With P k + 1 \mathcal{P}_{k+1} Pk+1, we find edge points and planar points from the lidar cloud using the methodology discussed in the last section. Let E k + 1 \mathcal{E}_{k+1} Ek+1 and H k + 1 \mathcal{H}_{k+1} Hk+1 be the sets of edge points and planar points, respectively. We will find edge lines from P ‾ k \overline{\mathcal{P}}_{k} Pk as the correspondences for the points in E k + 1 \mathcal{E}_{k+1} Ek+1, and planar patches as the correspondences for those in H k + 1 \mathcal{H}_{k+1} Hk+1.
    让我们假设 P ‾ K \overline{\mathcal{P}}_{K} PK P K + 1 \mathcal{P}_{K+1} PK+1现在都可用,并从查找两个激光雷达云之间的对应关系开始。对于 P k + 1 \mathcal{P}_{k+1} Pk+1,我们使用上一节讨论的方法从激光雷达云中查找边缘点和平面点。设 E K + 1 \mathcal{E}_{K+1} EK+1 H K + 1 \mathcal{H}_{K+1} HK+1分别为边点集和平面点集。我们将从 P ‾ k \overline{\mathcal{P}}_{k} Pk 中找到边线作为 E k + 1 \mathcal{E}_{k+1} Ek+1中的点的对应,找到planar patches作为 H k + 1 \mathcal{H}_{k+1} Hk+1中的点的对应。

    Note that at the beginning of sweep k + 1 k+1 k+1, P k + 1 \mathcal{P}_{k+1} Pk+1 is an empty set, which grows during the course of the sweep as more points are received. The lidar odometry recursively estimates the 6DOF motion during the sweep, and gradually includes more points as P k + 1 \mathcal{P}_{k+1} Pk+1 increases. At each iteration, E k + 1 \mathcal{E}_{k+1} Ek+1 and H k + 1 \mathcal{H}_{k+1} Hk+1 are reprojected to the beginning of the sweep using the currently estimated transform. Let E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1 and H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1 be the reprojected point sets. For each point in E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1 and H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1, we are going to find the closest neighbor point in P ‾ k . \overline{\mathcal{P}}_{k} . Pk. Here, P ‾ k \overline{\mathcal{P}}_{k} Pk is stored in a 3D KD-tree [24] for fast index.
    请注意,在第 k + 1 k+1 k+1次扫描开始时, P k + 1 \mathcal{P}_{k+1} Pk+1是一个空集,在扫描过程中随着接收到更多点而增长。激光雷达里程计递归估计扫描期间的6自由度运动,并随着 P k + 1 \mathcal{P}_{k+1} Pk+1的增加逐渐包括更多点。在每次迭代中,使用当前估计的变换将 E k + 1 \mathcal{E}_{k+1} Ek+1 H k + 1 \mathcal{H}_{k+1} Hk+1重新投影到扫描的开始。设 E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1 H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1为重投影点集。对于 E ~ k + 1 \tilde{\mathcal{E}}{k+1} E~k+1 H ~ k + 1 \tilde{\mathcal{H}}{k+1} H~k+1中的每个点,我们将在 P ‾ k \overline{\mathcal{P}}{k} Pk中找到最近的邻居点,我们将 P ‾ k \overline{\mathcal{P}}{k} Pk存储在3D KD树[24]中,用于快速索引。

    Fig. 7(a) represents the procedure of finding an edge line as the correspondence of an edge point. Let i i i be a point in E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1,i∈ E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1. The edge line is represented by two points. Let j j j be the closest neighbor of i i i in P ‾ k \overline{\mathcal{P}}{k} Pk,j ∈ P ‾ k \overline{\mathcal{P}}{k} Pk, and let l l l be the closest neighbor of i i i in the two consecutive scans to the scan of j j j. ( j , l ) (j, l) (j,l) forms the correspondence of i i i. Then, to verify both j j j and l l l are edge points, we check the smoothness of the local surface based on (1). Here, we particularly require that j j j and l l l are from different scans considering that a single scan cannot contain more than one points from the same edge line. There is only one exception where the edge line is on the scan plane. If so, however, the edge line is degenerated and appears as a straight line on the scan plane, and feature points on the edge line should not be extracted in the first place.
    图7(a)表示寻找一个边缘点的对应边缘线的过程。设 i i i E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1中的一个点,i∈ E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1。边线由两点表示。定义 j j j 表示 P ‾ k \overline{\mathcal{P}}_{k} Pk i i i的最近邻居,j∈ P ‾ k \overline{\mathcal{P}}_{k} Pk,并定义 l l l i i i 在两次连续扫描中与 j j j 的扫描最近的邻居。 ( j , l ) (j, l) (j,l)形成 i i i的对应关系。然后,为了验证 j j j l l l都是边点,我们基于(1)检查局部曲面的平滑度。这里,我们特别要求 j j j l l l来自不同的扫描,因为单个扫描不能包含来自同一边缘线的多个点(保证线特征与扫描线不是同一条线)。只有一个例外,即边缘线位于扫描平面上。但是,如果是这样,则边缘线将退化并在扫描平面上显示为一条直线,并且不应首先提取边缘线上的特征点。

    在这里插入图片描述

    在这里插入图片描述
    Fig. 7. Finding an edge line as the correspondence for an edge point in E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1 (a), and a planar patch as the correspondence for a planar point in H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1 (b). In both (a) and (b), j j j is the closest point to the feature point, found in P k \mathcal{P}_{k} Pk. The orange colored lines represent the same scan of j j j, and the blue colored lines are the two consecutive scans. To find the edge line correspondence in (a), we find another point, l l l, on the blue colored lines, and the correspondence is represented as ( j , l ) (j, l) (j,l). To find the planar patch correspondence in (b), we find another two points, l l l and m m m, on the orange and blue colored lines, respectively. The correspondence is ( j , l , m ) (j, l, m) (j,l,m).
    图7。 E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1(a)表示如何查找对应的线点, H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1(b)表示如何查找对应的面点。在(a)和(b)中, j j j是距离特征点最近的点,位于 P k \mathcal{P} _{k} Pk中。橙色线条表示 j j j的同一次扫描,蓝色线条表示两次连续扫描。为了找到(a)中的边线对应关系,我们在蓝色线上找到另一个点, l l l,对应关系表示为 ( j , l ) (j,l) jl。为了找到(b)中的平面面片对应关系,我们在橙色和蓝色线上分别找到另外两个点, l l l m m m。对应关系为 ( j , l , m ) (j,l,m) jlm

    Fig. 7(b) shows the procedure of finding a planar patch as the correspondence of a planar point. Let i i i be a point in H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1, i ∈ H ~ k + 1 i \in \tilde{\mathcal{H}}_{k+1} iH~k+1. The planar patch is represented by three points. Similar to the last paragraph, we find the closest neighbor of i i i in P ‾ k \overline{\mathcal{P}}_{k} Pk, denoted as j j j. Then, we find another two points, l l l and m m m, as the closest neighbors of i i i, one in the same scan of j j j, and the other in the two consecutive scans to the scan of j j j. This guarantees that the three points are non-collinear. To verify that j , l j, l j,l, and m m m are all planar points, we check again the smoothness of the local surface based on (1).
    图7(b)示出了寻找作为平面点对应的平面面片的过程。设 i i i H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1中的一个点, i ∈ H ~ k + 1 i \in \tilde{\mathcal{H}}_{k+1} iH~k+1。平面面片由三个点表示。与上一段类似,我们在 P ‾ k \overline{\mathcal{P}}{k} Pk中找到 i i i的最近邻,表示为 j j j。然后,我们发现另外两个点, l l l m m m i i i的最近邻点,一个在 j j j的相同扫描中,另一个在 j j j的两个连续扫描中。这保证了三个点不共线。为了验证 j 、 l j、l jl m m m都是平面点,我们根据(1)再次检查局部曲面的平滑度。

    j j j l l l也不在同一条线上的话,会存在三个点都在竖直方向山共线的风险

    With the correspondences of the feature points found, now we derive expressions to compute the distance from a feature point to its correspondence. We will recover the lidar motion by minimizing the overall distances of the feature points in the next section. We start with edge points. For a point i ∈ E ~ k + 1 i \in \tilde{\mathcal{E}}_{k+1} iE~k+1, if ( j , l ) (j, l) (j,l) is the corresponding edge line, j , l ∈ P ‾ k j, l \in \overline{\mathcal{P}}_{k} j,lPk, the point to line distance can be computed as
    根据找到的特征点的对应关系,现在我们导出表达式来计算从特征点到其对应关系的距离。在下一节中,我们将通过最小化特征点的总距离来恢复激光雷达运动。我们从边缘点开始。对于点 i ∈ E ~ k + 1 i\in\tilde{\mathcal{E}}_{k+1} iE~k+1,如果 ( j , l ) (j,l) jl是对应的边线, j , l ∈ P ‾ k j,l\in\overline{\mathcal{P}}_{k} jlPk,则点到线的距离可以计算为

    将重新投影的点云表示为 P ‾ k \overline{P}_k Pk(k时刻的点云投影到k+1时刻)
    使用当前估计的变换将 E k + 1 \mathcal{E}_{k+1} Ek+1 H k + 1 \mathcal{H}_{k+1} Hk+1重新投影到扫描的开始。 X ~ ( k + 1 , i ) L \tilde{\boldsymbol{X}}_{(k+1, i)}^{L} X~(k+1,i)L表示为k+1时刻的点云重新投影到k时刻

    在这里插入图片描述
    在这里插入图片描述

    where X ~ ( k + 1 , i ) L , X ‾ ( k , j ) L \tilde{\boldsymbol{X}}_{(k+1, i)}^{L}, \overline{\boldsymbol{X}}_{(k, j)}^{L} X~(k+1,i)L,X(k,j)L, and X ‾ ( k , l ) L \overline{\boldsymbol{X}}_{(k, l)}^{L} X(k,l)L are the coordinates of points i , j i, j i,j, and l l l in { L } \{L\} {L}, respectively. Then, for a point i ∈ H ~ k + 1 i \in \tilde{\mathcal{H}}_{k+1} iH~k+1, if ( j , l , m ) (j, l, m) (j,l,m) is the corresponding planar patch, j , l , m ∈ P ‾ k j, l, m \in \overline{\mathcal{P}}_{k} j,l,mPk, the point to plane distance is
    其中 X ~ ( k + 1 , i ) L , X ‾ ( k , j ) L \tilde{\boldsymbol{X}}_{(k+1, i)}^{L}, \overline{\boldsymbol{X}}_{(k, j)}^{L} X~(k+1,i)L,X(k,j)L, 和 X ‾ ( k , l ) L \overline{\boldsymbol{X}}_{(k, l)}^{L} X(k,l)L 分别是 L {L} L中点 i , j i,j ij l l l的坐标。然后,对于点 i ∈ H ~ k + 1 i\in\tilde{\mathcal{H}}_{k+1} iH~k+1,如果 ( j , l , m ) (j,l,m) jlm是对应的平面面片, j , l , m ∈ P ‾ k j,l,m\in\overline{\mathcal{P}}_{k} jlmPk,则点到平面的距离为


    在这里插入图片描述

    求取点到平面的距离最小相当于让向量ij到向量jn的投影的模长最小

    where X ‾ ( k , m ) L \overline{\boldsymbol{X}}_{(k, m)}^{L} X(k,m)L is the coordinates of point m m m in { L } \{L\} {L}.
    其中 X ‾ ( k , m ) L \overline{\boldsymbol{X}}_{(k, m)}^{L} X(k,m)L 是点 m m m { L } \{L\} {L}中的坐标

    C. Motion Estimation

    The lidar motion is modeled with constant angular and linear velocities during a sweep. This allows us to linear interpolate the pose transform within a sweep for the points that are received at different times. Let t t t be the current time stamp, and recall that t k + 1 t_{k+1} tk+1 is the starting time of sweep k + 1 k+1 k+1. Let T k + 1 L \boldsymbol{T}_{k+1}^{L} Tk+1L be the lidar pose transform between [ t k + 1 , t ] \left[t_{k+1}, t\right] [tk+1,t]. T k + 1 L T_{k+1}^{L} Tk+1L contains rigid motion of the lidar in 6-DOF, T k + 1 L = T_{k+1}^{L}= Tk+1L= [ t x , t y , t z , θ x , θ y , θ z ] T \left[t_{x}, t_{y}, t_{z}, \theta_{x}, \theta_{y}, \theta_{z}\right]^{T} [tx,ty,tz,θx,θy,θz]T, where t x , t y t_{x}, t_{y} tx,ty, and t z t_{z} tz are translations along the x − , y − x-, y- x,y, and z z z - axes of { L } \{L\} {L}, respectively, and θ x \theta_{x} θx, θ y \theta_{y} θy, and θ z \theta_{z} θz are rotation angles, following the right-hand rule. Given a point i , i ∈ P k + 1 i, i \in \mathcal{P}_{k+1} i,iPk+1, let t i t_{i} ti be its time stamp, and let T ( k + 1 , i ) L \boldsymbol{T}_{(k+1, i)}^{L} T(k+1,i)L be the pose transform between [ t k + 1 , t i ] . T ( k + 1 , i ) L \left[t_{k+1}, t_{i}\right] . \boldsymbol{T}_{(k+1, i)}^{L} [tk+1,ti].T(k+1,i)L can be computed by linear interpolation of T k + 1 L T_{k+1}^{L} Tk+1L,
    激光雷达运动在扫描过程中以恒定的角速度和线速度进行建模。这允许我们在扫描中对在不同时间接收到的点进行线性插值计算姿势变换。让 t t t作为当前时间戳,并记住 t k + 1 t_{k+1} tk+1是第 k + 1 k+1 k+1次扫描的开始时间。设 T k + 1 L \boldsymbol{T}_{k+1}^{L} Tk+1L [ T k + 1 , T ] \left[T_{k+1},T\right] [Tk+1T]之间的激光雷达姿态变换。 T k + 1 L T_{k+1}^{L} Tk+1L包含激光雷达在6-DOF中的刚体运动, T k + 1 L = T_{k+1}^{L}= Tk+1L= [ t x , t y , t z , θ x , θ y , θ z ] T \left[t_{x}, t_{y}, t_{z}, \theta_{x}, \theta_{y}, \theta_{z}\right]^{T} [tx,ty,tz,θx,θy,θz]T,其中 t x , t y t_{x}, t_{y} tx,ty, 和 t z t_{z} tz分别表示为沿着{L}的x,y和z轴的平移, θ x \theta_{x} θx, θ y \theta_{y} θy, and θ z \theta_{z} θz表示为绕轴的旋转角度,遵循右手规则。给定一个点 i , i ∈ P k + 1 i,i\in\mathcal{P}_{k+1} iiPk+1,让 t i t_{i} ti作为它的时间戳,让 T ( k + 1 , i ) L \boldsymbol{T}_{(k+1, i)}^{L} T(k+1,i)L作为 [ t k + 1 , t i ] \left[t_{k+1}, t_{i}\right] [tk+1,ti]之间的姿势变换。 T ( k + 1 , i ) L \boldsymbol{T}_{(k+1, i)}^{L} T(k+1,i)L可以通过 T k + 1 L T_{k+1}^{L} Tk+1L的线性插值来计算,
    在这里插入图片描述

    Recall that E k + 1 \mathcal{E}_{k+1} Ek+1 and H k + 1 \mathcal{H}_{k+1} Hk+1 are the sets of edge points and planar points extracted from P k + 1 \mathcal{P}_{k+1} Pk+1, and E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1 and H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1 are the sets of points reprojected to the beginning of the sweep, t k + 1 t_{k+1} tk+1. To solved the lidar motion, we need to establish a geometric relationship between E k + 1 \mathcal{E}_{k+1} Ek+1 and E ~ k + 1 \tilde{\mathcal{E}}_{k+1} E~k+1, or H k + 1 \mathcal{H}_{k+1} Hk+1 and H ~ k + 1 \tilde{\mathcal{H}}_{k+1} H~k+1. Using the transform in (4), we can derive,
    回想一下, E k + 1 \mathcal{E}_{k+1} Ek+1 H k + 1 \mathcal{H}_{k+1} H