A-LOAM源码详解（一）scanRegistration.cpp

 文章主要是对A-LOAM源码的注释，在理解代码时，主要参考了以下两篇文章
 A-LOAM源码详解（一）：scanRegistration.cpp] https://zhuanlan.zhihu.com/p/398149559
 LOAM笔记] https://www.cnblogs.com/wellp/p/8877990.html
 代码如下，有点长

#include <cmath>
#include <iostream>
#include <vector>
#include <string>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

using std::atan2;
using std::cos;
using std::sin;

const double scanPeriod = 0.1;

const int systemDelay = 0; 
int systemInitCount = 0;
bool systemInited = false;
int N_SCANS = 0;
float cloudCurvature[400000];  //保存单帧16线扫描的点云的曲率
int cloudSortInd[400000];
int cloudNeighborPicked[400000];  //用于标记已处理的点
int cloudLabel[400000];           //赋予点云特征标签，用于将点云特征分类  

//如果点i的曲率小于j 则返回true
bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }

ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubRemovePoints;
std::vector<ros::Publisher> pubEachScan;

bool PUB_EACH_LINE = false;

double MINIMUM_RANGE = 0.1; 

template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0; 
    //size_t在32位架构上是4字节，在64位架构上是8字节，在不同架构上进行编译时需要注意这个问题。
    //而int在不同架构下都是4字节，与size_t不同；且int为带符号数，size_t为无符号数。
    //与int固定四个字节有所不同,size_t的取值range是目标平台下最大可能的数组尺寸,
    //一些平台下size_t的范围小于int的正数范围,又或者大于unsigned int. 使用Int既有可能浪费，又有可能范围不够大。
    
    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {   
        //去除以激光雷达为中心原点，半径thres以内的点
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;  //跳出当前循环，即执行到这个语句，后面的都不执行了，强制开始下一次循环，该语句只能在循环中使用
        cloud_out.points[j] = cloud_in.points[i];
        j++;  //对滤除后的点进行计数
    }
    if (j != cloud_in.points.size()) 
    {
        cloud_out.points.resize(j); //如果有点被滤除掉，则对输出点云进行resize
    }

    cloud_out.height = 1;   //如果cloud是无序的 height是1
    cloud_out.width = static_cast<uint32_t>(j);  //14241左右 16线数据
    //printf("cloud_out_width:%d\n",cloud_out.width);
    cloud_out.is_dense = true;
}

//对获取的每一帧点云数据进行处理
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    if (!systemInited)
    { 
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

    TicToc t_whole;     
    TicToc t_prepare;  
    //对计算时间间隔的类初始化
    //vector是一个封装了动态大小数组的顺序容器
    std::vector<int> scanStartInd(N_SCANS, 0);   //长度为N_SCANS
    std::vector<int> scanEndInd(N_SCANS, 0);
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    //将ros系统中接受到的LaserCloudMsg转为LaserCloudIn作为A-LOAM的点云输入 其包含的数据是相同的，只是格式不同，便于处理
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);  
    std::vector<int> indices;
    //首先对点云滤波，去除NaN值的无效点，以及在Lidar坐标系原点MINIMUM_RANGE距离以内的点
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);


    int cloudSize = laserCloudIn.points.size();  //点云数目 与刚才的cloud_out.width相同
    // printf("cloudSize is %d \n",cloudSize);
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    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;
    }
    //printf("end Ori %f\n", endOri);
    //printf("atan30 is :%.1f \n",float(atan(-1/2)));
    bool halfPassed = false;
    int count = cloudSize;
    PointType point;  //typedef pcl::PointXYZI PointType;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); //定义一个长度为16的点云向量
    //这个for循环，主要实现对无序点云中每个点所属的scadid(线数)划分、同线数内，点位置的确定
    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;
        
        if (N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);  //确定当前点的scanid 0-15
            //printf("scanid is %d \n",scanID);
            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);

        float ori = -atan2(point.y, point.x);  //对每一个点都求一次y/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;
            }

            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;
            }
        }

        //归一化 每个点的夹角比上扫描一圈的整个夹角 用realtime给每个点分配fireid
        float relTime = (ori - startOri) / (endOri - startOri); 
        //小数部分 扫描周期*点所在位置  得到在一圈扫描中扫描到该点时的时间  intensity  强度
        point.intensity = scanID + scanPeriod * relTime; // intensity字段的整数部分存放scanID，小数部分存放归一化后的fireID
        laserCloudScans[scanID].push_back(point); // 将点根据scanID放到对应的子点云laserCloudScans中
    }
    
    cloudSize = count;
    printf("points size %d \n", cloudSize);

    //一帧点云 有序点云 按照scanID和fireid进行顺序存放
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    //将子点云laserCloudScans合成一帧点云laserCloud 这里laserCloud也可以认为是有序点云按照scanid fireid的顺序存放
    for (int i = 0; i < N_SCANS; i++)
    { 
        scanStartInd[i] = laserCloud->size() + 5;  //记录每个scan开始的index 忽略前5个点
        *laserCloud += laserCloudScans[i];
        //记录每个scan结束的index,忽略后5个点，开始和结束处的点云容易产生不闭合的“接缝”对提取edge feature不利
        scanEndInd[i] = laserCloud->size() - 6;   
    }
    // printf("scanStartInd %d \n",scanStartInd[0]);   5
    // printf("scanEndInd %d \n",scanEndInd[0]);       900左右
    // printf("laserCloudScans %d \n",laserCloudScans[0]);    
    printf("prepare time %f \n", t_prepare.toc());

    //计算点的曲率
    for (int i = 5; i < cloudSize - 5; i++)   //cloudSize 是16条线的点数  900*16 =14400 左右
    {   
        //从第六个点开始，计算曲率时，选取前后5个点进行计算  点的编号从0开始
        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->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[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->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[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->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
        //printf("diffX %f \n",laserCloud->points[i - 5].x);
        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;   //curvature 曲率
        cloudSortInd[i] = i;  //一轮循环之后变成cloudSize - 5
        cloudNeighborPicked[i] = 0;  //点有没有被选择为feature点
        cloudLabel[i] = 0;  // Label 2: corner_sharp 
                            // Label 1: corner_less_sharp  包含label 2
                            // Label -1: surf_flat
                            // Label 0: surf_less_flat 包含label -1 因为点太多，最后会降采样
    }


    TicToc t_pts;

    pcl::PointCloud<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> surfPointsLessFlat;

    printf("cloudSortInd  %d %d %d %d %d\n",cloudSortInd,&cloudSortInd[0],cloudSortInd[2],cloudSortInd[3],cloudSortInd[5]);
    //输出结果 cloudSortInd  -638208576 -638208576 0 0 5   
    //直接用cloudSortInd打印出的是数组的首地址对应于cloudSortInd[0] 通过地址来索引数据
    float t_q_sort = 0;
    //for循环  根据曲率计算四种特征点
    for (int i = 0; i < N_SCANS; i++)  //按照scan的顺序 即线束的顺序 提取4种特征点
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)  //如果该scan的点少于7个就跳过
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)  //该scan分成6小段 执行特征检测
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;      
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
            //假设scanStartInd[0]=5  scanEndInd[0]=900
            //sp 0    154  303    subscan的起始index
            //ep 153  302  ....   subscan的结束index

            TicToc t_tmp;
            //根据曲率由小到大对subscan的点进行sort sort是排序函数  每一线都得到6个由小到大的点集
            //cloudSortInd为数组首元素地址，该函数表示在内存块 cloudSortInd+sp 到 cloudSortInd + ep + 1
            //通过比较曲率之间的大小改变对应内存块储存的曲率值实现排序
            //bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
            //使用cloudSortInd是使用地址来索引求曲率的点
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); 
            t_q_sort += t_tmp.toc();

            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--) //从后往前，即从曲率大的点开始提取corner feature
            {
                int ind = cloudSortInd[k];   //ind 从5-14155左右 为曲率最大的点的索引
                //printf("ind %d \n",ind);
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)  //如果该点没有被选择，且曲率大于0.1
                {

                    largestPickedNum++;
                    if (largestPickedNum <= 2)//该subscan中曲率最大的前2个点认为是corner_sharp的特征点
                    {                        
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]); //push_back 在末尾行添加元素
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)//曲率中大于0.1的前20个点被认为是corner_less_sharp的特征点
                    {                        
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;  //标记该点被选择过了

                    //与当前点距离的平方 <=0.05的点标记为选择过，避免特征点密集分布
                    for (int l = 1; l <= 5; l++)  // 参数5的选择
                    {
                        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;
                    }
                    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;
                    }
                }
            }

            //提取surfer平面feature 与上述类似，选取该subscan曲率最小的前4个点为surf_flat
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSortInd[k];

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {

                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)
                    { 
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    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;
                    }
                    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;
                    }
                }
            }

            //其他的非corner特征点与surf_flat特征点一起组成surf_flat_less特征点
            //eg: corner_sharp:192 corner_less_sharp:1763 surf_flat:375 surf_less_flat:6944 来源于对应的发布数据topic的width值
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

        //最后对该scan点云中提取的所有surf_less_flat点进行降采样
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;  //体素网格滤波
        //surfPointsLessFlatScan存放surf_less_flat的特征点
        downSizeFilter.setInputCloud(surfPointsLessFlatScan); 
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);  //设置体素大小
        downSizeFilter.filter(surfPointsLessFlatScanDS);  //执行滤波

        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }
    printf("sort q time %f \n", t_q_sort);
    printf("seperate points time %f \n", t_pts.toc());

    //发布滤波后的当前帧点云数据
    //将四种特征点云转为ros格式的点云
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    //laserCloudMsg 从激光雷达得到的点云数据 stamp的作用 --> 和原始点云数据的stamp保持一致
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 
    laserCloudOutMsg.header.frame_id = "/camera_init";
    pubLaserCloud.publish(laserCloudOutMsg);

    //发布corner_sharp特征点数据
    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    //发布corner_less_sharp特征点数据
    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    //发布surf_flat特征点数据
    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "/camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    //发布surf_less_flat特征点数据
    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "/camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

    // pub each scam
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i< N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            pcl::toROSMsg(laserCloudScans[i], scanMsg);
            scanMsg.header.stamp = laserCloudMsg->header.stamp;
            scanMsg.header.frame_id = "/camera_init";
            pubEachScan[i].publish(scanMsg);
        }
    }

    printf("scan registration time %f ms *************\n", t_whole.toc());
    if(t_whole.toc() > 100)
        ROS_WARN("scan registration process over 100ms");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");   //注册节点
    ros::NodeHandle nh;

    nh.param<int>("scan_line", N_SCANS, 16);    //参数定义 雷达线数

    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);   //定义最小有效距离

    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;
    }

    //话题订阅 首先实例化对象  参数依次为 消息类型  topic  消息队列 回调函数
    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);

    if(PUB_EACH_LINE)
    {
        for(int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
            pubEachScan.push_back(tmp);
        }
    }
    ros::spin();

    return 0;
}