这个文件主要分为3个函数，
一个点云滤除函数，主要是对距离过近的点进行滤除
第二个是特征提取函数，分别提取了极大边线点，次极大边线点，极小平面点，次极小平面点四个特征
第三个就是main函数了。
参考一些前人的工作，自己做了总结，进行学习。
#include
#include
#include
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using std::atan2;
using std::cos;
using std::sin;
const double scanPeriod = 0.1;//扫描周期？？
const int systemDelay = 0; // 系统接收雷达的延迟，接收到的雷达帧数量 > systemDelay，才初始化成功
int systemInitCount = 0;// 接收到的雷达帧的数量通过这个变量表示
bool systemInited = false;// systemInitCount > systemDelay后，systemInited = true
int N_SCANS = 0;
float cloudCurvature[400000];// 每个点的曲率大小,全局数组的大小要足够容纳一帧点云
int cloudSortInd[400000];// 存储按照点的曲率排好序的特征点的ID
int cloudNeighborPicked[400000];// 避免特征点密集分布，每选一个特征点其前后5个点**一般**就不选取了
int cloudLabel[400000];// 记录特征点属于那种类型：极大边线点、次极大边线点、极小平面点、次极小平面点
bool comp (int i,int j) { return (cloudCurvature[i]有序的处理）
ros::Publisher pubCornerPointsSharp;// 发布极大边线点
ros::Publisher pubCornerPointsLessSharp;// 发布次极大边线点
ros::Publisher pubSurfPointsFlat;// 发布极小平面点
ros::Publisher pubSurfPointsLessFlat;// 发布次极小平面点
ros::Publisher pubRemovePoints;//发布移除的点云，没有用
std::vector pubEachScan;// 发布雷达的每条线扫
bool PUB_EACH_LINE = false;//按每条线发布设为false
double MINIMUM_RANGE = 0.1;
template
//removeClosedPointCloud函数的作用是对距离小于阈值的点云进行滤除
void removeClosedPointCloud(const pcl::PointCloud &cloud_in,
pcl::PointCloud &cloud_out, float thres)//thres，激光点距离雷达中心的距离小于这个阈值就被去掉
{
// 假如输入输出点云不使用同一个变量，则需要将输出点云的时间戳和容器大小与输入点云同步
if (&cloud_in != &cloud_out)
{
cloud_out.header = cloud_in.header;//时间戳同步
cloud_out.points.resize(cloud_in.points.size());//容器大小同步
}
size_t j = 0;
// 把点云距离小于给定阈值的去除掉
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];
j++;
}
if (j != cloud_in.points.size())// 重新调整输出容器大小
{
cloud_out.points.resize(j);
}
// 这里是对每条扫描线上的点云进行直通滤波，因此设置点云的高度为1，宽度为数量，稠密点云
cloud_out.height = 1;
cloud_out.width = static_cast(j);
cloud_out.is_dense = true;
}
//订阅雷达消息的回调函数，包括前端雷达数据处理，特征提取
//订阅雷达消息
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
if (!systemInited) // 判断系统是否进行初始化, 如果没有 则缓冲几帧 目前 systemDelay为0,自己用可以设置
//进行一个初始化的判断,前几帧可以不要
//源码的systemDelay为0
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//作者自己设计的计时类，以构造函数为起始时间，以toc()函数为终止时间，并返回时间间隔(ms)
TicToc t_whole;//计算整个回调函数的时间
TicToc t_prepare; //计算雷达点云有序化的时间
//定义每条雷达扫描线上可以计算曲率的点云，
std::vector scanStartInd(N_SCANS, 0); //点的起始索引
std::vector scanEndInd(N_SCANS, 0); //点的结束索引
//将ros点云转为pcl点云格式
pcl::PointCloud laserCloudIn; //声明pcl点云laserCloudIn
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); //将ros点云转为pcl点云格式
std::vector indices;//声明一个变量，用于保存下面去除nan点的序号
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); //调用pcl库函数 pcl::removeNaNFromPointCloud 去掉点云中的无效点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); //去除点云中距离小于阈值的点，这里调用作者写的函数
//接下来，计算点云角度范围，要使点云有序，就要为每个点找到其所对应的扫描线，为每条扫瞄线上的点分配时间戳。
//要计算每个点的时间戳，首先要确定这个点的角度范围
//这块在后面的激光框架基本都不需要了，因为后面雷达的驱动将每个点的线号、角度、时间戳都发出来了。
所以说这一块可以跳过不看。
//计算起始点和结束点的水平角度,与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;//结束点角度
//atan2()函数是atan(y， x)函数的增强版，不仅可以求取arctran(y/x)还能够确定象限
// startOri和endOri分别为起始点和终止点的方位角
//把一些不正确的转换到合理范围内
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//为点云点找到对应的扫描线，每条扫描线都有它固定的俯仰角，我们可以根据点云点的垂直角度为其寻找对应的扫描线。
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector> laserCloudScans(N_SCANS);//定义每条线的点云数组
//遍历每一个点
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 scanID
if (N_SCANS == 16)
{
//如果是16线激光雷达，结算出的angle应该在-15~15之间，+-15°的垂直视场，垂直角度分辨率2°，则-15°时的scanID = 0。
scanID = int((angle + 15) / 2 + 0.5); //计算每个点对应的线束
// scanID(0~15), 这些是无效的点，cloudSize--
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32) //32线激光雷达
{
// 垂直视场角+10.67~-30.67°，垂直角度分辨率1.33°，-30.67°时的scanID = 0
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)//64线激光雷达
{
// 垂直视场角+2~-24.8°，垂直角度分辨率0.4254°，+2°时的scanID = 0
if (angle >= -8.83)// scanID的范围是 [0,32]
scanID = int((2 - angle) * 3.0 + 0.5);
else// scanID的范围是 [33,]
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();
}
// 每个sweep不一定从水平0°开始，没有复位过程，雷达只在电机旋转到接近0°时记下当前对应激光点的精确坐标；
// 同样，结束的位置，也不一定是0°，都是在0°附近，这为只使用激光点坐标计算水平角度带来一定的复杂性；
// 若起始角度是0°，结束角度是10°+2*pi，若只通过坐标来计算水平角度，如果得到的角度是5°，那么这个激光点是
// 开始的时候（5°）扫描的，还是结束的时候（2*pi+5°）时扫描的？
// 所以只使用XYZ位置有时候无法得到其精确的扫描时间，还需要结合时序信息，因为一个sweep中返回的点是按照
// 时间的顺序排列的。
// 这里变量halfPassed就是来解决这个问题的
//计算水平角
//主要有 -pi 到 pi 的区间, 分成两个半圆算的,
//主要就是保证把计算的水平角 放到 开始角度和结束角度 合理 的区间之内
float ori = -atan2(point.y, point.x);
//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿，如果此时扫描没有过半，则halfPassed为false
if (!halfPassed)
{
// 确保-PI / 2 < ori - startOri < 3 / 2 * PI， 如果ori-startOri小于-0.5pi或大于1.5pi，则调整ori的角度
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
//扫描点过半则设定halfPassed为true，如果超过180度，就说明过了一半了
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
// 确保-PI * 3 / 2 < ori - endOri < PI / 2
ori += 2 * M_PI;// 先补偿2PI
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);//计算当前点 在起始和结束之间的比率
point.intensity = scanID + scanPeriod * relTime;// 整数部分是scan线束的索引，小数部分是相对起始时刻的时间
laserCloudScans[scanID].push_back(point); // 根据每条线的idx送入各自数组，表示这一条扫描线上的点
}
cloudSize = count;// cloudSize是有效的点云的数目
printf("points size %d \n", cloudSize);
// 将所有scan点拼接到laserCloud中，scan线扫是有顺序的存入laserCloud中的
pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());//缓存叠加的每条scan的点云
for (int i = 0; i < N_SCANS; i++)
{
//处理每个scan 每条scan上面的 点的起始 id 前5个点不要 结束的id 为后6个点不要
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
// 将一帧无序点云转换成有序点云消耗的时间，指的是前面处理雷达数据的时间
printf("prepare time %f \n", t_prepare.toc());
// 计算每一个点的曲率，这里的laserCloud是有序的点云，故可以直接这样计算
// 但是在每条scan的交界处计算得到的曲率是不准确的，这可通过scanStartInd[i]、scanEndInd[i]来选取
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->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相当于所有点的初始自然序列，每个点得到它自己的序号（索引）
//这俩个是标志位
cloudNeighborPicked[i] = 0;//此标志位置1时，代表这个点被选为特征点了
cloudLabel[i] = 0;//特征点的标签
}
TicToc t_pts;//计算特征提取的时间
//声明 特征点的点云
pcl::PointCloud cornerPointsSharp; //角特征点 点云
pcl::PointCloud cornerPointsLessSharp; //次角特征点 点云
pcl::PointCloud surfPointsFlat;//面特征 点云
pcl::PointCloud surfPointsLessFlat;//次面特征 点云，，经过体素滤波处理
float t_q_sort = 0; // 用来记录排序花费的总时间
// 遍历每个scan，对每条线进行 *** 作（曲率排序，选取对应特征点）
for (int i = 0; i < N_SCANS; i++)
{
// 如果最后一个可算曲率的点与第一个的数量差小于6，说明无法分成6个扇区，跳过
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);//用来存储不太平整的点 不经过体素滤波
// 将每个scan等分成6等分 分别计算里面的特征点
for (int j = 0; j < 6; j++)
{
// 每个等分的起始和结束点
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;//计算排序时间
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//对点云按照曲率进行排序,小的在前,大的在后
t_q_sort += t_tmp.toc();// t_q_sort累计每个扇区曲率排序时间总和
int largestPickedNum = 0;//挑选最大的曲率特征点的个数 即角点的个数
//挑选曲率比较大的部分
for (int k = ep; k >= sp; k--)
{
//排序后顺序就乱了, 用之前存的 索引值 取到 点的id
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&// 看看这个点是否是有效点，同时曲率是否大于阈值，即没被选过 && 曲率 > 0.1
cloudCurvature[ind] > 0.1)
{
largestPickedNum++;//满足要求,则选择的 角点个数 加1
// 每段选择2个曲率最大的点
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2; // label为2是曲率大的标记，即设置标签为极大边线点
// cornerPointsSharp用来存放大曲率的点，既放入极大边线点，也放入次极大边线点
cornerPointsSharp.push_back(laserCloud->points[ind]);
//cornerPointsLessSharp 存放曲率稍微大的点
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)// 取20个曲率稍微大一些的点
{
cloudLabel[ind] = 1; // label 为1 是曲率稍微大的标记
// label置1表示曲率稍微大一些，超过2个选择点以后，设置为次极大边线点，放入次极大边线点容器
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else// 超过20个就跳过
{
break;
}
cloudNeighborPicked[ind] = 1;// 这个点被选中后pick标志位置1 ，记录这个点已经被选择了
for (int l = 1; l <= 5; l++)//为了保证特征点不过度集中,将选中的点周围5个点都置1 避免后续会选到
{
//查看相邻点距离是否差异过大,如果差异过大,说明点云在此不连续,是特征边缘,就是新的特征,就不置位了
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--)//为了保证特征点不过度集中,将选中的点周围5个点都置1 避免后续会选到
{
//查看相邻点距离是否差异过大,如果差异过大,说明点云在此不连续,是特征边缘,就是新的特征,就不置位了
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;
}
}
}
// 下面开始挑选面点，选取极小平面点（4个）
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; // 标记为-1，认为是平坦的点
surfPointsFlat.push_back(laserCloud->points[ind]);//surfPointsFlat存放曲率小的点
smallestPickedNum++; //满足要求,则选择的 面点个数 加1
// 这里不区分平坦和比较平坦，因为剩下的点label默认是0,就是比较平坦
if (smallestPickedNum >= 4) //选择4个面点
{
break;//够数了则跳出循环
}
cloudNeighborPicked[ind] = 1;//这个点被选中后 pick标志位置 1
// 保证特征点不过度集中,将选中的点周围（左右）5个点都置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;
}
}
}
// 选取次极小平面点，除了极小平面点，剩下的都是次极小平面点
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);//surfPointsLessFlatScan存放曲率较小的点
}
}
}
//以上就完成了特征的提取
//通过标签判断,前面的标签已将打上了 角点为-2 ,弱角点为-1 ,面点为1 .剩下的均为弱面点
// 对每一条scan线上的次极小平面点进行一次降采样
pcl::PointCloud surfPointsLessFlatScanDS;
pcl::VoxelGrid downSizeFilter;
// 一般次极小平面点比较多，所以这里做一个体素滤波来降采样
downSizeFilter.setInputCloud(surfPointsLessFlatScan);//给滤波对象设置需过滤的点云
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);//设置滤波时创建的体素大小为0.2*0.2*0.2的立方体
downSizeFilter.filter(surfPointsLessFlatScanDS);//执行滤波处理，存储输出为surfPointsLessFlatScanDS
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
printf("sort q time %f \n", t_q_sort);//打印排序时间
printf("seperate points time %f \n", t_pts.toc());//打印点云分类时间
// 将原始点云（经过无序-->有序的处理）发布出去
sensor_msgs::PointCloud2 laserCloudOutMsg;// 创建publish msg实例
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);// 有序点云转化为msg
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;// 时间戳保持不变
laserCloudOutMsg.header.frame_id = "/camera_init";// frame_id名字，坐标系
pubLaserCloud.publish(laserCloudOutMsg);
//发布角点
sensor_msgs::PointCloud2 cornerPointsSharpMsg;// 创建publish msg实例
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);// 有序点云转化为msg
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;// 时间戳保持不变
cornerPointsSharpMsg.header.frame_id = "/camera_init";// frame_id名字，坐标系
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
//发布弱角点
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;// 创建publish msg实例
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);// 有序点云转化为msg
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;// 时间戳保持不变
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";// frame_id名字，坐标系
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
//发布面点
sensor_msgs::PointCloud2 surfPointsFlat2;// 创建publish msg实例
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);// 有序点云转化为msg
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;// 时间戳保持不变
surfPointsFlat2.header.frame_id = "/camera_init";// frame_id名字，坐标系
pubSurfPointsFlat.publish(surfPointsFlat2);
//发布弱面点
sensor_msgs::PointCloud2 surfPointsLessFlat2;// 创建publish msg实例
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);// 有序点云转化为msg
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;// 时间戳保持不变
surfPointsLessFlat2.header.frame_id = "/camera_init";// frame_id名字，坐标系
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
if(PUB_EACH_LINE)// 可以按照每个scan发出去，不过这里是false
{
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函数，slam前端的参数读取，订阅话题，发布话题的初始化
int main(int argc, char **argv)
{
ros::init(argc, argv, "scanRegistration"); //ros的基本 *** 作，初始化ros节点，节点名称为scanRegistration
ros::NodeHandle nh;//声明ros句柄 nh
nh.param("scan_line", N_SCANS, 16); //从配置参数中读取读取激光雷达的scan line，16线激光雷达
nh.param("minimum_range", MINIMUM_RANGE, 0.1); //从launch文件参数服务器中读取minimum range，最小有效距离，小于0.1m的点将被滤除
printf("scan line number %d \n", N_SCANS); //打印线束
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64) //if条件判断，对激光雷达线束进行判断
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
//定义要发布的消息
//订阅初始激光雷达数据，注册回调函数laserCloudHandler
ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, laserCloudHandler);
//发布有序点云（删除过近点，设置索引）
pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100);
//发布极大边线点集合
pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100);
//发布次极大边线点集合
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100);
//发布极小平面点集合
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100);
//发布次极小平面点集合
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100);
//删除的点云
pubRemovePoints = nh.advertise("/laser_remove_points", 100);
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();// 循环执行回调函数
return 0;
}
参考：
3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读_月照银海似蛟龙的博客-CSDN博客
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）