0. 简介

在讲述完一，二后IMU和雷达的数据输入算是讲明白了，下面几节我们将结合主程序来介绍本文最关键的迭代卡尔曼滤波器和iKD-Tree的算法。

1. 主程序

一开始在laserMapping.cpp主程序中定义了一系列待使用的变量定义，包含了ESKF和IKF-Tree的定义

#define INIT_TIME (0.1)
#define LASER_POINT_COV (0.001)
#define MAXN (720000)
#define PUBFRAME_PERIOD (20)

/*** Time Log Variables ***/
// kdtree_incremental_time为kdtree建立时间，kdtree_search_time为kdtree搜索时间，kdtree_delete_time为kdtree删除时间;
double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
// T1为雷达初始时间戳，s_plot为整个流程耗时，s_plot2特征点数量,s_plot3为kdtree增量时间，s_plot4为kdtree搜索耗时，s_plot5为kdtree删除点数量
//，s_plot6为kdtree删除耗时，s_plot7为kdtree初始大小，s_plot8为kdtree结束大小,s_plot9为平均消耗时间，s_plot10为添加点数量，s_plot11为点云预处理的总时间
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];

// 定义全局变量，用于记录时间,match_time为匹配时间，solve_time为求解时间，solve_const_H_time为求解H矩阵时间
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
// kdtree_size_st为ikd-tree获得的节点数，kdtree_size_end为ikd-tree结束时的节点数，add_point_size为添加点的数量，kdtree_delete_counter为删除点的数量
int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
// runtime_pos_log运行时的log是否开启，pcd_save_en是否保存pcd文件，time_sync_en是否同步时间
bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false;
/**************************/

float res_last[100000] = {0.0};   //残差，点到面距离平方和
float DET_RANGE = 300.0f;         //设置的当前雷达系中心到各个地图边缘的距离
const float MOV_THRESHOLD = 1.5f; //设置的当前雷达系中心到各个地图边缘的权重

mutex mtx_buffer;              // 互斥锁
condition_variable sig_buffer; // 条件变量

string root_dir = ROOT_DIR;                 //设置根目录
string map_file_path, lid_topic, imu_topic; //设置地图文件路径，雷达topic，imu topic

double res_mean_last = 0.05, total_residual = 0.0;                                                 //设置残差平均值，残差总和
double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;                                        //设置雷达时间戳，imu时间戳
double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;                       //设置imu的角速度协方差，加速度协方差，角速度协方差偏置，加速度协方差偏置
double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; //设置滤波器的最小尺寸，地图的最小尺寸，视野角度

//设置立方体长度，视野一半的角度，视野总角度，总距离，雷达结束时间，雷达初始时间
double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;
//设置有效特征点数，时间log计数器, scan_count：接收到的激光雷达Msg的总数，publish_count：接收到的IMU的Msg的总数
int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
//设置迭代次数，下采样的点数，最大迭代次数，有效点数
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;
bool point_selected_surf[100000] = {0}; // 是否为平面特征点
// lidar_pushed：用于判断激光雷达数据是否从缓存队列中拿到meas中的数据, flg_EKF_inited用于判断EKF是否初始化完成
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
//设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;

vector<vector<int>> pointSearchInd_surf;      //每个点的索引,暂时没用到
vector<BoxPointType> cub_needrm;              // ikd-tree中，地图需要移除的包围盒序列
vector<PointVector> Nearest_Points;           //每个点的最近点序列
vector<double> extrinT(3, 0.0);               //雷达相对于IMU的外参T
vector<double> extrinR(9, 0.0);               //雷达相对于IMU的外参R
deque<double> time_buffer;                    // 激光雷达数据时间戳缓存队列
deque<PointCloudXYZI::Ptr> lidar_buffer;      //记录特征提取或间隔采样后的lidar（特征）数据
deque<sensor_msgs::Imu::ConstPtr> imu_buffer; // IMU数据缓存队列

//一些点云变量
PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());           //提取地图中的特征点，IKD-tree获得
PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());        //去畸变的特征
PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());        //畸变纠正后降采样的单帧点云，lidar系
PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());       //畸变纠正后降采样的单帧点云，w系
PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));       //特征点在地图中对应点的，局部平面参数,w系
PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); // laserCloudOri是畸变纠正后降采样的单帧点云，body系
PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); //对应点法相量
PointCloudXYZI::Ptr _featsArray;                                  // ikd-tree中，map需要移除的点云序列

//下采样的体素点云
pcl::VoxelGrid<PointType> downSizeFilterSurf; //单帧内降采样使用voxel grid
pcl::VoxelGrid<PointType> downSizeFilterMap;  //未使用

KD_TREE ikdtree; // ikd-tree类

V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);  //雷达相对于body系的X轴方向的点
V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于world系的X轴方向的点
V3D euler_cur;                                //当前的欧拉角
V3D position_last(Zero3d);                    //上一帧的位置
V3D Lidar_T_wrt_IMU(Zero3d);                  // T lidar to imu (imu = r * lidar + t)
M3D Lidar_R_wrt_IMU(Eye3d);                   // R lidar to imu (imu = r * lidar + t)

/*** EKF inputs and output ***/
// ESEKF操作
MeasureGroup Measures;
esekfom::esekf<state_ikfom, 12, input_ikfom> kf; // 状态，噪声维度，输入
state_ikfom state_point;                         // 状态
vect3 pos_lid;                                   // world系下lidar坐标

//输出的路径参数
nav_msgs::Path path;                      //包含了一系列位姿
nav_msgs::Odometry odomAftMapped;         //只包含了一个位姿
geometry_msgs::Quaternion geoQuat;        //四元数
geometry_msgs::PoseStamped msg_body_pose; //位姿

//激光和imu处理操作
shared_ptr<Preprocess> p_pre(new Preprocess()); // 定义指向激光雷达数据的预处理类Preprocess的智能指针
shared_ptr<ImuProcess> p_imu(new ImuProcess()); // 定义指向IMU数据预处理类ImuProcess的智能指针
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

下面的两个函数就比较简单，分别是接受到中断信号会触发以及将详细的信息打印到log中以便回溯

//按下ctrl+c后唤醒所有线程
void SigHandle(int sig)
{
    flg_exit = true;
    ROS_WARN("catch sig %d", sig);
    sig_buffer.notify_all(); //  会唤醒所有等待队列中阻塞的线程 线程被唤醒后，会通过轮询方式获得锁，获得锁前也一直处理运行状态，不会被再次阻塞。
}

//将fast lio2信息打印到log中
inline void dump_lio_state_to_log(FILE *fp)
{
    V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
    fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
    fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2));                            // Angle
    fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2));    // Pos
    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                                 // omega
    fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2));    // Vel
    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                                 // Acc
    fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2));       // Bias_g
    fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2));       // Bias_a
    fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a
    fprintf(fp, "\r\n");
    fflush(fp);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

然后就是一系列的点云转换公式，基本上就是将雷达的坐标转到世界坐标系中，或者将雷达坐标系转化到IMU坐标系中

//把点从body系转到world系，通过ikfom的位置和姿态
void pointBodyToWorld_ikfom(PointType const *const pi, PointType *const po, state_ikfom &s)
{
    V3D p_body(pi->x, pi->y, pi->z);
    //下面式子最里面的括号是从雷达到IMU坐标系 然后从IMU转到世界坐标系
    V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos);

    po->x = p_global(0);
    po->y = p_global(1);
    po->z = p_global(2);
    po->intensity = pi->intensity;
}
//把点从body系转到world系
void pointBodyToWorld(PointType const *const pi, PointType *const po)
{
    V3D p_body(pi->x, pi->y, pi->z);
    V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);

    po->x = p_global(0);
    po->y = p_global(1);
    po->z = p_global(2);
    po->intensity = pi->intensity;
}
//把点从body系转到world系
template <typename T>
void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
{
    V3D p_body(pi[0], pi[1], pi[2]);
    V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);

    po[0] = p_global(0);
    po[1] = p_global(1);
    po[2] = p_global(2);
}
// 含有RGB的点云从body系转到world系
void RGBpointBodyToWorld(PointType const *const pi, PointType *const po)
{
    V3D p_body(pi->x, pi->y, pi->z);
    V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);

    po->x = p_global(0);
    po->y = p_global(1);
    po->z = p_global(2);
    po->intensity = pi->intensity;
}
// 含有RGB的点云从Lidar系转到IMU系
void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
{
    V3D p_body_lidar(pi->x, pi->y, pi->z);
    V3D p_body_imu(state_point.offset_R_L_I * p_body_lidar + state_point.offset_T_L_I);

    po->x = p_body_imu(0);
    po->y = p_body_imu(1);
    po->z = p_body_imu(2);
    po->intensity = pi->intensity;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

然后在下面就是一个重构IKD-Tree过滤后剔除的点集

//得到被剔除的点
void points_cache_collect()
{
    PointVector points_history;
    ikdtree.acquire_removed_points(points_history); //返回被剔除的点
    for (int i = 0; i < points_history.size(); i++)
        _featsArray->push_back(points_history[i]); //存入到缓存中，后面没有用到该数据
}

1
2
3
4
5
6
7
8
9

2. 动态调整地图

地图点被组织成一个ikd-Tree，该树以里程计速率合并新的点云扫描而动态增长。为了防止地图的大小不受约束，ikid - tree中只保留LiDAR当前位置周围长为L的大型局部区域中的地图点。2D 演示图如下所示。

(a)中，蓝色矩形为长度为l的初始映射区域，红色圆形为以初始LiDAR位置$p_0$为中心的初始探测区域。在(b)中，检测区域(虚线红色圆圈)移动到一个新的位置$p^{{}^{\prime }}$(带实红线的圆圈)，该位置触碰了地图边界。根据距离$d$，地图区域被移动到一个新的位置(绿色矩形)。减法区域(橙色区域)中的点从地图(即 ikd-Tree)中移除。

将映射区域初始化为一个长为L的立方体，该立方体以初始LiDAR位置$p_0$为中心。假设LiDAR的探测区域为以(15)更新得到位置的LiDAR当前位置为中心的探测球。

假设探测球的半径$r=\gamma R$，其中R为LiDAR视场(FOV)范围，$\gamma$为大于1的松弛参数。当LiDAR移动到检测球接触到地图边界的新位置p0时，地图区域会向增加LiDAR检测区域到接触边界的距离的方向移动。地图区域移动的距离设置为常数$d=(\gamma -1)R$。通过后中详细介绍的逐框删除操作，新映射区域和旧映射区域之间的减法区域中的所有点将从ikd-Tree中删除。

…详情请参照古月居