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 是 1
cloud_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;
}
else
return;
}
TicToc t_whole;
TicToc t_prepare;
//每条扫描线上的可以计算曲率的点云点的起始索引和结束索引
std::vector<int> scanStartInd(N_SCANS, 0); //长度为N_SCANS，值全为0
std::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的点云向量laserCloudScans
for (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=CloudSize
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;
}
}
//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和fireID
point.intensity = scanID + scanPeriod * relTime; //intensity的整数部分存放scanID，小数部分存放归一化后的fireID
laserCloudScans[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 - 6
cloudNeighborPicked[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_flat
for (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个subcans
for (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/6
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; //subscan的结束index
TicToc 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_sharp
for (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_sharp
cornerPointsSharp.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_sharp
cornerPointsLessSharp.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_flat
int 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_flat
surfPointsFlat.push_back(laserCloud->points[ind]); //将ind对应点加入surf_flat
smallestPickedNum++;
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种特征点与滤波后的当前帧点云进行publish
sensor_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;
}
// 订阅初始的激光雷达数据，并执行回调函数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);
// 如果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;
}
1323
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
