LOAM论文看的算是跟没看差不多,师兄一问三不知,还是得结合代码细细看一下,写下来以便回顾。每个函数中的每句话都写了详细的解析,应该以后也可以看懂了。
scanRegistration主函数
int main(int argc, char **argv)
{
// 初始化节点
ros::init(argc, argv, "scanRegistration"); // argc为参数的个数,argv为参数的具体值
ros::NodeHandle nh; // 使得开启和结束一个节点
// 获取参数
// 默认雷达线数为 16,第一个参数表示优先获取对应的雷达launch文件中的"scan_line"参数值,N_SCANS为存储的变量,如果文件中没有则赋第三个default参数。
nh.param<int>("scan_line", N_SCANS, 16);
// 默认能够获取的最近的雷达点的距离为 0.1 m,低于这个范围内的点不使用,在laserCloudHandler函数中就会调用removeClosedPointCloud来实现
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;
}
// 初始化一系列 subscribers 和 publishers
// 订阅原始点云消息,订阅velodyne_points话题上的信息,当有消息发送到这个话题上就调用laserCloudHandler函数。
// 第二个参数是队列大小,以防我们处理消息的速度不够快,当缓存达到 100 条消息后,再有新的消息到来就将开始丢弃先前接收的消息。
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
// 发布过滤后的点云消息(去除 NaN /距离过近/ 测量值不准确的点)
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++)
{
// 对激光雷达每条线设置单独的点云 publisher
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
// 阻塞在此,一直等待着subscriber接受信息并调用函数
ros::spin();
return 0;
}
removeClosedPointCloud(过近点移除)
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;
// 将距离雷达原点过近的点去除
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);
}
// 过滤掉之后每一条线的激光雷达数量不一定所以没有办法通过宽和高区分线,所以采用直通滤波
// 因此这里不做特殊处理
cloud_out.height = 1;
cloud_out.width = static_cast<uint32_t>(j);
// 如果is_dense为true,则points中的数据是有限的,否则一些点的XYZ数值可能包含Inf/ NaN。
cloud_out.is_dense = true;
}
laserCloudHandler(初始点云处理:点云有序化 + 特征点提取)
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 这个if应该是设定一个systemDelay值,那么前面一些接收到的消息就不处理略过了,相当于系统先休眠一段时间忽略前systemDelay个消息
// 作用就是延时,为了确保有IMU数据后, 再进行点云数据的处理
if (!systemInited)
{
// 等待系统初始化
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
// 初始化计时器
TicToc t_whole;
TicToc t_prepare;
// 存储每条线对应的起始和结束索引,std::vector的第一个为数组参数,第二个参数为默认值
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
// 将点云消息转化为 pcl 点云类型
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
// 将接收到的消息转换为pcl::PointXYZ类型
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
// 对点云进行预处理,去除掉不合要求的点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); // 去除空值
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); // 离雷达圆心过于近的点
int cloudSize = laserCloudIn.points.size();
// 计算起始点和结束点的朝向,是否转过360°还是多了或少了
// 通常激光雷达扫描方向是顺时针,这里在取 atan 的基础上先取反,这样起始点理论上为 -pi,结束点为 pi,更符合直观
// 理论上起始点和结束点的差值应该是 0,为了显示两者区别,将结束点的方向补偿 2pi
// 表示结束点和起始点实际上逆时针经过一圈
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;
// 处理几种个别情况,以保持结束点的朝向和起始点的方向差始终在 [pi, 3pi] 之间 (实际上是 2pi 附近)
if (endOri - startOri > 3 * M_PI)
{
// case 1: 起始点在 -179°,结束点在 179°,补偿 2pi 后相差超过一圈,实际上是结束点相对于起始点还没转完整一圈
// 因此将结束点的 2pi 补偿去掉为 179°,与起始点相差 358°,表示结束点和起始点差别是一圈少2°
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
// case 2: 起始点为 179°,结束点为 -179°,补偿后结束点为 181°,此时不能反映真实差别,需要
// 对结束点再补偿上 2pi,表示经过了一圈多 2°
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
bool halfPassed = false;
int count = cloudSize;
PointType point;
// 将点云按扫描线分别存储在一个子点云中
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++)
{
// 小技巧对临时变量 Point 只初始化一次减少空间分配
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)
{
// velodyne 激光雷达的竖直 FoV 是[-15, 15],分辨率是 2°,这里通过这样的计算可以
// 对改激光点分配一个 [0, 15] 的扫描线 ID
scanID = int((angle + 15) / 2 + 0.5);
// 如果点的距离值不准有可能会计算出不在范围内的 ID 此时不考虑这个点
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
// 思路和 16 线的情况一致,垂直角度[-30.67,10.67]度,算出来为[0,31]
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)
{
// 和 16 线的情况一致,垂直角度[-24.3,2]度,算出来为scanID为[0,63],但是舍弃了大于50的扫描线
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
// 不考虑扫描线 id 在50以上的点
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);
// 计算激光点水平方向角,通过取反操作可以讲雷达扫描方向为逆时针(-pi 到 pi)方便和我们运算逻辑统一
float ori = -atan2(point.y, point.x);
if (!halfPassed)
{
// 对一些 Corner case 处理
if (ori < startOri - M_PI / 2)
{
// case 1:起始点在 179 °,逆时针转过几度后,当前点是 -179°,需要加上 2pi 作为补偿
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
// case 2: 理论上在逆时针转的时候不会出现这种情况,在顺时针的情况下,起始点在 -179°,
// 顺时针转过两度后到 179°,此时差距过大,需要减去 2pi
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
// 角度校正后如果和起始点相差超过 pi,表示已经过半圈
halfPassed = true;
}
}
else
{
// 经过半圈后,部分情况(扫描线从 179°到 -179°时)需要加 2pi,
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
// case 1: 在逆时针下理论上不会出现
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
// case 2: 起始点在 -179°,逆时针经过半圈后当前点在 1°,
// 此时差值是对的,因此将2pi补偿减去
ori -= 2 * M_PI;
}
}
// 估计当前当前点和起始点的时间差,当前经过了的角度占总角度的多少
float relTime = (ori - startOri) / (endOri - startOri);
// 小技巧:用 intensity的整数部分和小数部分来存储该点所属的扫描线以及相对时间:
// [线id].[相对时间*扫描周期]
point.intensity = scanID + scanPeriod * relTime;
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
printf("points size %d \n", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{
// 将每个扫描线上的局部点云汇总至一个点云里面,并计算每个扫描线对应的起始和结束坐标
// 相当于16条线合成一条长的线,并记录每条线的首尾坐标,方便后续计算取点
// 这里每个扫描线上的前 5 个和后 5 个点都不考虑(因为计算曲率时需要用到左右相邻 5 个点)
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
// * * * * * 1 1 1 ... 1 1 1 1 1 * * * * *,*表示用来计算曲率的点
}
// 打印点云预处理的耗时
printf("prepare time %f \n", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++)
{
// 计算当前点和周围十个点(左右各 5 个)在 x, y, z 方向上的差值: 10*p_i - sum(p_{i-5:i-1,i+1:i+5})
// 注意这里对每一条扫描线的边缘的计算是有问题的,因为会引入相邻扫描线的点,但见上面操作,每一条扫描线我们不考虑边缘的五个点,所以为了方便可以这么操作
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;
// 计算曲率,即对应论文中的C值计算,即左右周围5个点与中心点三个维度距离差的平方求和
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
// 特征点相关参数初始化,当前点在点云中的下标
cloudSortInd[i] = i;
// 这里实质上使用 1/0 来表示其相邻点是否已经选取,但是c++里面不推荐用 vector<bool> 来存储 bool,论文中对应的是选过的点不能选了
cloudNeighborPicked[i] = 0;
// 点的分类标签数组,如1表示 边缘点,2表示次边缘点,-1表示平面点
cloudLabel[i] = 0;
}
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++)
{
// 不考虑少于 5 个点的扫描线
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
// 将每条扫描线平均分成 6 个区域,按区域提取特征点(LOAM论文中是分为4个区域,有出入),所以应当每一圈扫描应当是有96个区域,192个边缘点,384个平面点
for (int j = 0; j < 6; j++)
{
// start pointer & end pointer:对于每个区域起始和末尾指针
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;
// 按照曲率对当前区域的点进行从小到大排序,sort函数的三个参数[起始坐标,结束坐标)
// 最后一个参数是排序规则,即表示依照cloudCurvature[i]<cloudCurvature[j]这个规则对数据排序
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 按曲率从大到小对激光进行遍历(从后往前),提取角点
for (int k = ep; k >= sp; k--)
{
// 获取当前的点在点云中的下标
int ind = cloudSortInd[k];
// 当前的点的曲率大于一定阈值并且还没有被选择
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++;
if (largestPickedNum <= 2)
{
// 如果已经选择的角点数小于2,将其加入两个角点点云中
// 标签 2 表示它是质量好的角点
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)
{
// 如果已经选择的角点数大于 2 但小于 20 个时,将其作为备用角点加入到
// 曲率相对不太高的角点点云中,标签 1 表示这个点可以作为角点,但质量不太好
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
// 如果已经选择的角点已经超过 20 个,则不再考虑后续的点
break;
}
// 设置相邻点选取标志位,表示当前已经被选择
cloudNeighborPicked[ind] = 1;
// 对选择点的左右相邻 5 个点进行分析
for (int l = 1; l <= 5; l++)
{
// 如果点和前一个点的距离不超过阈值,将其标记为已经被选择中
// 表示这个特征点附近的点之间如果距离过近,就不考虑接着作为特征点,防止点云密度过大
// A-LOAM中并没有过滤掉论文中提到的平行于扫描束和有遮挡关系的点
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;
}
}
}
int smallestPickedNum = 0;
// 按曲率从小到大遍历点,提取平面点
for (int k = sp; k <= ep; k++)
{
// 提取该点在点云的下标
int ind = cloudSortInd[k];
// 如果当前点曲率小于给定阈值并且还没有被选择,进行分析
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
// 标记当前点为 -1,表示其为平面度高的平面点,与边缘点不同,它并没有在这一个循环中同时保留次平面点。
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4)
{
// 如果已经选了 4 个平面点,后续的点不予考虑
break;
}
// 将其标记为已选择,并对其左右相邻 5 个距离较近的点也标记为已选择
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;
}
}
}
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
// 将所有不被判断为角点的点都作为平面点的候选点,而边缘点中除前20个都舍去了。
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
// 由于LessFlat 的平面点数量太多(除了角点以外都是),因此对其进行下采样存储,采用的是体素滤波器,即在每一个边长为0.2的立方体中的所有次平面点被质心的一个点替代
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
// surfPointsLessFlatScan只是临时存放次平面点的点云集,下采样之后存放到surfPointsLessFlatScanDS,并存储到先前定好的surfPointsLessFlat类中
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
// 输出点云排序时间
printf("sort q time %f \n", t_q_sort);
// 输出特征点提取的耗时
printf("seperate points time %f \n", t_pts.toc());
// 发布过滤后(去空值和过于近的点)的点云以及各个特征点消息
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 scan
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");
}