一、utility.h源码注释
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include "cloud_msgs/cloud_info.h"
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/range_image/range_image.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/registration/icp.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <vector>
#include <cmath>
#include <algorithm>
#include <queue>
#include <deque>
#include <iostream>
#include <fstream>
#include <ctime>
#include <cfloat>
#include <iterator>
#include <sstream>
#include <string>
#include <limits>
#include <iomanip>
#include <array>
#include <thread>
#include <mutex>
#define PI 3.14159265
using namespace std;
typedef pcl::PointXYZI PointType;
//在C++中，extern关键字用于声明一个变量或函数是在另一个文件中定义的。这允许你在多个源文件中使用同一个全局变量或函数，而不需要在每个文件中都重新定义它们。
extern const string pointCloudTopic = "/velodyne_points";
extern const string imuTopic = "/imu/data";
// Save pcd
extern const string fileDirectory = "/tmp/";
//在Linux和类Unix操作系统中，/tmp是一个特殊的目录，用于存储临时文件。这些文件通常由系统或正在运行的程序创建，并在一定时间后自动删除。/tmp目录通常位于文件系统的根目录下。
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used
以下均为扫描仪的参数：我使用的是hesai Pandar-XT32。
// VLP-16
//extern const int N_SCAN = 16;
//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;
//Hesai Pandar-XT32
extern const int N_SCAN = 32;
extern const int Horizon_SCAN = 2000;
extern const float ang_res_x = 0.18;
extern const float ang_res_y = 1.0;
extern const float ang_bottom = 15+0.1;
extern const int groundScanInd =12;
// 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;
// VLS-128
// extern const int N_SCAN = 128;
// extern const int Horizon_SCAN = 1800;
// extern const float ang_res_x = 0.2;
// extern const float ang_res_y = 0.3;
// extern const float ang_bottom = 25.0;
// extern const int groundScanInd = 10;
// Ouster users may need to uncomment line 159 in imageProjection.cpp
// Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
// Ouster OS1-16
// extern const int N_SCAN = 16;
// extern const int Horizon_SCAN = 1024;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 33.2/float(N_SCAN-1);
// extern const float ang_bottom = 16.6+0.1;
// extern const int groundScanInd = 7;
// Ouster OS1-64
// extern const int N_SCAN = 64;
// extern const int Horizon_SCAN = 1024;
// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
// extern const float ang_res_y = 33.2/float(N_SCAN-1);
// extern const float ang_bottom = 16.6+0.1;
// extern const int groundScanInd = 15;// N_SCAN：表示激光雷达每次扫描的线束数量。
//ang_res_x：表示激光雷达在水平方向上的角分辨率。
//ang_res_y：表示激光雷达在垂直方向上的角分辨率。
//Horizon_SCAN：每条扫描线上激光的点数。360°/ang_res_x。
//ang_bottom：表示激光雷达的最低扫描角度。
extern const bool loopClosureEnableFlag = false; //未启动回环检测
extern const double mappingProcessInterval = 0.3;//建图的间隔0.3s。
extern const float scanPeriod = 0.1;//扫描周期0.1s。
extern const int systemDelay = 0;
extern const int imuQueLength = 200;
//imuQueLength：表示惯性测量单元（IMU）数据队列的长度。这个值决定了可以存储多少IMU数据。
extern const float sensorMinimumRange = 1.0; //最小测距距离1米。
extern const float sensorMountAngle = 0.0;
//sensorMountAngle：表示激光雷达安装的角度。这个值用于校正激光雷达数据，以确保数据的准确性。
extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
//将水平扫描点云分为六个部分。
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; //表示搜索最近特征时的最大平方距离。
// Mapping Params
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)
//文章中给了两种方法，第一种是类似loam的方式
In the first approach, Q(t-1) feature sets that are in the field of view of the sensor. For simplicity, we can choose the feature sets whose sensor poses are within 100m of the current position of the sensor. The chosen feature sets are then transformed and fused into a single surrounding map Q(t-1) . This map selection technique is similar to the method used in.
extern const int surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
//第二种是lego-loam的回环检测方法。具体实现还是看后面的代码。
// 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
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
struct smoothness_t{
float value;
size_t ind;};
struct by_value{
bool operator()(smoothness_t const &left, smoothness_t const &right) {
return left.value < right.value;}};
//定义了两个结构体，smoothness_t 和 by_value。smoothness_t 结构体用于存储一个浮点值 value 和一个大小类型 size_t 的索引 ind。by_value 结构体则定义了一个重载的函数调用运算符 operator()，这个运算符用于比较两个 smoothness_t 类型的对象。
/*
* A point cloud type that has "ring" channel
//两种不同的点云格式对应不同的激光雷达，对于Velodyne VLP-16这种激光雷达的数据结构就是XYZIR。
*/
struct PointXYZIR
{
PCL_ADD_POINT4D //这是一个宏，用于添加四个浮点数表示坐标
PCL_ADD_INTENSITY; //表示点强度,这里的强度也是用来计算时间戳的。
uint16_t ring; //定义了一个无符号16位整数 ring，用于存储点所在的环编号。在一些激光雷达系统中，不同的环编号代表不同的扫描层或不同的方位角，这个字段有助于区分这些不同的层或角度。
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//用于确保在创建新对象时，对象的内存对齐符合Eigen库的要求。Eigen是一个高效的数学库，用于线性代数、矩阵和向量运算。这个宏确保了 PointXYZIR 结构体的对象在内存中正确对齐，以提高性能。
} EIGEN_ALIGN16;
//在PCL（点云库）中，EIGEN_ALIGN16 宏用于确保结构体在内存中以16字节对齐。这种对齐方式是为了兼容SIMD指令，比如SSE（Streaming SIMD Extensions）指令集，它们通常要求数据以16字节对齐。
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring)
)
在点云库（PCL）中，`POINT_CLOUD_REGISTER_POINT_STRUCT` 宏用于注册自定义的点类型，使其能够在PCL中使用，并且能够与PCL的序列化和反序列化机制兼容。这个宏允许PCL知道如何读取和写入自定义点类型，包括哪些字段是存在的，以及它们的类型和名称。
这个宏的具体参数如下：
- `PointXYZIR`：这是自定义点类型的名称。
- `(float, x, x)`：这指定了一个名为 `x` 的浮点数字段，用于存储点的x坐标。
- `(float, y, y)`：这指定了一个名为 `y` 的浮点数字段，用于存储点的y坐标。
- `(float, z, z)`：这指定了一个名为 `z` 的浮点数字段，用于存储点的z坐标。
- `(float, intensity, intensity)`：这指定了一个名为 `intensity` 的浮点数字段，通常用于存储点的强度信息。
- `(uint16_t, ring, ring)`：这指定了一个名为 `ring` 的16位无符号整数字段，用于存储与点相关的额外信息，比如在激光雷达扫描中的环编号。
这个宏的工作原理是，它在编译时生成额外的代码，这些代码定义了如何处理 `PointXYZIR` 类型的点云数据。这包括如何访问点云中的每个字段，以及如何将这些字段映射到PCL内部的数据结构。
注册点类型后，PCL的点云容器（如 `pcl::PointCloud<PointXYZIR>`）就能够正确地处理 `PointXYZIR` 类型的点，包括在读写点云文件时正确地序列化和反序列化这些点。
例如，如果你有一个点云文件包含 `PointXYZIR` 类型的点，PCL的I/O函数（如 `pcl::io::loadPCDFile` 和 `pcl::io::savePCDFile`）就能够自动识别 `PointXYZIR` 类型，并正确地读取和写入点云数据。
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
float roll;
float pitch;
float yaw;
double time;？？为什么还有一个time
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
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;
??PointXYZIR为什么没定义一个PointTypePose;
#endif