激光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

参考博客：http://xchu.net/2020/08/17/49floam/
在该文件中主要有函数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则不适用// 计算每个点的曲率(粗糙度)，参考LOAMstd::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参考博客：https://blog.csdn.net/weixin_38275649/article/details/80972554

遍历所有边缘点！
以该点为中心，即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体素滤波器对点云进行下采样，最终加入到地图中！