A-LOAM——scanRegistration.cpp

scanRegistration.cpp

参考了网上多篇文章，给aloam代码加了详尽的注释

removeClosedPointCloud

该函数去除半径thres内的点，以此来保证留下的点不在激光雷达附近，再对保留下的点进行resize。

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) //对输入进来的每个点{
    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];       //遍历输入点云cloud_in.point[]，如果当前点的x^2+y^2+z^2 >= thres^2, 保存到cloud_out.points[]，否则去除//即：去除以激光雷达坐标原点半径为thres内的球体里的点j++;}if (j != cloud_in.points.size()) //如果上面确实发现有被剔除的点，则对输出点云cloud_out.points进行resize(j){
    cloud_out.points.resize(j);}cloud_out.height = 1; //如果cloud 是无序的 height 是 1cloud_out.width = static_cast<uint32_t>(j); //点云的长度；static_cast强制将j转换为uint32_t类型，不可能为负数cloud_out.is_dense = true;
}

laserCloudHandler

? 该函数为接收到激光雷达点云消息的回调函数。该函数的参数为激光雷达的点云消息，sensor_msgs::PointCloud2ConstPtr型的引用法输入 &laserCloudMsg。
? 函数是订阅的点云句柄，在其中进行了格式转换、1.中函数的调用并进行曲率求解，特征分割，并准备消息发布工作。

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
//每接收到一帧点云就执行一次laserCloudHandler
{
    // 作用就是延时，为了确保有IMU数据后, 再进行点云数据的处理if (!systemInited)   {
     systemInitCount++;if (systemInitCount >= systemDelay){
    systemInited = true;}elsereturn;}TicToc t_whole;TicToc t_prepare;//每条扫描线上的可以计算曲率的点云点的起始索引和结束索引std::vector<int> scanStartInd(N_SCANS, 0);  //长度为N_SCANS，值全为0std::vector<int> scanEndInd(N_SCANS, 0);pcl::PointCloud<pcl::PointXYZ> laserCloudIn;pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); //将ROS系统接收到的laserCloudMsg转为laserCloudIn点云作为A-LOAM的输入点云std::vector<int> indices;//首先对该点云进行滤波，去除NaN值的无效点，以及在Lidar坐标系原点MINIMUM_RANGE距离以内的点pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);  //调用removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, float thres) ，剔除半径10cm内点int cloudSize = laserCloudIn.points.size();  //点云数目//atan2( )默认返回逆时针角度，由于LiDAR通常是顺时针扫描，所以往往使用-atan2( )函数。float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); // double atan2(double y, double x), 返回以弧度表示的 y/x 的反正切float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) +  2 * M_PI;  //通过上面的公式，endOri - startOri理应在pi~3pi之间//正常情况下在这个范围内：pi < endOri - startOri < 3*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);bool halfPassed = false;int count = cloudSize;PointType point;  //typedef pcl::PointXYZI PointType;std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); //定义一个长度为16的点云向量laserCloudScansfor (int i = 0; i < cloudSize; i++){
    //对于每一个点云point.x = laserCloudIn.points[i].x; //pcl::PointXYZI point;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;//针对不同线数的雷达，确定该点的scanID。if (N_SCANS == 16){
    scanID = int((angle + 15) / 2 + 0.5);   //确定当前点的scanID：scanID[0~16]if (scanID > (N_SCANS - 1) || scanID < 0){
     //如果scanID超出了范围，则舍弃该点count--;   //count=CloudSizecontinue;}}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);elsescanID = 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;}}//relTime 是一个0~1之间的小数，代表占用扫描时间的比例，乘以扫描时间得到真实扫描时刻。//scanPeriod扫描时间默认为0.1s。float relTime = (ori - startOri) / (endOri - startOri);  //得到一个relTime用来给每个点分配fireID//假设16线Lidar的水平角度分辨率为0.2°（实际是0.1~0.4），则每个scan理论上有360/0.2=1800个点，为方便叙述，称每个scan点的ID为fireID，即fireID[0~1799]，相应的scanID[0~16]//通过Lidar坐标系下的仰角和水平夹角计算点云的scanId和fireIDpoint.intensity = scanID + scanPeriod * relTime;  //intensity的整数部分存放scanID，小数部分存放归一化后的fireIDlaserCloudScans[scanID].push_back(point);      //将点根据scanID放到对应的子点云laserCloudScans中}cloudSize = count;printf("points size %d \n", cloudSize);pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>()); //定义一个laserCloud，将合并后的laserCloudScans存进来//计算曲率//将子点云laserCloudScans合并成一帧点云laserCloud，注意这里的单帧点云laserCloud可以认为已经是有序点云了，按照scanID和fireID的顺序存放for (int i = 0; i < N_SCANS; i++){
     scanStartInd[i] = laserCloud->size() + 5;   //记录每个scan的开始index，忽略前五个点*laserCloud += laserCloudScans[i];   //把scanID[i]加入到laserCloud中scanEndInd[i] = laserCloud->size() - 6; //记录每个scan的结束index，忽略后五个点，开始和结束处的点云容易产生步闭合的"接缝"，对提取edge_feature不利}printf("prepare time %f \n", t_prepare.toc());// 将一帧无序点云转换成有序点云消耗的时间//计算点的曲率for (int i = 5; i < cloudSize - 5; i++){
     //从第6个点开始，当前点的曲率由该点及其前后五个点的对应坐标之和的平方和表示。与论文不同的是没有进行除法。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;cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;//cloudSortInd[i] = i相当于所有点的初始自然序列，从此以后，每个点就有了它自己的序号（索引）cloudSortInd[i] = i;   //一轮循环后变为cloudSize - 6cloudNeighborPicked[i] = 0; //标记点i是否已经被选择过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，因为点太多，最后会降采样//先默认每个点都是surf_less_flat}TicToc t_pts;//定义四种特征点云pcl::PointCloud<PointType> cornerPointsSharp; //极大边缘点pcl::PointCloud<PointType> cornerPointsLessSharp; //次极大边缘点pcl::PointCloud<PointType> surfPointsFlat; //极小平面点pcl::PointCloud<PointType> surfPointsLessFlat;//次极小平面点，经过降采样防止特征点聚堆float t_q_sort = 0;// 用来记录排序花费的总时间/*1、为了提高效率，每条扫描线分成6个扇区，在每个扇区内，寻找曲率最大的20个点，作为次极大边线点，其中最大的2个点，同时作为极大边线点；寻找曲率最小的4个点，作为极小平面点，剩下未被标记的点，全部作为次极小平面点。2、为了防止特征点聚堆，每提取一个特征点（极大/次极大边线点，极小平面点），都要将这个点和它附近的点全都标记为“已选中”，在下次提取特征点时，将会跳过这些点。对于次极小平面点，采取降采样的方法避免聚堆。*///根据曲率计算四种特征点：边缘点特征：sharp、less_sharp；面特征：flat、less_flatfor (int i = 0; i < N_SCANS; i++) //按照scan的顺序提取4种特征点{
    if( scanEndInd[i] - scanStartInd[i] < 6) //如果该scan的点数小于7个点，就不能分为6个扇区，跳过；第i线的扫描末端索引 — 扫描开始索引continue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>); //定义surfPointsLessFlatScan储存该扫描线上的less_flat点云// 为了使特征点均匀分布，将一个scan分成6个subcansfor (int j = 0; j < 6; j++) //将第i个scan分成6小段subscan执行特征检测{
    int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;  // subscan的起始index，j=0,sp=scanStartInd[i]; j=1, sp=scanStartInd[i]+第i线的scan点数的1/6int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; //subscan的结束indexTicToc t_tmp;std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); //根据曲率由小到大对subscan的点进行sort; 对每一线都得到了6个曲率由小到大的点集//bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); //cloudSortInd为数组首元素地址，该函数表示在内存块 cloudSortInd+sp 到 cloudSortInd+ep+1内，通过比较曲率之间的大小改变对应内存块储存的曲率值实现排序t_q_sort += t_tmp.toc();// t_q_sort累计每个扇区曲率排序时间总和// 选取极大边线点（2个）和次极大边线点（20个）int largestPickedNum = 0;//提取corner_sharp的feature，选择该subscan曲率最大的前2个点为corner_sharp，曲率最大的前20个点认为是corner_less_sharpfor (int k = ep; k >= sp; k--)      //从曲率大的点开始提取corner feature，此时sp到ep内对应的曲率由小到大{
    int ind = cloudSortInd[k]; //初始k=ep，ind为曲率最大的点的索引if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1)  //如果该点没有被选择过，并且曲率大于0.1{
    largestPickedNum++;if (largestPickedNum <= 2) //该subscan中曲率最大的前2个点认为是Corner_sharp特征点{
                            cloudLabel[ind] = 2;    //Label 2：corner_sharpcornerPointsSharp.push_back(laserCloud->points[ind]);//选取该点为极大边缘点cornerPointsLessSharp.push_back(laserCloud->points[ind]); //也将这两个corner_sharp点认为是次极大边缘点}else if (largestPickedNum <= 20) //该subscan中曲率最大的前20个点认为是corner_less_sharp特征点{
                            cloudLabel[ind] = 1;  // Label 1：corner_less_sharpcornerPointsLessSharp.push_back(laserCloud->points[ind]); //只选取该点为次极大边缘点}else{
    break;}cloudNeighborPicked[ind] = 1;  //标记该点被选择过//与当前点簇距离的平方小于等于0.05的点标记为选择过，避免特征点密集分布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;}}}//提取surf_flat的feature，选择该subscan曲率最小的前4个点为surf_flatint smallestPickedNum = 0;for (int k = sp; k <= ep; k++)  //从曲率小的点开始提取surf_flat特征，此时sp到ep内对应的曲率由小到大{
    int ind = cloudSortInd[k];  //ind对应曲率最小的点的索引if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1)  //如果ind对应的点未被选中，且曲率小于0.1{
    cloudLabel[ind] = -1;   // Label -1：surf_flatsurfPointsFlat.push_back(laserCloud->points[ind]);  //将ind对应点加入surf_flatsmallestPickedNum++;if (smallestPickedNum >= 4)     //选择曲率最小的前4个点为surf_flat{
     break;//跳出循环层}cloudNeighborPicked[ind] = 1; //将该点标记为已选//与当前点距离的平方小于等于0.05的点标记为选择过，避免特征点密集分布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;}}}//目前选出了曲率最大的前20个点认为是corner_less_sharp(其中前两个为corner_sharp)，选出了曲率最小的4个点认为是surf_flat//其他的非corner特征点与surf_flat特征点共同组成surf_less_flat特征点for (int k = sp; k <= ep; k++){
    if (cloudLabel[k] <= 0) // Label -1：surf_flat{
     // Label 0：surf_less_flat，包含Label -1，因为点太多，最后会降采样 //最初将所有点都标记为0了，见 cloudLabel[i] = 0;surfPointsLessFlatScan->push_back(laserCloud->points[k]); //包含了极小平面点}}}//最后对该scan点云中提取的所有surf_less_flat特征点进行降采样pcl::PointCloud<PointType> surfPointsLessFlatScanDS;pcl::VoxelGrid<PointType> downSizeFilter; //体素网格滤波downSizeFilter.setInputCloud(surfPointsLessFlatScan); //surfPointsLessFlatScan中存着surf_less_flat特征点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格式的点云//将当前帧点云提取出的4种特征点与滤波后的当前帧点云进行publishsensor_msgs::PointCloud2 laserCloudOutMsg;pcl::toROSMsg(*laserCloud, laserCloudOutMsg); laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; //时间戳保持不变laserCloudOutMsg.header.frame_id = "/camera_init"; //坐标系pubLaserCloud.publish(laserCloudOutMsg);sensor_msgs::PointCloud2 cornerPointsSharpMsg;pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsSharpMsg.header.frame_id = "/camera_init";pubCornerPointsSharp.publish(cornerPointsSharpMsg);sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsLessSharpMsg.header.frame_id = "/camera_init";pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);sensor_msgs::PointCloud2 surfPointsFlat2;pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsFlat2.header.frame_id = "/camera_init";pubSurfPointsFlat.publish(surfPointsFlat2);sensor_msgs::PointCloud2 surfPointsLessFlat2;pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsLessFlat2.header.frame_id = "/camera_init";pubSurfPointsLessFlat.publish(surfPointsLessFlat2);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");
}

main

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");ros::NodeHandle nh;// 设置激光雷达的扫描线数量，预设可以使用16线/32线/64线nh.param<int>("scan_line", N_SCANS, 16);// 设置点云点与激光雷达的最近距离MINIMUM_RANGE，小于MINIMUM_RANGE的点将被滤除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;}// 订阅初始的激光雷达数据，并执行回调函数laserCloudHandlerros::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);// 如果PUB_EACH_LINE == true ,则发布每条扫描线上的点云点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;
}