[笔记]三维激光SLAM学习一（2）——LOAM代码scanRegistration篇

[笔记]三维激光SLAM学习一（2）——LOAM代码scanRegistration篇概述第一部分：scanRegistration节点介绍 main函数回调函数 laserCloudHandler丢弃前20帧点云数据去除异常点按照激光线号从小到大放到容器中筛选且计算曲率$c$ 回调函数imuHanler总结

[笔记]三维激光SLAM学习一（2）——LOAM代码scanRegistration篇

概述

LiDAR&IMU坐标系：x轴向前，y轴向左，z轴向上的右手坐标系 LOAM坐标系：z轴向前，x轴向左，y轴向上的右手坐标系

第一部分：scanRegistration

根据VLP16的激光扫描模型, 对单帧点云（论文中称为一个Sweep）进行分线束（分为16束）, 每束线的一次扫描称为一个Scan， 并记录每个点所属线束和每个点在此帧点云内的相对扫描时间（相对于本帧第一个点）。 针对单个Scan提取特征点，而相对时间会在laserOdometry中用于运动补偿。所有Scan的特征点，拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云)。 至此，每帧点云，输出两帧对应的特征点云给下一个节点laserOdometry。

节点介绍

这一节点主要功能是：对点云和IMU数据进行预处理，用于特征点的配准。所以这个节点实际上就是一个计算准备的过程。 其实就做了一个工作：那就是根据点的曲率$c$来将点划分为不同的类别（边缘/平面特征或不是特征），公式如下： $$c=\frac{1}{\left|\mathbf{S}\right|\cdot \Vert {\mathbf{X}}_{\left(k,i\right)}^L\Vert }\Vert \sum _{j\in \mathbf{S},j\ne i}({\mathbf{X}}_{(k,i)}^{\mathbf{L}}-{\mathbf{X}}_{(k,j)}^{\mathbf{L}})\Vert$$ 论文中选择在锐利边缘和平面曲面片上的特征点。设$i$为$P_k$中的一个点，$i\in P_k$，设$S$为Lidar在同一扫描中返回的$i$的连续点集。定义一个“距离值”$c$来评估局部曲面的平滑度，用于边缘特征点、平面特征点和普通点的分类。

扫描中的点根据$c$值进行排序，然后用最大$c$值（即边缘点）和最小$c$值（即平面点）选择特征点。一次扫描分为四个均等的区域。每个区域中最多可提供2个边缘点和4个平面点。不能位于与激光束大致平行的曲面片上不能位于遮挡区域的边界上

main函数

下面，我们先来看看main函数。

int main(int argc, char** argv) { ros::init(argc, argv, "scanRegistration");//初始化节点，并命名 ros::NodeHandle nh; //创建此进程节点的句柄。 //订阅点云信息和IMU信息 ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 2, laserCloudHandler); ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler); pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_2", 2); pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_sharp", 2); pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_less_sharp", 2); pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_flat", 2); pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_less_flat", 2); pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5); ros::spin(); //ros::spin（）进入循环，尽快调用消息回调。 return 0; }

subLaserCloud订阅 /velodyne_points点云消息，并由 回调函数 laserCloudHandler 进行处理，订阅器缓存区大小为2； subImu订阅 /imu/data IMU消息，由 回调函数imuHanler 处理，缓存区大小为50。 pubLaserCloud 发布以不同线分类的点云的指针 /velodyne_cloud_2 ，缓存区大小为2； pubCornerPointSharp 和 pubCornerPointLessSharp 发布特征点 /laser_cloud_sharp 和 /laser_cloud_less_sharp，缓存区大小为2； pubSurfPointFlat 和 pubSurfPointLessFlat 发布特征点 /laser_cloud_flat 和 /laser_cloud_less_flat，缓存区大小为2； pubImuTrans 发布IMU信息 /imu_trans，缓存区大小为5。 下面从两个回调函数分别介绍这个节点的代码。

回调函数 laserCloudHandler

该回调函数是这scanRegistration节点的重点部分，主要点云进行预处理，完成分类。 具体分类内容为：

按照不同线，将点云保存在点云指针中；对其进行特征分类。

丢弃前20帧点云数据

if (!systemInited) {//丢弃前20个点云数据 systemInitCount++; if (systemInitCount >= systemDelay) { systemInited = true; } return; }

去除异常点

去除异常点并计算起始和终止位置的角度：

//记录每个scan有曲率的点的开始和结束索引 std::vector<int> scanStartInd(N_SCANS, 0); std::vector<int> scanEndInd(N_SCANS, 0); //当前点云时间 double timeScanCur = laserCloudMsg->header.stamp.toSec(); pcl::PointCloud<pcl::PointXYZ> laserCloudIn; //消息转换成pcl数据存放 pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); std::vector<int> indices; //移除空点 pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); //点云点的数量 int cloudSize = laserCloudIn.points.size(); //lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转 float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); //lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; //结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描 //正常情况下在这个范围内：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; }

按照激光线号从小到大放到容器中

然后遍历所有点，根据角度计算结果将其划分到不同的线号：计算角度–>计算起始和终止位置–>插入IMU数据–>将点插入容器中

//lidar扫描线是否旋转过半 bool halfPassed = false; int count = cloudSize; PointType point; std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS); for (int i = 0; i < cloudSize; i++) { //坐标轴交换，velodyne lidar的坐标系也转换到z轴向前，x轴向左的右手坐标系 point.x = laserCloudIn.points[i].y; point.y = laserCloudIn.points[i].z; point.z = laserCloudIn.points[i].x; //计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度 float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI; int scanID; //仰角四舍五入(加减0.5截断效果等于四舍五入) int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); if (roundedAngle > 0){ scanID = roundedAngle; } else { scanID = roundedAngle + (N_SCANS - 1); } //过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15] if (scanID > (N_SCANS - 1) || scanID < 0 ){ count--; continue; } //该点的旋转角 float ori = -atan2(point.x, point.z); if (!halfPassed) {//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿 //确保-pi/2 < ori - startOri < 3*pi/2 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; //确保-3*pi/2 < ori - endOri < pi/2 if (ori < endOri - M_PI * 3 / 2) { ori += 2 * M_PI; } else if (ori > endOri + M_PI / 2) { ori -= 2 * M_PI; } } //-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间） float relTime = (ori - startOri) / (endOri - startOri); //点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间 point.intensity = scanID + scanPeriod * relTime; //点时间=点云时间+周期时间 if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变 float pointTime = relTime * scanPeriod;//计算点的周期时间 //寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront while (imuPointerFront != imuPointerLast) { if (timeScanCur + pointTime < imuTime[imuPointerFront]) { break; } imuPointerFront = (imuPointerFront + 1) % imuQueLength; } if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用 imuRollCur = imuRoll[imuPointerFront]; imuPitchCur = imuPitch[imuPointerFront]; imuYawCur = imuYaw[imuPointerFront]; imuVeloXCur = imuVeloX[imuPointerFront]; imuVeloYCur = imuVeloY[imuPointerFront]; imuVeloZCur = imuVeloZ[imuPointerFront]; imuShiftXCur = imuShiftX[imuPointerFront]; imuShiftYCur = imuShiftY[imuPointerFront]; imuShiftZCur = imuShiftZ[imuPointerFront]; } else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角 int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; //按时间距离计算权重分配比率,也即线性插值 float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) { imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack; } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) { imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack; } else { imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; } //本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFront imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack; imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack; imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack; imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack; imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack; imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack; } if (i == 0) {//如果是第一个点,记住点云起始位置的速度，位移，欧拉角 imuRollStart = imuRollCur; imuPitchStart = imuPitchCur; imuYawStart = imuYawCur; imuVeloXStart = imuVeloXCur; imuVeloYStart = imuVeloYCur; imuVeloZStart = imuVeloZCur; imuShiftXStart = imuShiftXCur; imuShiftYStart = imuShiftYCur; imuShiftZStart = imuShiftZCur; } else {//计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移速度畸变，并对点云中的每个点位置信息重新补偿矫正 ShiftToStartIMU(pointTime); VeloToStartIMU(); TransformToStartIMU(&point); } } laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器 } //获得有效范围内的点的数量 cloudSize = count;

最后更新总的点云laserCLoud

pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>()); for (int i = 0; i < N_SCANS; i++) {//将所有的点按照线号从小到大放入一个容器 *laserCloud += laserCloudScans[i]; }

筛选且计算曲率$c$

上边的代码，将所有点按照线号从小到大放到容器中了，然后遍历每个点（除了前五个和后五个），计算各点曲率$c$并找到所有线的起点终点位置，从而在laserCloud中找特征点的候选点。

int scanCount = -1; 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; //初始时，点全未筛选过 cloudNeighborPicked[i] = 0; //初始化为less flat点 cloudLabel[i] = 0; //每个scan，只有第一个符合的点会进来，因为每个scan的点都在一起存放 if (int(laserCloud->points[i].intensity) != scanCount) { scanCount = int(laserCloud->points[i].intensity);//控制每个scan只进入第一个点 //曲率只取同一个scan计算出来的，跨scan计算的曲率非法，排除，也即排除每个scan的前后五个点 if (scanCount > 0 && scanCount < N_SCANS) { scanStartInd[scanCount] = i + 5; scanEndInd[scanCount - 1] = i - 5; } } } //第一个scan曲率点有效点序从第5个开始，最后一个激光线结束点序size-5 scanStartInd[0] = 5; scanEndInd.back() = cloudSize - 5;

参照论文对于点位筛选的条件：

不能位于与激光束大致平行的曲面片上；不能位于遮挡区域的边界上；

下边做法是： 遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。

//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到 for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值，所以减6 float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x; float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y; float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z; //计算有效曲率点与后一个点之间的距离平方和 float diff = diffX * diffX + diffY * diffY + diffZ * diffZ; if (diff > 0.1) {//前提:两个点之间距离要大于0.1 //点的深度 float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z); //后一个点的深度 float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); //按照两点的深度的比例，将深度较大的点拉回后计算距离 if (depth1 > depth2) { diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1; diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1; diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1; //边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上 if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点 //该点及前面五个点（大致都在斜面上）全部置为筛选过 cloudNeighborPicked[i - 5] = 1; cloudNeighborPicked[i - 4] = 1; cloudNeighborPicked[i - 3] = 1; cloudNeighborPicked[i - 2] = 1; cloudNeighborPicked[i - 1] = 1; cloudNeighborPicked[i] = 1; } } else { diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x; diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y; diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z; if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) { cloudNeighborPicked[i + 1] = 1; cloudNeighborPicked[i + 2] = 1; cloudNeighborPicked[i + 3] = 1; cloudNeighborPicked[i + 4] = 1; cloudNeighborPicked[i + 5] = 1; cloudNeighborPicked[i + 6] = 1; } } } float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x; float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y; float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z; //与前一个点的距离平方和 float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2; //点深度的平方和 float dis = laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y + laserCloud->points[i].z * laserCloud->points[i].z; //与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用 if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { cloudNeighborPicked[i] = 1; } }

经过上述处理，得到一组有条理的点云，且把不能被选的点标记了出来。下边开始筛选特征点，为了保证特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域，每个子区域最多提供2个边缘点和4个平面点。我们只需要预先设定好阈值，就可以将这些点进行分类了。代码中，把每条线分为6段进行处理，每段都将曲率$c$按照升序排列。 代码如下：

pcl::PointCloud<PointType> cornerPointsSharp; pcl::PointCloud<PointType> cornerPointsLessSharp; pcl::PointCloud<PointType> surfPointsFlat; pcl::PointCloud<PointType> surfPointsLessFlat; //将每条线上的点分入相应的类别：边沿点和平面点 for (int i = 0; i < N_SCANS; i++) { pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>); //将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点 for (int j = 0; j < 6; j++) { //六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6 int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6; //六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6 int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1; //按曲率从小到大冒泡排序 for (int k = sp + 1; k <= ep; k++) { for (int l = k; l >= sp + 1; l--) { //如果后面曲率点大于前面，则交换 if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) { int temp = cloudSortInd[l - 1]; cloudSortInd[l - 1] = cloudSortInd[l]; cloudSortInd[l] = temp; } } } //挑选每个分段的曲率很大和比较大的点 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个点放入sharp点集合 cloudLabel[ind] = 2;//2代表点曲率很大 cornerPointsSharp.push_back(laserCloud->points[ind]); cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else if (largestPickedNum <= 20) {//挑选曲率最大的前20个点放入less sharp点集合 cloudLabel[ind] = 1;//1代表点曲率比较尖锐 cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else { break; } cloudNeighborPicked[ind] = 1;//筛选标志置位 //将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀 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; } } } //挑选每个分段的曲率很小比较小的点 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]); smallestPickedNum++; if (smallestPickedNum >= 4) {//只选最小的四个，剩下的Label==0,就都是曲率比较小的 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; } } } //将剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中 for (int k = sp; k <= ep; k++) { if (cloudLabel[k] <= 0) { surfPointsLessFlatScan->push_back(laserCloud->points[k]); } } } //由于less flat点最多，对每个分段less flat的点进行体素栅格滤波 pcl::PointCloud<PointType> surfPointsLessFlatScanDS; pcl::VoxelGrid<PointType> downSizeFilter; downSizeFilter.setInputCloud(surfPointsLessFlatScan); downSizeFilter.setLeafSize(0.2, 0.2, 0.2); downSizeFilter.filter(surfPointsLessFlatScanDS); //less flat点汇总 surfPointsLessFlat += surfPointsLessFlatScanDS; }

通过计算$c$值且按升序排序，找到了特征点，下面代码是将其发布出去。

//publich消除非匀速运动畸变后的所有的点 sensor_msgs::PointCloud2 laserCloudOutMsg; pcl::toROSMsg(*laserCloud, laserCloudOutMsg); laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; laserCloudOutMsg.header.frame_id = "/camera"; pubLaserCloud.publish(laserCloudOutMsg); //publich消除非匀速运动畸变后的平面点和边沿点 sensor_msgs::PointCloud2 cornerPointsSharpMsg; pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; cornerPointsSharpMsg.header.frame_id = "/camera"; pubCornerPointsSharp.publish(cornerPointsSharpMsg); sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; cornerPointsLessSharpMsg.header.frame_id = "/camera"; pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); sensor_msgs::PointCloud2 surfPointsFlat2; pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; surfPointsFlat2.header.frame_id = "/camera"; pubSurfPointsFlat.publish(surfPointsFlat2); sensor_msgs::PointCloud2 surfPointsLessFlat2; pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; surfPointsLessFlat2.header.frame_id = "/camera"; pubSurfPointsLessFlat.publish(surfPointsLessFlat2); //publich IMU消息,由于循环到了最后，因此是Cur都是代表最后一个点，即最后一个点的欧拉角，畸变位移及一个点云周期增加的速度 pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1); //起始点欧拉角 imuTrans.points[0].x = imuPitchStart; imuTrans.points[0].y = imuYawStart; imuTrans.points[0].z = imuRollStart; //最后一个点的欧拉角 imuTrans.points[1].x = imuPitchCur; imuTrans.points[1].y = imuYawCur; imuTrans.points[1].z = imuRollCur; //最后一个点相对于第一个点的畸变位移和速度 imuTrans.points[2].x = imuShiftFromStartXCur; imuTrans.points[2].y = imuShiftFromStartYCur; imuTrans.points[2].z = imuShiftFromStartZCur; imuTrans.points[3].x = imuVeloFromStartXCur; imuTrans.points[3].y = imuVeloFromStartYCur; imuTrans.points[3].z = imuVeloFromStartZCur; sensor_msgs::PointCloud2 imuTransMsg; pcl::toROSMsg(imuTrans, imuTransMsg); imuTransMsg.header.stamp = laserCloudMsg->header.stamp; imuTransMsg.header.frame_id = "/camera"; pubImuTrans.publish(imuTransMsg);

回调函数imuHanler

double roll, pitch, yaw; tf::Quaternion orientation; //convert Quaternion msg to Quaternion tf::quaternionMsgToTF(imuIn->orientation, orientation); //This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll). //Here roll pitch yaw is in the global frame tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); //减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll). float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81; float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81; //循环移位效果，形成环形数组 imuPointerLast = (imuPointerLast + 1) % imuQueLength; imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); imuRoll[imuPointerLast] = roll; imuPitch[imuPointerLast] = pitch; imuYaw[imuPointerLast] = yaw; imuAccX[imuPointerLast] = accX; imuAccY[imuPointerLast] = accY; imuAccZ[imuPointerLast] = accZ; AccumulateIMUShift();

总结

上边介绍了LOAM的第一部分，后面的内容在之后的文章继续更新。