A_loam源码解析一

最新推荐文章于 2024-04-26 19:46:50 发布

mcoll

最新推荐文章于 2024-04-26 19:46:50 发布

阅读量660

点赞数 2

文章标签：自动驾驶

本文链接：https://blog.csdn.net/mcoll/article/details/125522036

版权

A_LOAM源码解析

A_LOAM源码解析0 前言1 ROS节点示意图2 代码激光雷达测距原理：多线激光雷达：2.1scanRegistration.cpp1、设置节点2、订阅velodyn_points话题消息，执行laserCloudHandler()回调函数3、laserCloudHaint cloudsize = laserCloudIn.points.size();4、定义需要发布的消息2.2laserOdometry.cpp2.3laserMapping.cpp2.4lidarFactor.cpp

0 前言

主要特点：

1.去掉了和IMU相关的部分

2.使用Eigen(四元数)做位姿转换，简化了代码

3.使用ceres_solver做位姿优化，简化了代码

4.原loam使用了大量的欧拉角计算，源码隐晦难懂

1 ROS节点示意图

2 代码

激光雷达测距原理：

激光雷达发射脉冲打到物体后返回，根据激光测距原理，通过计算发射时间和接收时间的时间差就能计算出该物体距离激光雷达中心的距离；

多线激光雷达：

多线激光雷达有多个发射器同时工作，这些发射器竖排排列，一起水平旋转；激光雷达在一定时间内旋转一圈，即一帧的点云数据，一帧的数据会包括不同时间的点云，然而当激光雷达运动时，则会产生运动畸变；

2.1scanRegistration.cpp

主要作用：特征提取根据曲率划分角点和面点

1、设置节点

ros::init(argc , argv, "scanRegistration");  //节点明晨和launch中预设的名称保持一致

2、订阅velodyn_points话题消息，执行laserCloudHandler()回调函数

    ros::Subscriber subLaserCloud= nh.subscribe<sensor_msgs::PointCloud2>("/velodyn_points", 100 ,laserCloudHandler());

3、laserCloudHaint cloudsize = laserCloudIn.points.size();

1）如果系统没有初始化，就等几帧

2）将ros消息转换为pcl消息laserCloudIn，取出nan点和近点

PCL:: PointCloud<pcl::PointXYZ> laserCloudIn;
    pcl::fromROSMsg(*laserCloudMsg , laserCloudIn);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(laserCloudIn,laserCloudIn,indices);
    removeClosedPintCloud(laserCloudIn,laserCloudIn,MINIMUM_RANGE);

3）计算起始点的结束点的角度，由于激光雷达是顺时针旋转，这里取反就相当于顺时针

int cloudsize = laserCloudIn.points.size();
    float start0ri = -atan2(laserCloidIn.points[0].y,laserCloidIn.points[0].x);
    float endt0ri = -atan2(laserCloidIn.points[0].y,laserCloidIn.points[0].x)+2*M_PI;

4）遍历每一个点，计算俯仰角角判断是第几根scan,计算水平角，将scan分为两部分,这里主要是利用intensity，保存scan索引和相对起始时刻的时间；并将不同线束上的点云进行存储入数组laserCloudScans中 16*pointsize

 for (int i=0; i<cloudsize;i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;
        //计算俯仰角
        float angle = atan(pont.z/sqrt(point.x*point.x+point.y*point.y))*180/M_PI;
        int scanID = 0；
        //计算是第几根scan
        //计算水平角
      float ori = -atan(point.y,point.x);
      float relTime = (ori - start0ri)/(endt0ri -start0ri);
      //整数部分是scan的索引，小数部分是相对起始时刻的时间 scanPeriod=0.1
      point.intensity = scanID +scanPeriod - relTime;
      //根据scan的idx送入各自数组
      laserCloudScans[scanID].push_back(point);
    }

5）记录每个scan的开始和结束索引，抛弃开始和末尾的五个点，laserCloud为一帧的所有点云

cloudsize =count;
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    //全部集合到一个点云里面，但是使用两个数组标记起始和结束，
    for(int i=0;i<N_SCANS;++)
    {
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i]; //64*n点个数
        scanEndInd[i] = laserCloud->size() - 6;
    }

6)计算曲率cloudCurature

for(int i = 5; i < cloudsize-5; i++)
    {
        float diffx = laserCloud->points[i-5].x+laserCloud->points[i-4].x+laserCloud->points[i-3].x+laserCloud->points[i-2].x+laserCloud->points[i-1].x+
                10*laserCloud->point[i].x + laserCloud->point[i+1].x +laserCloud->point[i+2].x +laserCloud->point[i+3].x +laserCloud->point[i+4].x+laserCloud->point[i+5].x;
        float diffy = laserCloud->points[i-5].y+laserCloud->points[i-4].y+laserCloud->points[i-3].y+laserCloud->points[i-2].y+laserCloud->points[i-1].y
                10*laserCloud->point[i].y + laserCloud->point[i+1].y +laserCloud->point[i+2].y +laserCloud->point[i+3].y +laserCloud->point[i+4].y+laserCloud->point[i+5].y;
        float diffz = laserCloud->points[i-5].z+laserCloud->points[i-4].z+laserCloud->points[i-3].z+laserCloud->points[i-2].z+laserCloud->points[i-1].z+
                10*laserCloud->point[i].z + laserCloud->point[i+1].z +laserCloud->point[i+2].z +laserCloud->point[i+3].z +laserCloud->point[i+4].z+laserCloud->point[i+5].z;
        //存储曲率，索引
        cloudCurature[i] = diffx*diffx+diffy*diffy+diffz*diffz;
        cloudSortInd[i] = i;  //点云id
        cloudNeighborPicked[i] =0; //标志位
        cloudLable[i] = 0;
    }

7)特别角点，一般角点，特别面点，一般面点

pcl::PointClound<PointType> cornerPointsSharp;
pcl::PointClound<PointType> cornerPointsLessSharp;
pcl::PointClound<PointType> surfPointsflat;
pcl::PointClound<PointType> surfPointsLessflat;

8）将每个scan等分成6等份，对曲率进行排序，曲率大的点为角点，每段挑选2个曲率特别大和20个曲率稍大的，为了避免特征点过于集中，不选择选中的点的周围5个点；并比较它和周围点的距离是否相差过大，是否是边缘点；

int sp = scanStartInd[i] +(scanEndInt[i] - scanStartInt[i]) * j / 6;
int ep = scanStartInd[i] +(scanEndInt[i] - scanStartInt[i]) * (j+1) / 6 - 1;

TicToc t_tmp;0
//对点云按照曲率进行排序，从小到大
//它有三个参数sort(begin, end, cmp)，其中begin为指向待sort()的数组的第一个元素的指针，
//end为指向待sort()的数组的最后一个元素的下一个位置的指针，cmp参数为排序准则，cmp参数可以不写，
            std::sort(cloudSortInd+sp,cloudSortInd+ep+1,comp);   //依据的是序号cloudSortInd[i]的点的曲率从小到大
 TicToc t_tmp.toc();

 int lagersPickedNum = 0;
 //在mapping里面，也会对角点和面点做校验
 //挑选曲率比较大的部分  ,已经排好了  ep-> es
  for(int k=ep; k>=sp; k--)
  {
  //排序后顺序乱了，索引的作用体现出来
   int ind = cloudSortInd[k];
   //判断点是否有效，同时曲率是否大于阈值0.1
 if(cloudNeighborPicked[ind] == 0 && cloudCurature[ind] > 0.1)
 {
    lagersPickedNum ++;
    //每段取两个曲率大的点
     if (lagersPickedNum <=2)
     {
     //label为2是曲率大的标记
     cloudLable[ind] =2;
      //cornerPointsSharp存放曲率大的点
                        pubCornerPointsSharp.push_back(laserCloud->points[ind]);
                        pubCornerPointsLessSharps.push_back(lasercloud->points[ind])
                    }
     //以及20个曲率稍微大一点的点
else if(lagersPickedNum < =20)
  {
     //label置1表示曲率稍微大
      cloudLable [ind]=1;
                        pubCornerPointsLessSharps.push_back(lasercloud->points[ind])
  }
   else
   {
 break;
     }
       //被选中的点，pick标志位置为1
       cloudNeighborPicked[ind]=1;
       //为了避免特征点不过度集中，将选中的点周围5个点置为1，避免后续会选到
   for (int l=0; l <=5; i++)
    {
    //查看相邻点距离是否相差过大，如果差异过大说明点云在此不连续，是边缘特征，因此不置位
                        float diffx = lasercloud->points[ind+l].x - lasercloud->points[ind+l-1].x;
                        float diffy = lasercloud->points[ind+l].y - lasercloud->points[ind+l-1].y;
                        float diffz = lasercloud->points[ind+l].z - lasercloud->points[ind+l-1].z;
  if(diffx*diffx + diffy*diffy + diffz*diffz > 0.05)
   {
          break;
   }
    cloudNeighborPicked[ind+l]=1;
  }
    //as same
  for(int l=-1 ;l>= -5;l--)
    {
    //查看相邻点距离是否相差过大，如果差异过大说明点云在此不连续，是边缘特征，因此不置位
                        float diffx = lasercloud->points[ind+l].x - lasercloud->points[ind+l-1].x;
                        float diffy = lasercloud->points[ind+l].y - lasercloud->points[ind+l-1].y;
                        float diffz = lasercloud->points[ind+l].z - lasercloud->points[ind+l-1].z;
                        if(diffx*diffx + diffy*diffy + diffz*diffz > 0.05)
   {
     break;
   }
    cloudNeighborPicked[ind+l]=1;
  }
 }

9）同理，挑选面点，曲率小的点，4个面点，并将其他点都认为是比较平坦的点，并只对其进行体素网格滤波

10）发布当前点云消息和四类特征点

pcl::toROSMsg(*lasercloud , laserCloutOutMsg);

pcl::toROSMsg(*cornerPointsSharp , cornerPointsSharpMsg); *

*pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);

pcl::toROSMsg(*surfPointsFlat , surfPointsFlat2);

pcl::toROSMsg(*surfPointsLessFlat , surfPointsLessFlat2);

4、定义需要发布的消息

pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyn_points",100);
    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_sharp",100);
    pubCornerPointsLessSharps = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_less)sharp",100);
    pubSurPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_flat",100);
    pubSurPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_flat_less",100);
    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("laser_remove_points",100);