LeGO-LOAM源码解析1 ： 算法整体框架和utility.h

loam源码地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.

论文学习：【论文阅读】LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain.

LeGO-LOAM源码解析汇总：

一、算法框架与流程

回顾论文，我们可以将整个代码的流程分为10HZ点云输入部分、五个处理部分和10HZ位姿估计输出部分。五个处理部分由点云分割、特征提取、高频激光里程计、低频激光建图和变换融合组成。

在代码中包含imageProjection、featureAssociation、mapOptmization、transformFusion四个cpp文件和一个头文件：utility.h。

通过对节点和话题关系图的查看，我们可以大概看出代码的执行逻辑如下：

imageProjection

featureAssociation

mapOptmization

transformFusion

所以我们的代码分析也会按照这个逻辑逐一展开。在这里提供一份大佬注释好的LeGo-LOAM的代码，链接如下： https://github.com/wykxwyc/LeGO-LOAM_NOTED。接下来我们的工作都会以这个为基础展开。

二、utility.h分析

utility.h作为代码中唯一的头文件，它的意义在于定义一些基本的使用变量和结构体。

首先为了简化代码，将常用的带强度信息的点云类型pcl::PointXYZI定义为PointType

typedef pcl::PointXYZI  PointType;

定义点云和imu的话题名称：

extern const string pointCloudTopic = "/velodyne_points";
extern const string imuTopic = "/imu/data";

在新的代码中作者提供了CloudRing 的使用标志以帮助进行点云投影（如 VLP-32C、VLS-128）。 Velodyne 点云具有“环”通道，可直接给出范围图像中的点行 id。 其他激光雷达可能具有相同类型的通道，即 Ouster 中的“r”。 如果您使用的是非 Velodyne 激光雷达，但它具有类似的“环形”通道，则可以更改 Utility.h 中的 PointXYZIR 定义和 imageProjection.cpp 中的相应代码:

extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used

接下来作者提供了多种类型激光雷达的使用配置如VLP-16、HDL-32E、VLS-128、Ouster OS1-16等等，默认使用VLP-16。具体注释如下：

// VLP-16
//激光线束
extern const int N_SCAN = 16;
//水平方向激光点 360/水平分辨率
extern const int Horizon_SCAN = 1800;
//水平分辨率
extern const float ang_res_x = 0.2;
//垂直分辨率
extern const float ang_res_y = 2.0;
//与与水平方向的夹角
extern const float ang_bottom = 15.0+0.1;
//扫描到地面的激光线数
extern const int groundScanInd = 7;

// HDL-32E
// extern const int N_SCAN = 32;
// extern const int Horizon_SCAN = 1800;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 41.33/float(N_SCAN-1);
// extern const float ang_bottom = 30.67;
// extern const int groundScanInd = 20;

默认关闭回环：

extern const bool loopClosureEnableFlag = false;

建图处理时间间隔，只有时间间隔大于这个值才能进行建图优化：

extern const double mappingProcessInterval = 0.3;

处理频率0.1s（10HZ）及IMU队列长度

extern const float scanPeriod = 0.1;
extern const int systemDelay = 0;
extern const int imuQueLength = 200;

点云分割的一些设置：

extern const float sensorMinimumRange = 1.0;
extern const float sensorMountAngle = 0.0;
//点云分割的跨度60度，在imageProjection中用于判断平面
extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
//检查上下左右连续5个点做为分割的特征依据
extern const int segmentValidPointNum = 5;
extern const int segmentValidLineNum = 3;
extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;

特征点选取的一些设置：

//点特征数
extern const int edgeFeatureNum = 2;
//面特征数
extern const int surfFeatureNum = 4;
//总特征数
extern const int sectionsTotal = 6;
extern const float edgeThreshold = 0.1;
extern const float surfThreshold = 0.1;
//特征选取间隔
extern const float nearestFeatureSearchSqDist = 25;

建图参数：

//选取关键帧的范围（不使用回环）
extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
//子图关键帧数目（使用回环）
extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)

// history key frames (history submap for loop closure)
//考虑回环成立的阈值
extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
//回环子图的构建关键帧数目2n+1
extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure

extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized

自定义 PointTypePose类型指的是具备姿态角的特定点：

struct PointXYZIRPYT
{
	// 该点类型有4个元素
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    // 确保new操作符对齐操作
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;// 强制SSE对齐

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
                                   (double, time, time)
)

typedef PointXYZIRPYT  PointTypePose;