LOAM源码解析2——laserOdometry

最新推荐文章于 2024-02-18 20:20:29 发布

try_again_later

最新推荐文章于 2024-02-18 20:20:29 发布

阅读量1.5k

点赞数

CC 4.0 BY-SA版权

分类专栏：视觉、激光SLAM

本文链接：https://blog.youkuaiyun.com/try_again_later/article/details/105596239

本文详细解析了LOAM（Lidar Odometry and Mapping）算法中的激光雷达里程计部分，包括点云预处理、特征点关联、Jacobian矩阵构建与姿态解算、坐标转换等关键步骤。通过边缘点和平面点的匹配，使用LM算法估计位姿，并结合IMU信息修正旋转量，最终实现局部到全局坐标系的转换。

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

这是LOAM第二部分Lidar laserOdometry雷达里程计。
在第一章提取完特征点后，需要对特征点云进行关联匹配，之后估计姿态。
主要分为两部分：

特征点关联使用scan-to-scan方式t和t+1时刻相邻两帧的点云数据进行配准，分为边缘点匹配和平面点匹配两部分。计算点到直线的距离和点到平面的距离。
姿态解算根据匹配的特征点云使用LM算法估计接收端位姿。

这部分代码完全是放在main函数。
首先介绍前面的参数。后面出现的会重点标注下。

//一个点云周期
const float scanPeriod = 0.1;

//接收到边缘点和平面点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
//上一帧点云的边缘点和平面点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//保存前一个节点发过来的未经处理过的特征点和权重
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//全部点云
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//imu信息
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//上一帧边缘点、平面点构建的KD-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());

//当前帧相对上一帧的状态转移量，局部坐标系
float transform[6] = {
   
   0};
//当前帧相对于第一帧的状态转移量，全局坐标系
float transformSum[6] = {
   
   0};

//点云第一个点的RPY、最后一个点的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变位移、速度
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

0、预处理

订阅节点，发布消息
创建里程计对象、坐标转换对象
同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入

6个sub订阅器分别订阅scanRegistration节点发布的6个消息：/laser_cloud_sharp、/laser_cloud_less_sharp、pubSurfPointFlat、pubSurfPointLessFlat、/velodyne_cloud_2、/imu_trans消息。
调用6个Handler回调函数处理这些边特征、面特征、全部点云和IMU数据，把他们从ROS::Msg类型转换成程序可以处理的pcl点云类型；
4个pub发布器发布/laser_cloud_corner_last、/laser_cloud_surf_last、/velodyne_cloud_3、/laser_odom_to_init消息。

int main(int argc, char** argv)
{
   
   
  /****************0、预处理************************/
  ros::init(argc, argv, "laserOdometry");
  ros::NodeHandle nh;
  // 1.订阅节点，发布消息
  // 订阅scanRegistration发布的六个节点,调用六个回调函数
  ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_sharp", 2, laserCloudSharpHandler);
  ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                             ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);
  ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                      ("/laser_cloud_flat", 2, laserCloudFlatHandler);
  ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                          ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);
  ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> 
                                         ("/velodyne_cloud_2", 2, laserCloudFullResHandler);
  ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> 
                                ("/imu_trans", 5, imuTransHandler);// imu队列长度5
  // 发布四个节点，发布消息
  ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_corner_last", 2);// 队列长度2
  ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_surf_last", 2);
  ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> 
                                        ("/velodyne_cloud_3", 2);
  ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);// 队列长度5
  // 2.创建里程计对象、坐标转换对象
  // 创建里程计对象 laserOdometry
  nav_msgs::Odometry laserOdometry;
  laserOdometry.header.frame_id = "/camera_init";
  laserOdometry.child_frame_id = "/laser_odom";

  tf::TransformBroadcaster tfBroadcaster;

  // 创建坐标变换对象 laserOdometryTrans
  tf::StampedTransform laserOdometryTrans;
  laserOdometryTrans.frame_id_ = "/camera_init";
  laserOdometryTrans.child_frame_id_ = "/laser_odom";

  std::vector<int> pointSearchInd;//搜索到的点序
  std::vector<float> pointSearchSqDis;//搜索到的点平方距离

  PointType pointOri, pointSel/*选中的特征点*/, tripod1, tripod2, tripod3/*特征点的对应点*/, pointProj/*unused*/, coeff;

  //退化标志
  bool isDegenerate = false;
  //P矩阵，预测矩阵
  cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

  int frameCount = skipFrameNum;
  ros::Rate rate(100);
  bool status = ros::ok();

  while (status) {
   
   
    // 调用一次回调函数，订阅当前时刻msg和发布上一时刻处理后的msg
    ros::spinOnce();
    // 3.同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
    if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 
        newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
        fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) 
        {
   
     
      newCornerPointsSharp = false;
      newCornerPointsLessSharp = false;
      newSurfPointsFlat = false;
      newSurfPointsLessFlat = false;
      newLaserCloudFullRes = false;
      newImuTrans = false;

接下来分为：初始化、点云处理、构建Jacobian矩阵估计位姿、坐标转换。

1、初始化

将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准。

将第一个点云订阅的数据保存为上一时刻数据
从ROS转化为pcl，再发布出去
记住原点的翻滚角和俯仰角为imu
T平移量的初值赋值为imu加减速的位移量，为其梯度下降的方向

    //********************1、初始化：将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准****************
      if (!systemInited) {
   
   
        // 1.将订阅的数据保存为上一时刻数据。
        // 将边缘点和上一帧边缘点交换，保存当前帧边缘点值cornerPointsLessSharp下轮使用
        pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
        cornerPointsLessSharp = laserCloudCornerLast;
        laserCloudCornerLast = laserCloudTemp;
        //将平面点和上一帧平面点交换，，保存当前帧平面点surfPointsLessFlat的值下轮使用
        laserCloudTemp = surfPointsLessFlat;
        surfPointsLessFlat = laserCloudSurfLast;
        laserCloudSurfLast = laserCloudTemp;
        //使用上一帧的特征点（边缘+平面）构建kd-tree
        kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合
        kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合

        // 2.将上一时刻数据（边缘+平面）从ROS转化为pcl，再发布出去
        sensor_msgs::PointCloud2 laserCloudCornerLast2;
        pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
        laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudCornerLast2.header.frame_id = "/camera";
        pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

        sensor_msgs::PointCloud2 laserCloudSurfLast2;
        pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
        laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudSurfLast2.header.frame_id = "/camera";
        pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

        //3.记住原点的翻滚角和俯仰角
        transformSum[0] += imuPitchStart;
        transformSum[2] += imuRollStart;

        systemInited = true;
        continue;
      }

      //4.T平移量的初值赋值为加减速的位移量，为其梯度下降的方向
      //（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）
      transform[3] -= imuVeloFromStartX * scanPeriod;
      transform[4] -= imuVeloFromStartY * scanPeriod;
      transform[5] -= imuVeloFromStartZ * scanPeriod;

2、点云配准

点云配准前言：首先保证上一时刻特征点（边缘+平面）数量足够再开始进行匹配，其次，后面需要25次迭代LM算法求解位姿。
重要参数：

laserCloudOri：点云过滤：保留距离小，权重大的点；舍弃距离为零的点。
coeffSel：权重

      if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
   
   //上一时刻特征点数量足够
        // 当前时刻特征点（边缘+平面）数量
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
        int cornerPointsSharpNum = cornerPointsSharp->points.size();
        int surfPointsFlatNum = surfPointsFlat->points.size();
        
        //多次迭代LM算法求解前后位姿变化，这里25次
        for (int iterCount = 0; iterCount < 25; iterCount++) {
   
   
          
          laserCloudOri->clear();
          coeffSel->clear();

针对边缘点和平面点两部分分别进行匹配关系处理。
以边缘点为例：

kd-tree查找一个最近距离点j，Ind为索引，SqDis为距离平方
寻找最邻近点j附近的次临近点l：分为scanID增加和减小两个方向分别进行寻找。
计算点到线的距离，根据距离分配权重

边缘点处理：
在这里插入图片描述

/********************边缘点处理************/
          //从上个点云中边缘点中找两个最近距离点j和l
          //一个点j使用kd-tree查找，在最邻近点j附近(向上下三条扫描线以内)找到次临近点l
          for (int i = 0; i < cornerPointsSharpNum; i++) {
   
   

            // 0、t+1时刻的边缘点转换到初始imu坐标系的点坐标 pointSel
            TransformToStart(&cornerPointsSharp->points[i], &pointSel);

            // ？？？每迭代五次，重新查找最近点（降采样）
            if (iterCount % 5 == 0) {
   
   
              std::vector<int> indices;
              pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
              // 1、kd-tree查找一个最近距离点j，Ind为索引，SqDis为距离平方
              kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
              int closestPointInd = -1, minPointInd2 = -1;

              //在最邻近点j附近(向上下三条扫描线以内)找到次临近点l
              //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
              if (pointSearchSqDis[0] < 25) {
   
   //找到的最近点距离的确很近的话
                closestPointInd = pointSearchInd[0];// 最近点j
                //提取最近点线号
                int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值

                // 2、寻找最邻近点j附近的次临近点l

                // 向scanID增大的方向查找
                for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
   
   
                  //非相邻线结束，距离超过三条线跳出
                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {
   
   
                    break;
                  }
                 // 计算所有点和最近邻点j距离平方
                  pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                               (laserCloudCornerLast->points[j].x - pointSel.x) + 
                               (laserCloudCornerLast->points[j].y - pointSel.y) * 
                               (laserCloudCornerLast->points[j].y - pointSel.y) + 
                               (laserCloudCornerLast->points[j].z - pointSel.z) * 
                               (laserCloudCornerLast->points[j].z - pointSel.z);

                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {
   
   //确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
                    if (pointSqDis < minPointSqDis2) {
   
   //距离更近，要小于初始值5米
                        //更新最小距离与点序
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  }
                }

                //向scanID减小的方向查找，向下三条线
                for (int j = closestPointInd - 1; j >= 0; j--) {
   
   
                  if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
   
   
                    break;
                  }

                  pointSqDis = (laserCloudCornerLast->points