激光SLAM系统Fast LOAM (Lidar Odometry And Mapping)源码解析

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

---

github地址：Fast LOAM (Lidar Odometry And Mapping)

Fast LOAM提供了mapping和localization的两个节点，目前只使用其定位部分，以velodyne为例分析一下源码。

    <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
    <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
    <node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>

即只需要运行floam_odom_estimation_node和floam_laser_processing_node两个节点即可！

1. laserProcessingNode.cpp

先从点云处理开始，看一下其订阅和发布的topic

ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, velodyneHandler);
pubLaserCloudFiltered = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points_filtered", 100);
pubEdgePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_edge", 100);
pubSurfPoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf", 100); 

其订阅了velodyne发来的点云数据，并将其不断的存到pointCloudBuf中，用于处理。 将其所有帧提取到的面特征和边缘特征拼接之后发出去velodyne_points_filtered，其时间戳同处理的该帧点云，frame_id则为base_link。

//拼接
*pointcloud_filtered+=*pointcloud_edge;
*pointcloud_filtered+=*pointcloud_surf;
//设定时间戳和frame
laserCloudFilteredMsg.header.stamp = pointcloud_time;
laserCloudFilteredMsg.header.frame_id = "/base_link";

---

在该节点中主要函数为laser_processing()，每次从pointCloudBuf中取出一帧，即pointcloud_in，提取其面特征和边缘特征。

laserProcessing.featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf);

该函数后面会在laserProcessingClass.cpp中介绍。 然后便发布了计算得到的面特征laser_cloud_surf和边缘特征laser_cloud_edge。

2.laserProcessingClass.cpp

参考博客：xchu.net/2020/08/17/… 在该文件中主要有函数featureExtraction和featureExtractionFromSector

（1）featureExtraction

首先计算点云线束并分类

// 遍历点云，给点云的每个点进行分类，具体属于哪个线束
 for (int i = 0; i < (int) pc_in->points.size(); i++)
  {
      int scanID=0;   
        // 比较远或者过近的点去掉
      double distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].y * pc_in->points[i].y);
      if(distance<lidar_param.min_distance || distance>lidar_param.max_distance)
          continue;
        // 计算点在垂直方向的角度，用来计算属于哪个线束
      double angle = atan(pc_in->points[i].z / distance) * 180 / M_PI;
        // 根据激光雷达的线束进行点的分类
      if (N_SCANS == 16)
      {
          scanID = int((angle + 15) / 2 + 0.5);// 16线两线束间间隔30度，从-15到+15共16根线，这里为了避免出现负数，统一加15度
          if (scanID > (N_SCANS - 1) || scanID < 0){continue;}
}
      laserCloudScans[scanID]->push_back(pc_in->points[i]);  // 将每个点装入不同的容器

  }

接下来计算每个点的曲率

// 按点云线束进行遍历
for(int i = 0; i < N_SCANS; i++){
      if(laserCloudScans[i]->points.size()<131){continue;} // 某一束点云少于131则不适用
      // 计算每个点的曲率(粗糙度)，参考LOAM
      std::vector<Double2d> cloudCurvature; 
      int total_points = laserCloudScans[i]->points.size()-10;
      // 当前点前后各五个点，计算其三个方向上距离和的平方
      for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){
	... 
          Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ);
          cloudCurvature.push_back(distance);

      }

将点云均匀划分成6个子图，保证各方向都能提取到特征

 for(int j=0;j<6;j++){
      int sector_length = (int)(total_points/6);
      int sector_start = sector_length *j;
      int sector_end = sector_length *(j+1)-1;
      if (j==5){
          sector_end = total_points - 1; 
      } 
      std::vector<Double2d> subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end); 
      // 特征提取
      featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf);   
}

（2）LaserProcessingClass

在特征提取时，将点云按曲率大小进行排序，遍历点云，根据曲率判断其是平面点还是边缘点。 找到曲率最大的20个点，认为是边缘点，其他的则为平面点。

3.odomEstimationNode.cpp

在经过点云处理之后，发出面特征和边缘特征数据，在该节点中订阅。

ros::Subscriber subEdgeLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_edge", 100,velodyneEdgeHandler);
ros::Subscriber subSurfLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf", 100,velodyneSurfHandler);

首先是时间戳对齐，去掉较老时间戳的点云，然后有一个初始化的过程，该函数会在后面说到：

if(is_odom_inited == false){
	odomEstimation.initMapWithPoints(pointcloud_edge_in, pointcloud_surf_in);
	is_odom_inited = true;
	ROS_INFO("odom inited");
}

初始化完成之后，便将两种特征传入到updatePointsToMap函数中，求解优化得到位姿odomEstimation.odom。

odomEstimation.updatePointsToMap(pointcloud_edge_in, pointcloud_surf_in,filter_distance);

此处filter_distance，是我修改引入的参数，为了解决保留点云太多时，求解位姿效率太低，导致发出的tf延迟较高问题，后面在函数中会详细说明。

最终将其tf广播出去，即transform

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), odom_frame_id, "base_link"));

此处的odom_frame_id即为velodyne发出的里程计，tf的时间为当前的时间，如果为了解决两台主机时间不同步的问题，可以将该tf的ros::Time::now()修改成点云数据的时间pointcloud_time，但是此时忽略了位姿求解的时间，影响还有待测试。

4.odomEstimationClass.cpp

初始化initMapWithPoints的过程，即直接把第一帧加入到地图中去，毕竟第一帧也没什么可参考的。 当初始化完成后的updatePointsToMap中，设置了一些优化的代价和参数。 在初始化完成之后optimization_count便被设置成12，然后迭代次数不断减少至2，即只迭代两次。

假设当前帧和上一帧有相同的运动，然后开始优化：

for (int iterCount = 0; iterCount < optimization_count; iterCount++){
        ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);
        
        problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization());  
        addEdgeCostFactor(downsampledEdgeCloud,laserCloudCornerMap,problem,loss_function);
        addSurfCostFactor(downsampledSurfCloud,laserCloudSurfMap,problem,loss_function);
        
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.max_num_iterations = 4;
        options.minimizer_progress_to_stdout = false;
        options.check_gradients = false;
        options.gradient_check_relative_precision = 1e-4;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);

    }

其中主要为添加两种特征点云分别到平面和边缘的残差项，即addEdgeCostFactor和addSurfCostFactor。 此外，在使用该里程计时，实测中高度信息并不准确，不知是环境问题还是参数问题，有待解决，因此目前只使用其2D信息，添加如下修改：

//like
Eigen::Vector3d tran(t_w_curr(0),t_w_curr(1),0);
odom.translation() = tran;

(1) addEdgeCostFactor && addSurfCostFactor

KD tree参考博客：blog.csdn.net/weixin_3827…

遍历所有边缘点！ 以该点为中心，即point_temp，计算近邻的索引值pointSearchInd、近邻的中心距pointSearchSqDis，寻找最邻近的5个点！

• 该点point_temp是经过pointAssociateToMap，转换到地图坐标系下的点

 kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 

然后构建点到平面/直线的残差，具体便不再详细介绍！

(2) addPointsToMap

该函数则是将点云添加到地图中，并且进行过滤下采样。 首先则是把得到的边缘特征和面特征不断的加入到map当中，虽然我们没用到其建图功能，但是在优化求解位姿时，需要和地图中的点云作为参考。

    for (int i = 0; i < (int)downsampledEdgeCloud->points.size(); i++)
    {
        pcl::PointXYZI point_temp;
        pointAssociateToMap(&downsampledEdgeCloud->points[i], &point_temp);
        laserCloudCornerMap->push_back(point_temp); 
    }
    
    for (int i = 0; i < (int)downsampledSurfCloud->points.size(); i++)
    {
        pcl::PointXYZI point_temp;
        pointAssociateToMap(&downsampledSurfCloud->points[i], &point_temp);
        laserCloudSurfMap->push_back(point_temp);
    }

为了避免场景过大导致优化计算位姿太耗时，需要对加入到地图中的点云进行剔除

double x_min = +odom.translation().x()-filter_distance;
double y_min = +odom.translation().y()-filter_distance;
double z_min = +odom.translation().z()-filter_distance;
double x_max = +odom.translation().x()+filter_distance;
double y_max = +odom.translation().y()+filter_distance;
double z_max = +odom.translation().z()+filter_distance;
cropBoxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));
cropBoxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
cropBoxFilter.setNegative(false);  

即以filter_distance为范围，将当前位置附近阈值之外的点进行过滤。 然后经过VoxelGrid体素滤波器对点云进行下采样，最终加入到地图中！