LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)

本文详细解读LeGO-LOAM中featureAssociation.cpp的代码,涉及特征关联、点云处理、IMU数据处理等功能。介绍了点云坐标变换、加速度处理、特征抽取、光滑性计算等关键步骤,旨在帮助读者理解LeGO-LOAM的优化建图原理。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法,建图效果比LOAM要好,但是建图较为稀疏,计算量也更小了。

本文原地址:wykxwyc的博客

github注释后LeGO-LOAM源码:LeGO-LOAM_NOTED
关于代码的详细理解,建议阅读:

1.地图优化代码理解

2.图像重投影代码理解

3.特征关联代码理解

4.LeGO-LOAM中的数学公式推导

以上博客会随时更新,如果对你有帮助,请点击注释代码的github仓库右上角star按钮,你的鼓励将给我更多动力。

featureAssociation.cpp概述

featureAssociation.cpp顾名思义,进行特征关联的过程。

FeatureAssociation

FeatureAssociation()构造函数的内容如下:

  1. 订阅话题:

  2. 发布话题,这些topic有:

    • "/laser_cloud_sharp"(sensor_msgs::PointCloud2)
    • "/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)
    • "/laser_cloud_flat"(sensor_msgs::PointCloud2)
    • "/laser_cloud_less_flat"(sensor_msgs::PointCloud2)
    • "/laser_cloud_corner_last"(sensor_msgs::PointCloud2)
    • "/laser_cloud_surf_last"(cloud_msgs::cloud_info)
    • "/outlier_cloud_last"(sensor_msgs::PointCloud2)
    • "/laser_odom_to_init"(nav_msgs::Odometry)

然后初始化各类参数。

laserCloudHandler

laserCloudHandler修改点云数据的时间戳,将点云数据从ROS定义的格式转化到pcl的格式。

laserCloudInfoHandler

函数比较小:

void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn){
   
   
    timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
    segInfo = *msgIn;
    newSegmentedCloudInfo = true;
}

outlierCloudHandler

void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
   
   
    timeNewOutlierCloud = msgIn->header.stamp.toSec();
    outlierCloud->clear();
    pcl::fromROSMsg(*msgIn, *outlierCloud);
    newOutlierCloud = true;
}

imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)接触过很多次,因为它就是LOAM代码里的那个。
函数的实现:

  1. 通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角;
  2. 对加速度进行坐标变换(坐标变换可以参考下面这张图);
  3. 将欧拉角,加速度,速度保存到循环队列中;
  4. 对速度,角速度,加速度进行积分,得到位移,角度和速度(AccumulateIMUShiftAndRotation());

加速度坐标交换的示意图:
在这里插入图片描述

  • 进行加速度坐标交换时将重力加速度去除,然后再进行 x x x z z z, y y y x x x, z z z y y y的变换。
  • 去除重力加速度的影响时,需要把重力加速度分解到三个坐标轴上,然后分别去除他们分量的影响,在去除的过程中需要注意加减号(默认右手坐标系的旋转方向来看)。
  • 在上面示意图中,可以简单理解为红色箭头实线分解到红色箭头虚线上(根据 p i t c h pitch pitch进行分解),然后再按找 r o l l roll roll角进行分解。

runFeatureAssociation

void runFeatureAssociation()是featureAssociation.cpp中最主要的函数,它调用这个cpp文件中的其他函数。算法步骤如下:

  1. 如果有新数据进来则执行,否则不执行任何操作;
  2. 将点云数据进行坐标变换,进行插补等工作;
  3. 进行光滑性计算,并保存结果;
  4. 标记阻塞点(阻塞点:点云中可能出现的互相遮挡的点);
  5. 特征抽取,然后分别保存到cornerPointsSharp等等队列中去;
  6. 发布cornerPointsSharp等4种类型的点云数据;
  7. 预测位姿;
  8. 更新变换;
  9. 积分总变换;
  10. 发布里程计信息及上一次点云信息;

adjustDistortion

void adjustDistortion()将点云数据进行坐标变换,进行插补等工作。

  1. 对每个点进行处理,首先进行和laboshin_loam代码中的一样的坐标轴变换过程。
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
  1. 针对每个点计算偏航角yaw,然后根据不同的偏航角,可以知道激光雷达扫过的位置有没有超过一半,计算的时候有一部分需要注意一下:
    函数原型:
// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw,因为前面有负号,ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI),在2PI附近
// 下面过程的主要作用是调整ori大小,满足start<ori<end
float ori = -atan2(point.x, point.z);

这里分为4种情况:

  • 没有转过一半,但是start-ori>M_PI/2
  • 没有转过一半,但是ori-start>3/2*M_PI,说明ori太大,不合理(正常情况在前半圈的话,ori-start范围[0,M_PI]
  • 转过一半,end-ori>3/2*PI,ori太小
  • 转过一半,ori-end>M_PI/2,太大
  1. 然后进行imu数据与激光数据的时间轴对齐操作。
    对齐时候有两种情况,一种不能用插补来优化,一种可以通过插补进行优化。
  • 不能通过插补进行优化:imu数据比激光数据早,但是没有更后面的数据(打个比方,激光在9点时出现,imu现在只有8点的)
    这种情况下while循环是以imuPointerFront == imuPointerLast结束的:
// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
   
   
    if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
   
   
        break;
    }
     imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
  • 可以通过插补来进行数据的优化:
    这种情况只有在imu数据充足的情况下才会发生,
    进行插补时,当前timeScanCur + pointTime < imuTime[imuPointerFront],
    而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。
    imuPointerBack是imuPointerFront的前一个imu数据指针。
    插补的代码:
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 
                                 / (imuTime[imuPoi
<think>嗯,用户遇到了LeGO-LOAM编译时的错误,特别是在featureAssociation模块出现CMake编译错误,提示'make[2]: *** [build.make:471]'。我需要先分析可能的原因,然后根据提供的引用信息找到解决方案。 首先,用户提供的引用中有关于CMake找不到geographic_msgs包的错误(引用[1])。这可能是因为缺少相关的ROS包,解决方法通常是安装对应的包,比如sudo apt-get install ros-melodic-geographic-msgs。 另一个常见的问题是C++标准设置。引用[2]提到在CMakeLists.txt中设置C++14标准,这可能是必要的,因为LeGO-LOAM可能需要较新的C++特性。用户需要检查CMakeLists.txt是否包含这些设置。 还有头文件缺失的问题,比如pcl_conversions/pcl_conversions.h找不到(引用[4])。解决方法是安装对应的ROS包,例如ros-melodic-pcl-conversions。这可能也是导致编译错误的原因之一。 此外,用户提到的错误信息可能与依赖项未正确安装或版本不兼容有关。需要确保所有依赖项如PCL、OpenCV、Eigen3等都已正确安装,并且版本与LeGO-LOAM兼容。 接下来,我需要将这些可能的解决方案按步骤整理出来,确保用户能一步步检查和解决问题。同时,要参考用户提供的引用信息,确保解决方案的准确性。例如,引用[3]提到分块查看LeGO-LOAM源码解析,可能涉及曲率计算的问题,但用户当前的错误更可能与编译环境和依赖项相关,所以需要优先处理这些方面。 最后,生成相关的问题,帮助用户进一步理解和解决类似问题,比如如何配置CMake或处理其他编译错误。</think>### LeGO-LOAM featureAssociation模块编译错误解决方案 根据错误日志`make[2]: *** [build.make:471]`和ROS melodic环境特点,按以下步骤排查: 1. **安装缺失的ROS依赖包** - 若出现`geographic_msgs`相关错误,需执行: ```bash sudo apt-get install ros-melodic-geographic-msgs ``` 类似地,若缺少`pcl_conversions`: ```bash sudo apt-get install ros-melodic-pcl-conversions # 引用[4] ``` 2. **配置CMakeLists.txt** - 在项目的CMakeLists.txt中添加C++14标准支持: ```cmake set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # 引用[2] ``` - 添加PCL库依赖: ```cmake find_package(PCL 1.8 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) ``` 3. **验证依赖完整性** - 确保已安装完整依赖: ```bash sudo apt-get install ros-melodic-velodyne-pointcloud \ ros-melodic-tf2-sensor-msgs \ libgoogle-glog-dev ``` 4. **重新编译** ```bash cd ~/catkin_ws catkin clean # 清理旧编译文件 catkin build legoloam ```
评论 12
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值