scanRegistration 的内容是 corner 与 surface 特征点的提取。
一、main()函数
订阅:velodyne_points,激光雷达点云数据。
发布:velodyne_cloud_2,由 velodyne_points 经过处理后的有序点云;
laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat,特征点云。
二、laserCloudHandler()函数
laserCloudHandler 是对 velodyne_points 点云数据的处理。
1. 去除无效点云
// 首先对点云滤波,去除 NaN 值的无效点云,以及在 Lidar 坐标系原点 MINIMUM_RANGE 距离以内的点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
2. 根据仰角确定 scanID
根据多线激光雷达 scan 的角度分布情况,结合仰角,即可确定所有点是由哪个 scan 扫描得到的。
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);
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();
}
3. 根据水平方位角确定 fireID
计算各个点相对于起始点的时间差,便于后续消除点云数据的运动畸变。
1) 计算起始点与终止点的方位角
// 点云的方位角,起始角度和终止角度
// 扫描起始的方位角,atan2范围 [-pi,pi],计算方位角时取负号是因为 velodyne 顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
// 扫描结束点的方位角,加 2*pi 使点云旋转周期为 2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
// 终止方位角与起始方位角之间的角度差在 (pi, 3*pi), 允许雷达不是一个圆周扫描
if (endOri - startOri > 3 * M_PI)
endOri -= 2 * M_PI;
else if (endOri - startOri < M_PI)
endOri += 2 * M_PI;
2) 计算各个点相对于起始点的时间差
// 确定每一个点的水平方位角
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;
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;
}
// 点云中点的相对时间:点旋转的角度与整个周期旋转角度的比率
float relTime = (ori - startOri) / (endOri - startOri);
// 点强度 = scanID + 点相对时间
point.intensity = scanID + scanPeriod * relTime; // intensity 字段的整数部分存放 scanID,小数部分存放归一化后的 fireID
laserCloudScans[scanID].push_back(point); // 将点根据 scanID 放到对应的子点云 laserCloudScans 中
intensity 用于存放 scanID 和 fireID。
4. 有序点云
// 将子点云 laserCloudScans 合并成一帧点云 laserCloud
// 这里的单帧点云 laserCloud 可以认为已经是有序点云了,按照 scanID 和 fireID 的顺序存放
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{ // 确定每一个 scanline 对应点云的起始和结束索引
// 记录每个scan的开始index,忽略前5个点
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
// 记录每个 scan 的结束 index,忽略后5个点,开始和结束处的点云 scan 容易产生不闭合的“接缝”,对提取 edge feature 不利
scanEndInd[i] = laserCloud->size() - 6;
}
5. 特征点提取
1) 计算点的曲率
遍历每个点(除了前五个点和后五个点)差值的平方代表曲率。
// 使用每个点的前后五个点计算曲率,因此前五个点和后五个点跳过
for (int i = 5; i < cloudSize - 5; i++)
{
// 该点与周围10个点的偏差
// diff = sum(p(i-5:i-1)) + sum(p(i+1:i+5)) - 10*p(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;
// 初始时,点都未被选为特征点
cloudNeighborPicked[i] = 0;
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,因为点太多,最后会降采样
}
2) 根据曲率计算4种特征点
TicToc t_pts;
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
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小段(subscan)执行特征检测
{
// subscan 的起始 index
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
// subscan 的结束 index
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
// 根据曲率由小到大对 subscan 的点进行 sort
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 从后往前,即从曲率大的点开始提取 corner feature
for (int k = ep; k >= sp; k--)
{
int ind = cloudSortInd[k]; // ind != k ?
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]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)
{ // 该 subscan 中曲率最大的前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++)
{
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 平面 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_less_flat 特征点
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;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
/* sharp corner 和 flat surface 特征作为激光里程计中 source 点云,less sharp corner 和 less flat surface 特征作为激光里程计中的 target 点云 */
/* 添加less sharp、less flat ? :
* 基于曲率的方法并不能准确、稳定地检出 corner 和 surface 特征,尤其是 corner 特征,
* 将数量更多的 less sharp corner 和 less flat surface 特征作为激光里程计中的 target 点云,有利于提高有效关联的数量,以提高激光里程计的精度
*/
}
3) 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);
// 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);
}
}