标题：Gravity-constrained point cloud registration

作者：Vladim ́ır Kubelka, Maxime Vaidis and Franc ̧ois Pomerleau，加拿大拉瓦尔大学

来源：IROS 2022

摘要

    视觉和激光SLAM算法从IMU中获益良多。高频率的IMU数据弥补了低频的传感器数据。除此之外，可以从重力向量中获取三自由度姿态中的两个。在视觉里程计中，IMU已经被用来将6自由度位姿估计转换为4自由度。在lidar SLAM中，重力测量在后端优化中经常被用来作为惩罚项来防止地图变形。在这篇文章中，我们提出了一种基于迭代最近点(ICP)的前端方法，其利用了重力向量提供的旋转可观自由度，并且提供了与重力向量对齐的位姿估计。我们相信这个方法可以支持回环检测，从而加速全局地图的优化。所提出的方法在DARPA比赛中已经被充分的测试过。我们的方法相比于传统的ICP算法，可以减小30%的定位漂移。并且，其代码已经作为libpoiintmatcher库的一部分开源给社区。

方法论

1. ICP Mapper

用norlab_icp_mapper（github同名）来建图。这是一个轻量化的建图系统，包含ICP与地图管理。没有使用全局优化。该系统包含以下几个步骤：

1. 根据初始位姿估计将点云转到世界坐标系中去。

2. 根据转换后的点云与地图做ICP配准。

3. 将配准后的点云塞入到地图中去。

其中，第一步的初始位姿包含旋转与位移，旋转通过IMU给出，位移通过机器人的轮速里程计给出。为了系统的实时性，输入的点云被随机降采样过，并且根据里程计与IMU的信息去除了运动畸变。最后，只有一部分配准后的点云被塞入到地图中，以保证地图点云的密度不会过大。

2. 4-dof ICP

本文采用的是点到平面的ICP方法。首先定义要求解的transformation参数，为一个四维向量：

其中，γ是机器人旋转中绕着z轴的分量（即偏航角），而 t_x, t_y, t_z是旋转分量。传统点到平面ICP的误差为：

由于在配准前已经将点云转到世界坐标系，且仅对yaw角方向的旋转进行优化，因此重新将旋转参数化为

其中n_k是点q_k所在的平面。将上述的R代入到误差中，有

通过使得上式偏导数为0，可以求得最佳的γ, t：

将上述偏导数写成Aτ = b的形式，可得

令等式左边为A，右边为b，则有

上式可以通过cholesky 分解求解。为了构建A与b，需要进行一个k次的for循环。另一种构建公式的方法可以为：

从而有

ICP方法根据上式不断调整 τ的值，直到误差小于特定的阈值。我们的方法不调整roll与pitch，因此估计出来的旋转的yaw与pitch角与IMU估计出来的值相同。

实验

    本文的代码在libpointmatcher库，通过开启ICP匹配的force4DOF选项来获得。本文在两种实验条件下测试了我们的代码，其一是DARPA的地下环境，另一个是蒙莫朗西研究森林中的 1.5 公里长的森林步道。该数据集中，一个机器人在2021年三月，在这条道路的两侧沿着该道路行进了5次。

1. DARPA 比赛环境

DARPA比赛环境包括城市场景以及地下场景，DARPA官方给出了一个准确的点云地图，以此可以计算定位的真值。该实验给出了本文提出的方法的定位效果：

地下矿井的定位精度对比：

城市场景下的定位精度对比：

2. 蒙莫朗西 森林大尺度户外场景

将ICP的方法与本文的方法进行了对比，真值由GPS-RTK提供。

路线：

精度比较：

总结

    本文给出了一个在长直路线下，没有后端优化情况下的一种依赖于IMU的降维的ICP方法。（推测其不用传统激光SLAM方法的原因，一是为了节省算力给其他模块，二是路径中长直场景比较多。）相较于传统的6自由度ICP方法，其提供了更高的定位精度。在低速场景下，会有比较好的效果。此外，imu与lidar的外参误差也会影响定位的结果。

Abstract

Visual  and  lidar  Simultaneous  Localization  andMapping  (SLAM)  algorithms  benefit  from  the  Inertial  Mea-surement  Unit  (IMU)  modality.  The  high-rate  inertial  datacomplement  the  other  lower-rate  modalities.  Moreover,  in  theabsence of constant acceleration, the gravity vector makes twoattitude angles out of three observable in the global coordinateframe. In visual odometry, this is already being used to reducethe 6-Degrees Of Freedom (DOF) pose estimation problem to 4-DOF. In lidar SLAM, the gravity measurements are often usedas a penalty in the back-end global map optimization to preventmap deformations. In this work, we propose an Iterative ClosestPoint (ICP)-based front-end which exploits the observable DOFand  provides  pose  estimates  aligned  with  the  gravity  vector.We believe that this front-end has the potential to support theloop  closure  identification,  thus  speeding  up  convergences  ofglobal  map  optimizations.  The  presented  approach  has  beenextensively tested against accurate ground-truth localization inlarge-scale outdoor environments as well as in the SubterraneanChallenge  organized  by  Defense  Advanced  Research  ProjectsAgency (DARPA).  We show that  it can reduce  the localizationdrift  by  30 %  when  compared  to  the  standard  6-DOF  ICP.Moreover,  the  code  is  readily  available  to  the  community  as  apart  of  the  libpointmatcher  library.