3D激光点云数据处理入门(一)—— 使用LOAM进行点云地图创建
LOAM 原理简述
论文地址 : LOAM: Lidar Odometry and Mapping in Real-time
作者主页: Ji Zhang, Ph.D 其中有作者所有的相关论文,并且都附有 Video, 感兴趣的可以去查看。
Google Scholar
作者想解决的问题: 构建低漂移的里程计。论文完成的工作是使用3D激光雷达构建出的里程计构来建三维点云地图。 但是作者的目标是缩小激光里程计的漂移和计算量,重点并不是构建出三维地图,所以作者没有考虑回环问题(loop closure)。
作者实现的方法 : 作者的思路同SLAM(simulanteous localization and mapping)的思路一致,算法思路如下图
主要经过以下几个步骤
- Point Cloud Registration - 接收激光数据,完成点云的注册和特征点云(边和面)提取。
- Lidar Odometry - 通过特征点云的匹配以10hz频率发布一个里程计。此时的里程计精度并不高。
- Lidar Mapping - 接收里程计信息和当前点云数据,基于粗匹配的里程计位姿进行精匹配,根据估计出的位姿将点云注册到地图中,并按照1hz发布更新后的位姿。
- Transform Integration - 接收 lidar odometry 和 lidar mapping 发布的位姿信息,以10hz 的频率发布里程计信息。
topic关系
运行 'rosrun rqt_graph rqt_graph
, 我们可以查看各个 topic 和节点的关系。
算法分析
- 如何选取特征点 边点(edge point)和平面点 (planar point)
作者首先定义了一个衡量点平滑度的公式 , 通过测量点两边的距离差异,来衡量该点的平滑程度。c值最大的点将作为边点而cz值最小的点为平面点。 将360度分为四个区域,每个区域最多产生两个边点和四个平面点。这些边点和平面点如下图所示。
这些提取出来的点作为特征点来完成前后帧的匹配,来估计自身的位姿。
- 构建损失函数 特征点选取后, 根据 tf 关系转换到 map 坐标系下,在map 中寻找最近的 边点和平面点, 将两个最近的边点和平面点当成对应点构建一个转换矩阵,(t_x, t_y, t_z, theta), 对该转换举证求雅克比,使用高斯牛顿法缩小损失函数直至收敛。
算法伪代码
===========
未完待续!!!
- 矩阵的转换和实现
- 算法结果
LOAM 建图实践
创建你的 ROS Workspace
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
下载LOAM Package
$ cd ~/catkin_ws/src
$ git clone https://github.com/laboshinl/loam_velodyne.git
$ cd ..
$ catkin_make -DCMAKE_BUILD_TYPE=Release
$ source ~/catkin_ws/devel/setup.bash
下载数据包
$ wget http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz
$ tar -axvf hdl_400.bag.tar.gz
运行 LOAM
$ roslaunch loam_velodyne loam_velodyne.launch
$ rosbap play hdl_400.bag