Lidar SLAM Review

[WIP] Lidar SLAM Review(Mainly focus on lidar slam pipeline, especially the backend)

A-LOAM

Point Cloud Preprocess

• remove nan point cloud
• remove points that are too close to lidar, such as < 0.1m

Feature Extraction

• calculate smoothness for each scan in point cloud
• classify points into four types of feature points according to the smoothness
  • edge points: sharp, less sharp
  • plane points: flat, less flat

Lidar Odometry

• input: two lidar frame(last frame and current frame), there are four types point features in each frame. 如果使用了ROS来处理传感器的消息，要注意消息的同步。first lidar frame到达的时候系统还不能运行，标记为initialized状态，second lidar frame到达的时候，开始运行LO系统
• output: delta pose

Algorithm Steps

• For sharp edge and flat plane point features current lidar frame, do undistortion. 所谓undistortion就是把所有points transform 到 start timestamp 或者 end timestamp来进行对齐。利用了匀速运动模型，也就是用last delta pose对latest lidar frame做undistortion
• Use KNN to do association
  • last lidar frame as the tgt frame, use less flat plane features and less sharp features to build knn separately, denote as: last_plane_knn_, last_edge_knn_
  • current lidar frame as the src frame, 给当前frame中的flat plane features and sharp edge features 用knn去找associated features，搜索距离 5.0 m。
    • edge: current lidar frame edge point, denote as i, 利用knn找到一个，denote as j, 在 j的上下2条附近的laser scan中找到最近的一个，denote as l. correspondences denote as (i, j , l)
    • plane: current lidar frame plane point, denote as i， 利用knn找到一个，denote as j, 在 j的的同一条laser scan中找到最近的一个,denote as l，上下2条附近的laser scan 中找到一个point, denote as m. correspondences denote as (i, j , l, m)
• Build optimization problem with edge and plane correspondences.
  • edge factor: optimize distance point i to line jl to zero.
  • plane factor: optimize distance point i to plane jlm to zero.
• accumulate odometry to world pose

  t_w_curr_ = t_w_curr_ + q_w_curr_ * optimized_t_last_curr_;
  q_w_curr_ = q_w_curr_ * optimized_q_last_curr_;

• 每5帧发布一次 last_edge_features_, last_plane_features_,其实就是关键帧。后续利用关键帧构建地图，优化 map_to_last_frame_pose.

Mapping

• measurements data sync(feature cloud and odometry): 以其中一个measurement type为参考，始终将该type的measurement保持为最新，其他的几类measurements逐个进行drop处理，保证align到同一个时间戳上。
• init odomety to world pose

  optimized_q_w_curr_ = q_wmap_wodom_ * q_wodom_curr_;
  optimized_t_w_curr_ = q_wmap_wodom_ * t_wodom_curr_ + t_wmap_wodom_;

• map construction and update
• build optimization problem between last feature cloud to map
  • transform point in feature cloud in to map cs with optimized_q_w_curr_, optimized_t_w_curr_
  • use knn to find correspondences between the transformed last feature cloud and map feature cloud
  • build optimization problem and solve
• update poses

  q_wmap_wodom_ = optimized_q_w_curr_ * q_wodom_curr_.inverse();
  t_wmap_wodom_ = optimized_t_w_curr_ - q_wmap_wodom_ * t_wodom_curr_;

• transform last feature cloud to map cs

  Eigen::Vector3d point_w = optimized_q_w_curr_ * point_curr + optimized_t_w_curr_;

• visualization
  • downsampled cloud that need to be visualized

Conclusion and Comment

• A-LOAM相比LOAM，相当于重构了一遍代码，但是也有细微的区别
  • 使用ceres求解代替复杂的解析公式推导
  • undistortion没有使用IMU，直接假设的匀速运动模型
• A-LOAM， LOAM， LeGO-LOAM，在undistortion的实现上可读性都比较差。更好的一个工程实践是，拿到点云后，做完预处理，最后一步，统一进行去畸变，全部align到某一个时间戳下面（start, mid, end都可）。
• A-LOAM 中LO这一步，last frame中点云没有进行undistortion，但是current frame中点云在optimization过程中进行了一次undistortion。虽然看起来work，但个人认为这是不符合实际情况的，一旦车辆运动比较激进导致导致点云畸变的厉害，用去畸变点云和畸变点云进行匹配就会出比较大的问题。。
• mapping 部分找correspondences的时候，利用了PCA方法，PCA是依赖geometry-feature-based LO的核心。许多特征提取的方法的背后都和PCA有关系。
• 用更清晰的定义来描述各坐标系：
  • mapping part(input: keyframes, output: optimized T_ra and maps)
    • T_ra = T_map_to_odometry * T_odometry_to_ra, optimized parameters
    • T_map_to_odometry: middle variables
  • odometry part(input: two consecutive lidar frames, output: accumulated T_odometry_to_ra)
    • T_odometry_to_ra = T_odometry_to_ra * T_last_ra_to_curr_ra
    • T_last_ra_to_curr_ra = T_delta_ra_pose, optimized parameters.

LeGO-LOAM

Point Cloud Preprocess

• extract ground points, use the easiest algorithm ground points satisfy: tan-1(|delta_z|, |delta_xy|) < thresold. 缺点就是车辆在平地上时候，斜坡上的地面不能被很好的分割，显然kitti中没这个场景，所以对作者的实验影响也不大
• do segmentation for non-ground points and reject small size cluster, which are noise points. (BFS-based)

Feature Extraction

• do undistortion for segmented cloud with IMU, inherited from LOAM method
• calculate smoothness
• mark occluded points
• extract features from unpicked segmented cloud, curvature > threshold

个人认为特征提取才是LOAM的精髓。

Conclusion and Comments

• 相比LOAM，有一些创新改动
  • 增加了前端点云预处理部分，提取了地面点。
  • 两步优化，ground来优化roll pitch z, 非ground来优化x y yaw

Reference && Appendix

• 欧拉角和旋转矩阵之间的转换
  • euler angle的叫法跟坐标轴强相关。对于ADV来说，一般定义：x front(roll), y left(pitch), z up(yaw). 对于loam中定义的相机坐标系而言，z front(roll), x left(pitch), y up(yaw).
  • euler angle 正负：右手定则，四个手指的方向为正方向
  • euler angle range: 如果是ADV，roll/pitch in (-90, +90], yaw in (-180, +180]; 如果是飞机坐标系， roll/pitch/yaw in (-180, + 180]
  • 旋转顺序和旋转轴
    • 三个轴不同的顺序共有6种，欧拉角wiki上定义的更为详细，按照静态和动态分类共有12种。
    • 旋转轴为饶固定轴（外旋），和绕旋转后的自身轴（内旋）两种
  • 欧拉角的叫法虽说和坐标轴的定义强相关，但是按照roll， pitch, yaw的物理意义表示之后，旋转矩阵的计算方式是唯一的。 R(yaw) * R(pitch) * R(roll). 对ADV来讲，我们可以这样解释这个公司：ZYX顺序的内旋(right multiply)等价于XYZ顺序的外旋(left multiply)，表示 transform_curr_imu_to_init_imu
  • 简单来说，euler angle是根据一个物体的姿态来定义的，翻滚角，俯仰角，航向角。为了和坐标轴对应起来，人们定义了12中旋转方式，但是无论哪种旋转方式，最终得到的旋转矩阵的形式都是唯一的，即R(yaw) * R(pitch) * R(roll)。
• LeGO-LOAM中的坐标系