在ROS Kinetic上运行loam_velodyne

• 系统框架简介

　　系统主要分为Lidar Odometry与Lidar Mapping 两部分，里程计部分默认以10Hz的频率计算里程计，Mapping 部分以1Hz的频率进行地图的维护。

　　（1）Lidar Odometry主要部分包括：点云过滤；特征提取与分类；帧间时间补偿与运动补偿。　

　　　　点云过滤：剔除无效值，并避免选择与极线方向平行的点以及存在遮挡的点，因为这两种点会导致较大的不确定性。

图(a)中的点B与（b）中的A点均应避免选取

　　　　特征提取与分类：计算曲率，根据曲率可将特征划分为corner与surface两类，并进一步细分为sharp、sharp less、flat、flat less，并将特征保存至KDtree ε_k+1、H_k+1。

　　　　帧间时间、运动补偿：消除由运动、帧间时间切换导致的畸变。若配有IMU,则初始pose由IMU进行预测。时间上通过时序与扫描顺序的逻辑关系进行补偿,具体实现如下。

   bool  halfPassed = false;
   float ori = -atan2(point.x, point.z);
    if (!halfPassed) {
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else {
      ori += 2 * M_PI;
      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      } 
    }

运动配准主要通过构建点到直线以及点到平面的残差项，通过高斯牛顿方法求解，迭代。若为第一帧扫描T_k+1 =0 ，若一帧扫描结束则k+1时刻的点云映射到k+2时刻，并返回t_k+1 到t时刻点云的位姿变换关系T_k+1，若为一线激光扫描结束，则只返回T_k+1，具体算法如下图所示。

　　（2）Lidar Mapping部分以较低频率运行，每完成一帧扫描调用一次。

　　　　　对corner与surface特征点基于pose进行拼接，通过五个点求点云簇的协方差矩阵的特征值和特征向量，基于ICP方法，将新的帧与submap进行配准，最后进行地图发布。

• 源码地址

https://github.com/daobilige-su/loam_velodyne

•  测试

　　第一个终端启动建图：

roslaunch loam_velodyne loam_velodyne.launch

　　第二个终端回放录制的bag数据：

rosbay play nsh_indoor_outdoor.bag -r 0.5

bag链接：https://pan.baidu.com/s/1yPn6Eyu2KRWONn5N5_WDHg 
提取码：slam 

 

• 运行结果

 

•  参考文献

 　　　[1] Zhang J , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014.

　　     [2] Zhang J , Singh S . Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots, 2017, 41(2):401-416.

　　　 [3] https://zhuanlan.zhihu.com/p/57351961