A-LOAM代码的结构

A-LOAM的cpp有四个,其中 kittiHelper.cpp 的作用是将kitti数据集转为rosbag
剩下的三个是作为 slam 的 部分,分别是:

• laserMappin.cpp ++++ 当前帧到地图的优化
• laserOdometry.cpp ++++ 帧间里程计
• scanRegistration.cpp ++++ 前端lidar点预处理及特征提取

本片主要解读 前端lidar点预处理部分的代码

Code

int main(int argc, char **argv)
{
    //节点 名称:scanRegistration
    ros::init(argc, argv, "scanRegistration");
    ros::NodeHandle nh;//ros 句柄   

    //从配置参数中 读取 scan_line 参数, 多少线的激光雷达  在launch文件中配置的
    nh.param<int>("scan_line", N_SCANS, 16);

    //从配置参数中 读取 minimum_range 参数, 最小有效距离  在launch文件中配置的   踢出雷达上的载体出现在视野里的影响
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);

从main函数开始

首先是ros的基本操作,初始化节点和 声明句柄

然后从参数服务器中读取两个参数

• scan_line 多少线的激光雷达 在launch文件中配置的
• minimum_range 最小有效距离 在launch文件中配置的 踢出雷达上的载体出现在视野里的影响

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //打印线束
    printf("scan line number %d \n", N_SCANS);

    //做一个线束的判断 
    if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
    {
        printf("only support velodyne with 16, 32 or 64 scan line!");
        return 0;
    }

做一个线束的判断
目前仅支持 16线 32线 64线的 机械式lidar
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //订阅 velodyne 的 lidar消息 收到一个消息包 则进入 laserCloudHandler 回调函数一次
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);

    //定义要发布的消息
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);

    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);

    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);

    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);

    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);

    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);

本节点订阅和发布的消息
从这里可以看出来,这个节点是接收lidar的原始数据

• “/velodyne_points”

然后发布处理后的数据

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

下面来看这个节点的主要工作,接收到lidar数据后的处理,和特征提取部分,在回调函数laserCloudHandler 中

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    // 判断系统是否进行初始化, 如果没有 则缓冲几帧 目前 systemDelay为0,自己用可以设置
    if (!systemInited)
    { 
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

首先是进行一个初始化的判断,前几帧可以不要
源码的systemDelay为0
实际使用的时候可以设置个大小
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //将ros点云转为pcl点云格式
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;//声明pcl点云
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);//将ros点云转为pcl点云格式

将ros点云转为pcl点云格式

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    std::vector<int> indices;//这个变量保存了下面去除nan点的序号
    //去除点云中的nan点
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    //去除点云中的距离小于阈值的点  
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

去除点云中的nan点
去除点云中的距离小于阈值的点
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //计算起始点和结束点的水平角度,与x轴的夹角,由于激光雷达是顺时针旋转,这里取反就相当于转成了逆时针
    int cloudSize = laserCloudIn.points.size();

    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    //atan2的范围是 [-Pi,Pi] ,这里加上2Pi是为了保证起始到结束相差2PI,符合实际
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;

    // 总会有一些例外, 转换到合理的范围内
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }

计算起始点和结束点的水平角度,与x轴的夹角
为了给后面计算相对起点的时间戳用的

这里要说下 机械式lidar的坐标系

所以在求与x轴的夹角的时候是 arctan(y/x)
结束点的水平角加上了2pi,主要的目的是 将角的范围转为 0~360度,因为结束点大部分为负值,比如起点,30度,结束点为-90,转完即为[30,270]

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //遍历每一个点
    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(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;//线束的id
        //计算是第几个线束 scan
        if (N_SCANS == 16)
        {
            //垂直是30度  每个线之间的夹角是2度
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);

遍历每一个点
通过计算的俯仰角度 来判断 这个点 在哪个线的scan上面

float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
计算俯仰角的代码就是根据上面这个图

scanID = int((angle + 15) / 2 + 0.5);
根据俯仰角度求对应的线束就是 根据 上面这图 ，从最下面的线束算1，然后大约每2°，一根线，所以是 (angle + 15) / 2。最后加的0.5是为了进一位，因为是从1开始数的嘛

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        //计算水平角
        float ori = -atan2(point.y, point.x);
        /* 把计算的水平角 放到 开始角度和结束角度 合理 的区间之内  */
        if (!halfPassed)
        { 
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)//这种情况不会发生
            {
                ori -= 2 * M_PI;
            }
            //如果超过了180,就说明过一半了
            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

计算水平角
主要有 -pi 到 pi 的区间, 分成两个半圆算的,
主要就是保证把计算的水平角 放到 开始角度和结束角度 合理 的区间之内
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        //角度的计算是为了计算相对的起始时刻的时间  为点云畸变补偿使用
        float relTime = (ori - startOri) / (endOri - startOri);//计算当前点 在起始和结束之间的比率
        //整数部分是sacn的id ,小数部分是相对起始时刻的时间
        point.intensity = scanID + scanPeriod * relTime;
        //根据scan的ID存入各自数组
        laserCloudScans[scanID].push_back(point); 
    }

角度的计算是为了计算相对的起始时刻的时间 为点云畸变补偿使用
计算当前点 在起始和结束之间的比率
整数部分是sacn的id ,小数部分是相对起始时刻的时间
根据scan的ID存入各自数组

以上从回调函数开始 整个 很多行的代码 就在 完成一个功能,求点 相对的起始时刻的时间

现在有的lidar是把每个点的时间戳自带了
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //当前有效的点云的数目
    cloudSize = count;

    printf("points size %d \n", cloudSize);

    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());//缓存叠加的每条scan的点云

    //处理每个scan  每条scan上面的 点的起始 id 为 前5个点不要  结束的id 为后6个点不要
    for (int i = 0; i < N_SCANS; i++)
    { 
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;
    }

处理每个scan 每条scan上面的 点的起始 id 为 前5个点不要 结束的id 为后6个点不要
点很多的,这10个点无所谓,主要是求曲率方便

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

以上为A-LOAM中点的预处理部分代码内容