LeGo-LOAM源码篇:源码分析(1)

LeGo-LOAM

A lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (placed horizontal) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.

简单介绍一下LeGO-LOAM（激光SLAM，IMU+LiDAR），以LOAM为基础，实现与其同等的精度同时大大提高了速度。利用点云分割区分噪声，提取平面与边特征，使用两步LM优化方法求解位姿，并且添加回环检测减小漂移。

接下来直接切入代码部分：

github：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

LeGO-LOAM代码结构

首先看一下代码结构，非常清晰，cloud_msgs、launch、include、src（核心程序），接下来具体看一下:

• CMakeList.txt

  ...
  find_package(GTSAM REQUIRED QUIET)
  find_package(PCL REQUIRED QUIET)
  find_package(OpenCV REQUIRED QUIET)
  ...
  

  依赖库：GTSAM、PCL、OPenCV

  这里简单介绍一下GTSAM，GTSAM 是一个在机器人领域和计算机视觉领域用于平滑（smoothing）和建图（mapping）的C++库，采用因子图（factor graphs）和贝叶斯网络（Bayes networks）的方式最大化后验概率。如果想深入了解，可以查看参考资料[2]。

• launch/run.launch

  节点：rviz、camera_init_to_map、base_link_to_camera、imageProjection、featureAssociation、mapOptmization、transformFusion

  主要节点就是后4个，也是对应src目录下的核心程序。

• include/utility.h

  定义数据结构：点云、话题、激光雷达参数、图像分割参数、特征提取参数、优化参数、建图参数

  ...
  /*
      * A point cloud type that has "ring" channel
      */
  struct PointXYZIR
  {
      PCL_ADD_POINT4D;
      PCL_ADD_INTENSITY;
      uint16_t ring;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  } EIGEN_ALIGN16;
  
  POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
                                     (float, x, x) (float, y, y)
                                     (float, z, z) (float, intensity, intensity)
                                     (uint16_t, ring, ring)
  )
  ...
  

  这里定义了一种包含ring_id的点云数据类型，velodyne的激光雷达是有这个数据的，其它厂商的不一定有;区别于laser_id，ring_id是指从最下面的激光线依次向上递增的线号（0-15 for VLP-16）。

  ...
  // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
  extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
  
  // VLP-16
  extern const int N_SCAN = 16;//number of lasers
  extern const int Horizon_SCAN = 1800;//number of points from one laser of one scan
  extern const float ang_res_x = 0.2;//angle resolution of horizon
  extern const float ang_res_y = 2.0;//angle resolution of vertical
  extern const float ang_bottom = 15.0+0.1;
  extern const int groundScanInd = 7;//表示地面需要的线圈数;
  ...
  

  针对不同激光雷达的参数设置，要根据自己的实际情况去调整，假如你是用数据集的数据，比如KITTI，就需要针对HDL-64E进行调整，而且由于该雷达垂直角分辨率不是均匀的，还需要自己编写后续的投影程序。

  接下来切入核心程序，由于有四个cpp，为了不要过长，今天先剖析一下imageProjection.cpp。

• src/imageProjection.cpp

  总体流程：订阅点云数据回调处理->点云转换到pcl预处理->截取一帧激光数据->投影映射到图像->地面移除->点云分割->发布点云数据->重置参数;

  订阅到激光雷达数据的回调处理函数：

  void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   // 1. Convert ros message to pcl point cloud
  copyPointCloud(laserCloudMsg);//使用pcl库函数;
   // 2. Start and end angle of a scan
   findStartEndAngle();//1.一圈数据的角度差，使用atan2计算;2.注意计算结果的范围合理性
   // 3. Range image projection
   projectPointCloud();//将激光雷达数据投影成一个16x1800（依雷达角分辨率而定）的点云阵列
   // 4. Mark ground points
   groundRemoval();//根据上下两线之间点的位置计算两线之间俯仰角判断，小于10度则为地面点
   // 5. Point cloud segmentation
   cloudSegmentation();//首先对点云进行聚类标记，根据标签进行对应点云块存储;
   // 6. Publish all clouds
   publishCloud();//发布各类点云数据
   // 7. Reset parameters for next iteration
   resetParameters();
  }
  

  projectPointCloud()：将3D point cloud投影映射到2D range image

  // find the row and column index in the image for this point
  if (useCloudRing == true){
      rowIdn = laserCloudInRing->points[i].ring;
      }else{
      verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
      rowIdn = (verticalAngle + ang_bottom) / ang_res_y;//确定行索引
  }
  horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
  columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;//确定列索引
  range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);//确定深度
  

  激光雷达坐标系

  atan2(y，x);

  反正切的角度等于X轴与通过原点和给定坐标点(x, y)的直线之间的夹角，结果为正表示从X轴逆时针旋转的角度，结果为负表示从X轴顺时针旋转的角度;

  atan2函数

  1.确定行索引

  根据ring id或者根据坐标计算：

  ${\theta }_v=atan2(z/(x^2+y^2{)}^{1/2})$

  $row$