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. intintchar
3. "scanRegistration"
5. /*
12. /*
20. "/velodyne_points"
21. "/imu/data"
23. /*
43. "/velodyne_cloud_2"
46. "/laser_cloud_sharp"
49. "/laser_cloud_less_sharp"
52. "/laser_cloud_flat"
55. "/laser_cloud_less_flat"
57. "/imu_trans"
59. /**
65. return

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

laserCloudHandler与imuHandler。

・laserHandler

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

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

[cpp] view plain copy

1. voidconst
3. if
5. if
7. true
9. return
12. int
13. int
16. double
21. int
22. //去除无效值
24. int
26. //计算点云的起始角度/终止角度
27. float
28. float

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

[cpp] view plain copy

1. forint
6. float
7. int
8. intint
9. if
12. else
16. if
19. continue

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

[cpp] view plain copy

1. float
2. if
3. if
5. elseif
9. if
10. true
12. else
15. if
17. elseif
22. //scanPeriod=0.1，是因为lidar工作周期是10HZ，意味着转一圈是0.1秒；intensity是一个整数+小数，小数不会超过0.1，完成了按照时间排序的需求
23. float
28. if
29. float
30. while
33. if
34. break
39. if
53. else
56. int
57. float
59. float
64. if
66. elseif
68. else
80. if
92. else

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

[cpp] view plain copy

3. new
4. //将所有点存入laserCloud中，点按线序进行排列
5. forint
8. int
10. forint
12. float
18. float
24. float
35. ifint
36. int(laserCloud->points[i].intensity);//scanCount=scanID
39. if

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

[cpp] view plain copy

1. forint
2. float
3. float
4. float
5. float
7. if
9. float
13. float
20. if
25. if
33. else
38. if
53. float
54. float
55. float
56. float
58. float
62. if

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

[cpp] view plain copy

6. forint
9. new
10. forint
11. int
12. int
14. forint
15. forint
16. if
17. int
24. int
25. forint
26. int
27. if
31. if
35. elseif
38. else
39. break
44. forint
45. float
47. float
49. float
51. if
52. break
57. forint
58. float
60. float
62. float
64. if
65. break
73. int
74. forint
75. int
76. if
83. if
84. break
88. forint
89. float
91. float
93. float
95. if
96. break
101. forint
102. float
104. float
106. float
108. if
109. break
118. forint
119. if

之后再将信息发布出去

・imuHandler

[cpp] view plain copy

1. voidconst
3. double
9. float
10. float
11. float
26. void
28. float
29. float
30. float
31. float
32. float
33. float
35. float
36. float
37. float
39. float
40. float
41. float
48. int
49. double
50. if

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

・小结

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

文章来源: SLAM：loam算法架构分析