文章目录
[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 asj
, 在j
的上下2条附近的laser scan中找到最近的一个,denote asl
. correspondences denote as(i, j , l)
- plane: current lidar frame plane point, denote as
i
, 利用knn找到一个,denote asj
, 在j
的的同一条laser scan中找到最近的一个,denote asl
,上下2条附近的laser scan 中找到一个point, denote asm
. correspondences denote as(i, j , l, m)
- edge: current lidar frame edge point, denote as
- last lidar frame as the tgt frame, use less flat plane features and less sharp features to build knn separately, denote as:
- Build optimization problem with edge and plane correspondences.
- edge factor: optimize distance point
i
to linejl
to zero. - plane factor: optimize distance point
i
to planejlm
to zero.
- edge factor: optimize distance point
- 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
- transform point in feature cloud in to map cs with
- 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.
- mapping part(input: keyframes, output: optimized T_ra and maps)
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中的坐标系