1. 引言
LeGO-LOAM源码:LeGO-LOAM
github注释后LeGO-LOAM源码:LeGO-LOAM_NOTED
LeGO-LOAM在LOAM的基础上增加了回环检测,它分别由点云分割、特征提取、激光里程计、激光建图组成,系统通过接收来自三维激光雷达的输入并输出6自由度姿态估计。
整个系统分为五个模块:
(1)第一个是分割,将单个扫描的点云投影到一个固定范围的图像上进行分割;
(2)第二个是将分割的点云发送到特征提取模块;
(3)第三个是激光雷达里程计使用从上一个模块中提取的特征找到在连续扫描过程中机器人位姿的转换;
(4)第四个是这些信息最终用于激光雷达用点云方式的建图中;
(5)第五个模块是融合激光雷达里程测量和建图的姿态估计结果,并输出最终的姿态估计。
节点和话题关系图:
2.CMakeLists.txt
link_directories(
include
${OpenCV_LIBRARY_DIRS}
${PCL_LIBRARY_DIRS}
${GTSAM_LIBRARY_DIRS}
)
依赖于PCL、OpenCV、GTSAM三个库
GTSAM是一个在机器人领域和计算机视觉领域用于平滑(smoothing)和建图(mapping)的C++库。它与g2o不同的是,g2o采用稀疏矩阵的方式求解一个非线性优化问题,而GTSAM是采用因子图(factor graphs)和贝叶斯网络(Bayes networks)的方式最大化后验概率。
在这个工程中,具备了imageProjection(图像投影),featureAssociation(特征关联),mapOptmization(地图优化),transformFusion(位姿优化)四个可执行文件
3.utility.h
唯一的头文件utility.h定义了一些普适的东西
首先PointType是带intensity的PointXYZ:
typedef pcl::PointXYZI PointType;
这些参数是根据雷达品牌来定的,比如这是16线、每线1800个点的数据:
// VLP-16
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;//水平上每条线间隔0.2°
extern const float ang_res_y = 2.0;//竖直方向上每条线间隔2°
extern const float ang_bottom = 15.0+0.1;//竖直方向上起始角度是负角度,与水平方向相差15.1°
extern const int groundScanInd = 7;//以多少个扫描圈来表示地面
点云分割时的必要参数:
extern const float sensorMountAngle = 0.0;
extern const float segmentTheta = 1.0472;//点云分割时的角度跨度上限(π/3)
extern const int segmentValidPointNum = 5;//检查上下左右连续5个点做为分割的特征依据
extern const int segmentValidLineNum = 3;
而 PointTypePose指的是具备姿态角的特定点:
struct PointXYZIRPYT//定义点类型结构
{
PCL_ADD_POINT4D// 该点类型有4个元素
PCL_ADD_INTENSITY;
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 确保new操作符对齐操作
} EIGEN_ALIGN16;// 强制SSE对齐
typedef PointXYZIRPYT PointTypePose;