Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件。
该专栏的其他章节链接如下
https://blog.csdn.net/HUASHUDEYANJING/article/details/130332367

一、整体输入输出

LEGO-LOAM的第二个文件是Feature association，主要功能是对分割后的点云进行特征点提取和匹配操作，估算出位姿，本节将介绍整体功能和怎么从杂乱的点云中提取特征。该文件的整体框架如下：

1.1 输入如下

// 订阅了分割之后的点云
subLaserCloud=nh.subscribe("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
// 订阅的分割点云含有图像的深度信息
subLaserCloudInfo=nh.subscribe("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
// 非聚类的点
subOutlierCloud=nh.subscribe("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
// IMU传感器的消息
subImu=nh.subscribe(imuTopic, 50, &FeatureAssociation::imuHandler, this);

1.2 功能如下

首先对无序点云进行特征提取，之后利用特征点进行匹配计算出位姿

1. Feature Extraction特征提取
// 1.1 点云运动补偿
adjustDistortion();
// 1.2 计算平滑度
calculateSmoothness();
// 1.3 标记遮挡点
markOccludedPoints();
// 1.4 提取特征
extractFeatures();
// 1.5 发布点云
publishCloud();
2. Feature Association特征匹配
if (!systemInitedLM) {
// 2.1 检查系统初始化，点云投影到结束点
checkSystemInitialization();
return;}
// 2.2 更新初始猜测位置
updateInitialGuess();
// 2.3 特征匹配，输出Transformation
updateTransformation();
// 2.4 融合IMU坐标Transformation
integrateTransformation();
// 2.5 发布雷达里程计
publishOdometry();
// 2.6 发布点云用于建图
publishCloudsLast();

1.3 输出如下

pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 1); //当前帧角点
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 1); //当前帧较缓的角点
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 1);  //当前帧面点
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 1); //当前帧较缓的面点
pubAdjustPoints= nh.advertise("/adjust_pointcloud", 1);  //修正后的分割点云
pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 2); //前一帧所有角点
pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 2);  //前一帧所有面点
pubOutlierCloudLast = nh.advertise("/outlier_cloud_last", 2); //前一帧所有界外点pubLaserOdometry = nh.advertise (“/laser_odom_to_init”, 5); //激光里程计

二、Feature Extraction

提取特征能够避免使用全部点云进行计算和匹配，大大降低了内存资源，是3D激光SLAM能够实时建图的特新思想。所有LOAM系的算法均用角点和面点特征进行匹配。

特征识别：角点和面点是环境中的显著特征，它们通常具有高密度的信息，能够提供关于环境结构的重要信息。
区分度：角点和面点在图像中通常具有较高的对比度和区分度，因此它们更容易被检测和匹配。
减少计算负担：提取角点和面点可以减少SLAM系统需要处理的数据量，因为它们是环境中最重要的特征之一。这样可以降低定位和建图算法的计算负荷，提高系统的实时性能。
稳定性：角点和面点通常对于光照变化、视角变化和部分遮挡具有一定的稳定性，这使得它们在不同条件下都能够被可靠地检测和匹配。
特征提取主要包含5个函数如下所示，对点云进行一系列的预处理以提取稳健的特征点，下面将详细介绍每个函数的作用
```
// 1 点云运动补偿
adjustDistortion();
// 2 计算平滑度
calculateSmoothness();
// 3 标记遮挡点
markOccludedPoints();
// 4 提取特征
extractFeatures();
// 5 发布点云
publishCloud();
```
2.1 adjustDistortion

作用：对每一帧点云去除运动畸变。由于激光雷达的一帧点云是在不同时刻收集的，需要将所有点云统一到一个坐标系，如下图所示。点云去畸变可以提高点云的精度和一致性，改善姿态估计，使点云与真实场景更匹配，也能够提升建图的质量。在LEGO_LOAM中是通过IMU对点云去畸变，代码如下。
输入：分割点云segmentedCloud imu数据
输出：修正点云adjustCloud

代码

void adjustDistortion()
{
for (int i = 0; i < cloudSize; i++) {
//计算当前点与起始时刻的点云的相对时间，并保存到点的强度信息中：
  float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
  point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;
  // 速度投影到初始i=0时刻
  VeloToStartIMU();
  // 将点的坐标变换到当前帧初始i=0时刻
  TransformToStartIMU(&point);}
}

2.2 calculateSmoothness

作用：计算点云平滑度。LOAM采用平滑度的方式提取特征点，选择当前点的连续几个点，计算平滑度公式如下，高于阈值的是角点，低于阈值的是平面点
输入：分割点云信息segInfo
输出：点云曲率cloudSmoothness

代码

void calculateSmoothness()
{
  int cloudSize = segmentedCloud->points.size();
  for (int i = 5; i < cloudSize - 5; i++) {
  //计算相邻10个点的深度差的和
      float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
  + segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
  + segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
  + segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
  + segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
  + segInfo.segmentedCloudRange[i+5];            
  cloudCurvature[i] = diffRange*diffRange; //取平方
// 在markOccludedPoints()函数中对该参数进行重新修改
 cloudNeighborPicked[i] = 0;
 cloudLabel[i] = 0;
//1.2.2 保存曲率
  cloudSmoothness[i].value = cloudCurvature[i];
   cloudSmoothness[i].ind = i;}

2.3 markOccludedPoints

作用：标记遮挡点，在SLAM中，去除遮挡点的主要目的是提高地图的准确性和可靠性，以及改善机器人的定位和导航性能。遮挡点是指由于物体遮挡或传感器视野限制而无法观测到的点，可能会引入地图重建和定位的误差。
输入：分割点云信息segInfo
输出：标记遮挡点cloudNeighborPicked

代码

void markOccludedPoints()
{
  int cloudSize = segmentedCloud->points.size();
  for (int i = 5; i < cloudSize - 6; ++i){
  float depth1 = segInfo.segmentedCloudRange[i];
  float depth2 = segInfo.segmentedCloudRange[i+1];
  int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));
  // 标记有遮挡的点
  if (columnDiff < 10){
  if (depth1 - depth2 > 0.3){}
  else if (depth2 - depth1 > 0.3){}
      float diff1 = std::abs(float(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]));
      float diff2 = std::abs(float(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]));
  //标记离散点
  if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
      cloudNeighborPicked[i] = 1;

2.4 extractFeatures

作用：提取出平面点和角点
输入：cloudSmoothness平滑度信息 segmentedCloud分割点云 cloudNeighborPicked相邻遮挡点 segInfo分割信息

输出：//cornerPointsSharp 线特征（不为地面），每个方向上最多2个

           //cornerPointsLessSharp 比cornerPointsSharp平滑的线特征（不为地面），每个方向上20个
          //surfPointsFlat 面特征（为地面），每个方向上最多4个
          //surfPointsLessFlat 面特征（地面或者分割点云中不为线特征的点）

代码

VoidextractFeatures()
for(intj=0;j=ep)
  continue;
  //根据曲率排序
  std::sort(cloudSmoothness.begin()+sp,cloudSmoothness.begin()+ep,by_value());
  for(intk=ep;k>=sp;k--){//选择角特征
      intind=cloudSmoothness[k].ind;
      if(cloudNeighborPicked[ind]==0&&
      cloudCurvature[ind]>edgeThreshold&&
      segInfo.segmentedCloudGroundFlag[ind]==false){}
  for(intk=sp;k<=ep;k++){//选择面特征
      intind=cloudSmoothness[k].ind;
      if(cloudNeighborPicked[ind]==0&&
      cloudCurvature[ind]<surfThreshold&&
      segInfo.segmentedCloudGroundFlag[ind]==true){}

2.5 publishCloud

作用：发布特征点

输入：

  cornerPointsSharp角特征  
  cornerPointsLessSharp缓角特征  
  surfPointsFlat面特征  
  surfPointsLessFlat缓面特征 
  adjustCloud修正点云

输出：

      /laser_cloud_sharp
     /laser_cloud_less_sharp
     /laser_cloud_flat
     /laser_cloud_less_flat
     /adjust_pointcloud

代码

voidpublishCloud()
{
  sensor_msgs::PointCloud2laserCloudOutMsg;
  //最佳线特征
  if(pubCornerPointsSharp.getNumSubscribers()!=0){
  pcl::toROSMsg(*cornerPointsSharp,laserCloudOutMsg);
  laserCloudOutMsg.header.stamp=cloudHeader.stamp;
  laserCloudOutMsg.header.frame_id="camera";
  pubCornerPointsSharp.publish(laserCloudOutMsg);
}
//较为平滑线特征
if(pubCornerPointsLessSharp.getNumSubscribers()!=0){
  pcl::toROSMsg(*cornerPointsLessSharp,laserCloudOutMsg);
  laserCloudOutMsg.header.stamp=cloudHeader.stamp;
  laserCloudOutMsg.header.frame_id="camera";
  pubCornerPointsLessSharp.publish(laserCloudOutMsg);
}
//最佳面特征
if(pubSurfPointsFlat.getNumSubscribers()!=0){
  pcl::toROSMsg(*surfPointsFlat,laserCloudOutMsg);
  laserCloudOutMsg.header.stamp=cloudHeader.stamp;
  laserCloudOutMsg.header.frame_id="camera";
  pubSurfPointsFlat.publish(laserCloudOutMsg);
}
//剩下的面特征
if(pubSurfPointsLessFlat.getNumSubscribers()!=0){
  pcl::toROSMsg(*surfPointsLessFlat,laserCloudOutMsg);
  laserCloudOutMsg.header.stamp=cloudHeader.stamp;
  laserCloudOutMsg.header.frame_id="camera";
  pubSurfPointsLessFlat.publish(laserCloudOutMsg);
}
//发布修正运动畸变后的分割点云
if(pubAdjustPoints.getNumSubscribers()!=0){
  pcl::toROSMsg(*adjustCloud,laserCloudOutMsg);
  laserCloudOutMsg.header.stamp=cloudHeader.stamp;
  laserCloudOutMsg.header.frame_id="camera";
  pubAdjustPoints.publish(laserCloudOutMsg);
}
}

经过点云分割后，效果如下，分为最好的角点和面点

次好的角点和面点

剩下的不被选择的点如下，可以看到特征不明显且杂乱，这些点后续只在建图时用到

下一节将解析如果使用特征点进行匹配

从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析2

桦树无泪

一、整体输入输出

1.1 输入如下

1.2 功能如下

1.3 输出如下

二、Feature Extraction

2.1 adjustDistortion

2.2 calculateSmoothness

2.3 markOccludedPoints

2.4 extractFeatures

2.5 publishCloud

为你推荐

阿尔法形状算法与2D点云的C++实现

【自动驾驶】强化学习DDPG玩赛车游戏

精选自动驾驶TPM技术杂谈 ———— PAPS(Partially Automated Parking System)标准（ISO 20900）

史上最简SLAM零基础解读(8.1) - 旋转矩阵、旋转向量、欧拉角推导与相互转换

自动驾驶轨迹规划之时空语义走廊（一）

精选基于采样的规划——RRT

评论（0）

关于作者

桦树无泪

12

0

178

2

【文献解析】3D Gaussian Splatting for Real-Time Radiance Field Rendering

激光SLAM综述

从零入门激光SLAM（十七）——SLAM中为什么用ESKF误差卡尔曼滤波器

相关推荐

KITTI结果领先地位！Progressive LiDAR Adaptation for Road Detection——PLARD算法

史上最简SLAM零基础解读(10.2) - g2o(图优化)→顶点 (Vertex)编程细节

单目3D目标检测之入门

Lidar-MOS 安装与使用

HD-Map lanelet2(1)

Apollo planning之PathDecider

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

激光SLAM综述/论文解析

3DGS重塑SLAM系统

从0入门激光SLAM

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板