SLAM：loam算法架构分析

在研读了论文及开源代码后，对LOAM的一些理解做一个整理。

文章：Low-drift and real-time lidar odometry and mapping

开源代码：https://github.com/daobilige-su/loam_velodyne

系统概述

LOAM的整体思想就是将复杂的SLAM问题分为：1. 高频的运动估计； 2. 低频的环境建图。

Lidar接收数据，首先进行Point Cloud Registration，Lidar Odometry以10Hz的频率进行运动估计和坐标转换，Lidar Mapping以1Hz的频率构建三维地图，Transform Integration完成位姿的优化。这样并行的结构保证了系统的实时性。

接下来是代码的框架图：

整个算法分为四个模块，相对于其它直接匹配两个点云的算法，LOAM是通过提取特征点进行匹配之后计算坐标变换。具体流程为：ScanRegistration 提取特征点并排除瑕点；LaserOdometry从特征点中估计运动，然后整合数据发送给LaserMapping；LaserMapping输出的laser_cloud_surround为地图；TransformMaintenance订阅LaserOdometry与LaserMapping发布的Odometry消息，对位姿进行融合优化。后面将详细进行说明。

ScanRegistration

这一模块（节点）主要功能是：特征点的提取

一次扫描的点通过曲率值来分类，特征点曲率大于阈值的为边缘点；特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点。此外，将不稳定的特征点（瑕点）排除。下面将通过代码进行说明。

从主函数开始：

[cpp] view plain copy

1. int main(int argc, char** argv)  
2. {  
3.   ros::init(argc, argv, "scanRegistration");  
4.   
5.   /* 
6.    * NodeHandle 是节点同ROS系统交流的主要接口 
7.    * NodeHandle 在构造的时候会完整地初始化本节点  
8.    * NodeHandle 析构的时候会关闭此节点 
9.    */  
10.   ros::NodeHandle nh;  
11.   
12.   /* 
13.    * 参数1：话题名称     
14.    * 参数2：信息队列长度     
15.    * 参数3：<strong>回调函数</strong>，每当一个信息到来的时候，这个函数会被调用     
16.    * 返回一个ros::Subscriber类的对象，当此对象的所有引用都被销毁是，本节点将不再是该话题的订阅者  
17.    */     
18.   // 订阅了velodyne_points和imu/data  
19.   ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>   
20.                                   ("/velodyne_points", 2, laserCloudHandler);    
21.   ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);  
22.   
23.   /* 
24.    * 我们通过advertise() 函数指定我们如何在给定的topic上发布信息 
25.    * 它会触发对ROS master的调用，master会记录话题发布者和订阅者 
26.    * 在advertise()函数执行之后，master会通知每一个订阅此话题的节点 
27.    * 两节点间由此可以建立直接的联系 
28.  
29.    * advertise()会返回一个Publisher对象，使用这个对象的publish方法我们就可以在此话题上发布信息 
30.    * 当返回的Publisher对象的所有引用都被销毁的时候，本节点将不再是该话题的发布者 
31.  
32.    * 此函数是一个带模板的函数，需要传入具体的类型进行实例化 
33.    * 传入的类型就是要发布的信息的类型，在这里是String 
34.  
35.    * 第一个参数是话题名称 
36.  
37.    * 第二个参数是信息队列的长度，相当于信息的一个缓冲区 
38.    * 在我们发布信息的速度大于处理信息的速度时 
39.    * 信息会被缓存在先进先出的信息队列里 
40.    */  
41.   // 发布了6个话题：velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_trans  
42.   pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>  
43.                                  ("/velodyne_cloud_2", 2);  
44.   
45.   pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>  
46.                                         ("/laser_cloud_sharp", 2);  
47.   
48.   pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>  
49.                                             ("/laser_cloud_less_sharp", 2);  
50.   
51.   pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>  
52.                                        ("/laser_cloud_flat", 2);  
53.   
54.   pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>  
55.                                            ("/laser_cloud_less_flat", 2);  
56.   
57.   pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);  
58.   
59.   /** 
60.    * 它可以保证你指定的回调函数会被调用 
61.    * 程序执行到spin()后就不调用其他语句了 
62.    */  
63.   ros::spin();  
64.   
65.   return 0;  
66. }  

主函数比较简单， 订阅了2个节点和发布了6个节点。通过 回调函数 的处理，将处理后的点云重新发出去。主要涉及的函数为

laserCloudHandler与imuHandler。

·laserHandler

laserCloudHandler是这一模块的重点部分，主要功能是对接收到的点云进行预处理，完成分类。具体分类内容为：一是将点云划入不同线中存储；二是对其进行特征分类。

首先对收到的点云进行处理

[cpp] view plain copy

1. void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)  
2. {  
3.   if (!systemInited) {  
4.     systemInitCount++;  
5.     if (systemInitCount >= systemDelay) {  
6.       // systemDelay 有延时作用，保证有imu数据后在调用laserCloudHandler  
7.       systemInited = true;  
8.     }  
9.     return;  
10.   }  
11.   
12.   std::vector<int> scanStartInd(N_SCANS, 0);  
13.   std::vector<int> scanEndInd(N_SCANS, 0);  
14.   
15.   // Lidar的时间戳  
16.   double timeScanCur = laserCloudMsg->header.stamp.toSec();  
17.   pcl::PointCloud<pcl::PointXYZ> laserCloudIn;  
18.   
19.   // fromROSmsg(input,cloud1) 转为为模板点云laserCloudIn  
20.   pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);  
21.   std::vector<int> indices;  
22.   //去除无效值  
23.   pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);  
24.   int cloudSize = laserCloudIn.points.size();  
25.   
26.   //计算点云的起始角度/终止角度  
27.   float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);  
28.   float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,  
29.                         laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;  

接下来的处理是根据角度将点划入不同数组中

[cpp] view plain copy

1. for (int i = 0; i < cloudSize; i++) {  
2.   point.x = laserCloudIn.points[i].y;  
3.   point.y = laserCloudIn.points[i].z;  
4.   point.z = laserCloudIn.points[i].x;  
5.   
6.   float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;  
7.   int scanID;  
8.   int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));   
9.   if (roundedAngle > 0){  
10.     scanID = roundedAngle;  
11.   }  
12.   else {  
13.     // 角度大于0，由小到大划入偶数线0-16；角度小于0，由大到小划入奇数线15-1  
14.     scanID = roundedAngle + (N_SCANS - 1);  
15.   }  
16.   if (scanID > (N_SCANS - 1) || scanID < 0 ){  
17.     // 不在16线附近的点作为杂点进行剔除  
18.     count--;  
19.     continue;  
20.   }  

接下来计算每个点的相对方位角计算出相对时间，根据线性插值的方法计算速度及角度，并转换到sweep k的初始imu坐标系下，再划入16线数组中

[cpp] view plain copy

1. float ori = -atan2(point.x, point.z);  
2.   if (!halfPassed) {  
3.     if (ori < startOri - M_PI / 2) {  
4.       ori += 2 * M_PI;  
5.     } else if (ori > startOri + M_PI * 3 / 2) {  
6.       ori -= 2 * M_PI;  
7.     }  
8.   
9.     if (ori - startOri > M_PI) {  
10.       halfPassed = true;  
11.     }  
12.   } else {  
13.     ori += 2 * M_PI;  
14.   
15.     if (ori < endOri - M_PI * 3 / 2) {  
16.       ori += 2 * M_PI;  
17.     } else if (ori > endOri + M_PI / 2) {  
18.       ori -= 2 * M_PI;  
19.     }   
20.   }  
21.   
22.   //scanPeriod=0.1，是因为lidar工作周期是10HZ，意味着转一圈是0.1秒；intensity是一个整数+小数，小数不会超过0.1，完成了按照时间排序的需求  
23.   float relTime = (ori - startOri) / (endOri - startOri);  
24.   point.intensity = scanID + scanPeriod * relTime;  
25.   
26.   // imuPointerLast 是当前点，变量只在imu中改变，设为t时刻  
27.   // 对每一个cloud point处理  
28.   if (imuPointerLast >= 0) {  
29.     float pointTime = relTime * scanPeriod;  
30.     while (imuPointerFront != imuPointerLast) {  
31.   
32.       // (timeScanCur + pointTime)设为ti时刻；imuPointerFront 是 ti后一个时刻  
33.       if (timeScanCur + pointTime < imuTime[imuPointerFront]) {  
34.         break;  
35.       }  
36.       imuPointerFront = (imuPointerFront + 1) % imuQueLength;  
37.     }  
38.   
39.     if (timeScanCur + pointTime > imuTime[imuPointerFront]) {  
40.   
41.       // 这个的意思是 imuPointerFront=imuPointerLast时候  
42.       imuRollCur = imuRoll[imuPointerFront];  
43.       imuPitchCur = imuPitch[imuPointerFront];  
44.       imuYawCur = imuYaw[imuPointerFront];  
45.   
46.       imuVeloXCur = imuVeloX[imuPointerFront];  
47.       imuVeloYCur = imuVeloY[imuPointerFront];  
48.       imuVeloZCur = imuVeloZ[imuPointerFront];  
49.   
50.       imuShiftXCur = imuShiftX[imuPointerFront];  
51.       imuShiftYCur = imuShiftY[imuPointerFront];  
52.       imuShiftZCur = imuShiftZ[imuPointerFront];  
53.     } else {  
54.   
55.       // imuPointerBack = imuPointerFront - 1 线性插值求解出当前点对应的imu角度，位移和速度  
56.       int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;  
57.       float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])   
58.                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);  
59.       float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)   
60.                       / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);  
61.   
62.       imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;  
63.       imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;  
64.       if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {  
65.         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;  
66.       } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {  
67.         imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;  
68.       } else {  
69.         imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;  
70.       }  
71.   
72.       imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;  
73.       imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;  
74.       imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;  
75.   
76.       imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;  
77.       imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;  
78.       imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;  
79.     }  
80.     if (i == 0) {  
81.       imuRollStart = imuRollCur;  
82.       imuPitchStart = imuPitchCur;  
83.       imuYawStart = imuYawCur;  
84.   
85.       imuVeloXStart = imuVeloXCur;  
86.       imuVeloYStart = imuVeloYCur;  
87.       imuVeloZStart = imuVeloZCur;  
88.   
89.       imuShiftXStart = imuShiftXCur;  
90.       imuShiftYStart = imuShiftYCur;  
91.       imuShiftZStart = imuShiftZCur;  
92.     } else {  
93.         
94.       // 将Lidar位移转到IMU起始坐标系下  
95.       ShiftToStartIMU(pointTime);  
96.   
97.       // 将Lidar运动速度转到IMU起始坐标系下  
98.       VeloToStartIMU();  
99.   
100.       // 将点坐标转到起始IMU坐标系下  
101.       TransformToStartIMU(&point);  
102.     }  
103.   }  
104.   // 将点按照每一层线，分类压入16个数组中  
105.   laserCloudScans[scanID].push_back(point);  
106. }  

之后将对所有点进行曲率值的计算并记录每一层曲率数组的起始和终止

[cpp] view plain copy

1. cloudSize = count;  
2.   
3.   pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());  
4.   //将所有点存入laserCloud中，点按线序进行排列  
5.   for (int i = 0; i < N_SCANS; i++) {  
6.     *laserCloud += laserCloudScans[i];  
7.   }  
8.   int scanCount = -1;  
9.   
10.   for (int i = 5; i < cloudSize - 5; i++) {  
11.     // 对所有的激光点一个一个求出在该点前后5个点(10点)的偏差，作为cloudCurvature点云数据的曲率  
12.     float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x   
13.                 + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x   
14.                 + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x   
15.                 + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x  
16.                 + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x  
17.                 + laserCloud->points[i + 5].x;  
18.     float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y   
19.                 + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y   
20.                 + laserCloud->fpoints[i - 1].y - 10 * laserCloud->points[i].y   
21.                 + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y  
22.                 + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y  
23.                 + laserCloud->points[i + 5].y;  
24.     float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z   
25.                 + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z   
26.                 + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z   
27.                 + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z  
28.                 + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z  
29.                 + laserCloud->points[i + 5].z;  
30.     cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;  
31.     cloudSortInd[i] = i;  
32.     cloudNeighborPicked[i] = 0;  
33.     cloudLabel[i] = 0;  
34.   
35.     if (int(laserCloud->points[i].intensity) != scanCount) {  
36.       scanCount = int(laserCloud->points[i].intensity);//scanCount=scanID  
37.   
38.       // 记录每一层起始点和终止点的位置，需要根据这个起始/终止来操作点云曲率，在求曲率的过程中已经去除了前5个点和后5个点  
39.       if (scanCount > 0 && scanCount < N_SCANS) {  
40.         scanStartInd[scanCount] = i + 5;  
41.         scanEndInd[scanCount - 1] = i - 5;  
42.       }  
43.     }  
44.   }  

接下来，对提到的两种瑕点进行排除

[cpp] view plain copy

1. for (int i = 5; i < cloudSize - 6; i++) {  
2.    float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;  
3.    float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;  
4.    float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;  
5.    float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;  
6.   
7.    if (diff > 0.1) {  
8.   
9.      float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +   
10.                     laserCloud->points[i].y * laserCloud->points[i].y +  
11.                     laserCloud->points[i].z * laserCloud->points[i].z);  
12.   
13.      float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +   
14.                     laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +  
15.                     laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);  
16.   
17.      /*— 针对论文的(b)情况，两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点 —*/  
18.      /*— 构建了一个等腰三角形的底向量，根据等腰三角形性质，判断X[i]向量与X[i+1]的夹角小于5.732度(threshold=0.1) —*/  
19.      /*— depth1>depth2 X[i+1]距离更近，远侧点标记不特征；depth1<depth2 X[i]距离更近，远侧点标记不特征 —*/  
20.      if (depth1 > depth2) {  
21.        diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;  
22.        diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;  
23.        diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;  
24.   
25.        if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {  
26.          cloudNeighborPicked[i - 5] = 1;  
27.          cloudNeighborPicked[i - 4] = 1;  
28.          cloudNeighborPicked[i - 3] = 1;  
29.          cloudNeighborPicked[i - 2] = 1;  
30.          cloudNeighborPicked[i - 1] = 1;  
31.          cloudNeighborPicked[i] = 1;  
32.        }  
33.      } else {  
34.        diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;  
35.        diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;  
36.        diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;  
37.   
38.        if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {  
39.          cloudNeighborPicked[i + 1] = 1;  
40.          cloudNeighborPicked[i + 2] = 1;  
41.          cloudNeighborPicked[i + 3] = 1;  
42.          cloudNeighborPicked[i + 4] = 1;  
43.          cloudNeighborPicked[i + 5] = 1;  
44.          cloudNeighborPicked[i + 6] = 1;  
45.        }  
46.      }  
47.    }  
48.   
49.    /*— 针对论文的(a)情况，当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离） ———*/  
50.    /*— 若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点 ———————*/  
51.    /*—（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平） ———————————————*/  
52.   
53.    float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;  
54.    float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;  
55.    float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;  
56.    float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;  
57.   
58.    float dis = laserCloud->points[i].x * laserCloud->points[i].x  
59.              + laserCloud->points[i].y * laserCloud->points[i].y  
60.              + laserCloud->points[i].z * laserCloud->points[i].z;  
61.   
62.    if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {  
63.      cloudNeighborPicked[i] = 1;  
64.    }  
65.  }  

之后对平面点以及角点进行shai'xu

[cpp] view plain copy

1. pcl::PointCloud<PointType> cornerPointsSharp;  
2. pcl::PointCloud<PointType> cornerPointsLessSharp;  
3. pcl::PointCloud<PointType> surfPointsFlat;  
4. pcl::PointCloud<PointType> surfPointsLessFlat;  
5.   
6. for (int i = 0; i < N_SCANS; i++) {  
7.   /*—— 对于每一层激光点(总16层)，将每层区域分成6份，起始位置为sp，终止位置为ep。——————*/  
8.   /*—— 有两个循环，作用是对cloudCurvature从小到大进行排序，cloudSortedInd是它的索引数组 ————*/  
9.   pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);  
10.   for (int j = 0; j < 6; j++) {  
11.     int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;  
12.     int ep = (scanStartInd[i] * (5 - j)  + scanEndInd[i] * (j + 1)) / 6 - 1;  
13.   
14.     for (int k = sp + 1; k <= ep; k++) {  
15.       for (int l = k; l >= sp + 1; l--) {  
16.         if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {  
17.           int temp = cloudSortInd[l - 1];  
18.           cloudSortInd[l - 1] = cloudSortInd[l];  
19.           cloudSortInd[l] = temp;  
20.         }  
21.       }  
22.     }  
23.     /*—— 筛选特征角点 Corner: label=2; LessCorner: label=1 ————*/     
24.     int largestPickedNum = 0;  
25.     for (int k = ep; k >= sp; k--) {  
26.       int ind = cloudSortInd[k];  
27.       if (cloudNeighborPicked[ind] == 0 &&  
28.           cloudCurvature[ind] > 0.1) {  
29.   
30.         largestPickedNum++;  
31.         if (largestPickedNum <= 2) {  
32.           cloudLabel[ind] = 2;  
33.           cornerPointsSharp.push_back(laserCloud->points[ind]);  
34.           cornerPointsLessSharp.push_back(laserCloud->points[ind]);  
35.         } else if (largestPickedNum <= 20) {  
36.           cloudLabel[ind] = 1;  
37.           cornerPointsLessSharp.push_back(laserCloud->points[ind]);  
38.         } else {  
39.           break;  
40.         }  
41.   
42.         // 遍历该曲率点后，将该点标记，并将该曲率点附近的前后5个点标记不被选取为特征点  
43.         cloudNeighborPicked[ind] = 1;  
44.         for (int l = 1; l <= 5; l++) {  
45.           float diffX = laserCloud->points[ind + l].x   
46.                       - laserCloud->points[ind + l - 1].x;  
47.           float diffY = laserCloud->points[ind + l].y   
48.                       - laserCloud->points[ind + l - 1].y;  
49.           float diffZ = laserCloud->points[ind + l].z   
50.                       - laserCloud->points[ind + l - 1].z;  
51.           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {  
52.             break;  
53.           }  
54.   
55.           cloudNeighborPicked[ind + l] = 1;  
56.         }  
57.         for (int l = -1; l >= -5; l--) {  
58.           float diffX = laserCloud->points[ind + l].x   
59.                       - laserCloud->points[ind + l + 1].x;  
60.           float diffY = laserCloud->points[ind + l].y   
61.                       - laserCloud->points[ind + l + 1].y;  
62.           float diffZ = laserCloud->points[ind + l].z   
63.                       - laserCloud->points[ind + l + 1].z;  
64.           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {  
65.             break;  
66.           }  
67.   
68.           cloudNeighborPicked[ind + l] = 1;  
69.         }  
70.       }  
71.     }  
72.     /*—— 筛选特征平面点 Flat: label=-1 普通点和Flat点降采样形成LessFlat: label=0 ————*/  
73.     int smallestPickedNum = 0;  
74.     for (int k = sp; k <= ep; k++) {  
75.       int ind = cloudSortInd[k];  
76.       if (cloudNeighborPicked[ind] == 0 &&  
77.           cloudCurvature[ind] < 0.1) {  
78.   
79.         cloudLabel[ind] = -1;  
80.         surfPointsFlat.push_back(laserCloud->points[ind]);  
81.   
82.         smallestPickedNum++;  
83.         if (smallestPickedNum >= 4) {  
84.           break;  
85.         }  
86.   
87.         cloudNeighborPicked[ind] = 1;  
88.         for (int l = 1; l <= 5; l++) {  
89.           float diffX = laserCloud->points[ind + l].x   
90.                       - laserCloud->points[ind + l - 1].x;  
91.           float diffY = laserCloud->points[ind + l].y   
92.                       - laserCloud->points[ind + l - 1].y;  
93.           float diffZ = laserCloud->points[ind + l].z   
94.                       - laserCloud->points[ind + l - 1].z;  
95.           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {  
96.             break;  
97.           }  
98.   
99.           cloudNeighborPicked[ind + l] = 1;  
100.         }  
101.         for (int l = -1; l >= -5; l--) {  
102.           float diffX = laserCloud->points[ind + l].x   
103.                       - laserCloud->points[ind + l + 1].x;  
104.           float diffY = laserCloud->points[ind + l].y   
105.                       - laserCloud->points[ind + l + 1].y;  
106.           float diffZ = laserCloud->points[ind + l].z   
107.                       - laserCloud->points[ind + l + 1].z;  
108.           if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {  
109.             break;  
110.           }  
111.   
112.           cloudNeighborPicked[ind + l] = 1;  
113.         }  
114.       }  
115.     }  
116.   
117.     // surfPointsLessFlat为降采样后的flat点，采样前包含太多label=0的点  
118.     for (int k = sp; k <= ep; k++) {  
119.       if (cloudLabel[k] <= 0) {  
120.         surfPointsLessFlatScan->push_back(laserCloud->points[k]);  
121.       }  
122.     }  
123.   }  
124.   // 降采样  
125.   pcl::PointCloud<PointType> surfPointsLessFlatScanDS;  
126.   pcl::VoxelGrid<PointType> downSizeFilter;  
127.   downSizeFilter.setInputCloud(surfPointsLessFlatScan);  
128.   downSizeFilter.setLeafSize(0.2, 0.2, 0.2);  
129.   downSizeFilter.filter(surfPointsLessFlatScanDS);  
130.   
131.   surfPointsLessFlat += surfPointsLessFlatScanDS;  
132. }  

之后再将信息发布出去

·imuHandler

imuHandler功能为imu信息的解析：  

1. 减去重力对imu的影响 
2. 解析出当前时刻的imu时间戳，角度以及各个轴的加速度 
3. 将加速度转换到世界坐标系轴下 
4. 进行航距推算，假定为匀速运动推算出当前时刻的位置) 
5.  推算当前时刻的速度信息

[cpp] view plain copy

1. void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)  
2. {  
3.   double roll, pitch, yaw;  
4.   tf::Quaternion orientation;  
5.   tf::quaternionMsgToTF(imuIn->orientation, orientation);  
6.   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);  
7.   
8.   // 减去重力加速度的影响  
9.   float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;  
10.   float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;  
11.   float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;  
12.   
13.   // 表明数组是一个长度为imuQueLength的循环数组  
14.   imuPointerLast = (imuPointerLast + 1) % imuQueLength;  
15.   
16.   imuTime[imuPointerLast] = imuIn->header.stamp.toSec();  
17.   imuRoll[imuPointerLast] = roll;  
18.   imuPitch[imuPointerLast] = pitch;  
19.   imuYaw[imuPointerLast] = yaw;  
20.   imuAccX[imuPointerLast] = accX;  
21.   imuAccY[imuPointerLast] = accY;  
22.   imuAccZ[imuPointerLast] = accZ;  
23.   
24.   AccumulateIMUShift();  
25. }  
26. void AccumulateIMUShift()  
27. {  
28.   float roll = imuRoll[imuPointerLast];  
29.   float pitch = imuPitch[imuPointerLast];  
30.   float yaw = imuYaw[imuPointerLast];  
31.   float accX = imuAccX[imuPointerLast];  
32.   float accY = imuAccY[imuPointerLast];  
33.   float accZ = imuAccZ[imuPointerLast];  
34.   // 转换到世界坐标系下  
35.   float x1 = cos(roll) * accX - sin(roll) * accY;  
36.   float y1 = sin(roll) * accX + cos(roll) * accY;  
37.   float z1 = accZ;  
38.   
39.   float x2 = x1;  
40.   float y2 = cos(pitch) * y1 - sin(pitch) * z1;  
41.   float z2 = sin(pitch) * y1 + cos(pitch) * z1;  
42.   
43.   accX = cos(yaw) * x2 + sin(yaw) * z2;  
44.   accY = y2;  
45.   accZ = -sin(yaw) * x2 + cos(yaw) * z2;  
46.   
47.   // imuPointerBack 为 imuPointerLast-1， 这样处理是为了防止内存溢出  
48.   int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;  
49.   double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];  
50.   if (timeDiff < scanPeriod) {  
51.     // 推算当前时刻的位置信息  
52.     imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff   
53.                               + accX * timeDiff * timeDiff / 2;  
54.     imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff   
55.                               + accY * timeDiff * timeDiff / 2;  
56.     imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff   
57.                               + accZ * timeDiff * timeDiff / 2;  
58.     // 推算当前时刻的速度信息  
59.     imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;  
60.     imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;  
61.     imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;  
62.   }  
63. }  

后面涉及的坐标变换即欧拉角的旋转计算，这部分尚不熟悉，需要进一步学习。

·小结

以上数据传输流帮助理解这个模块源代码的功能。