FAST-LIO2 学习

预计完成时间：12月10日

这里选择了https://github.com/shijiehua/Fast_Lio_Change 的 Fast-Lio-Change 框架作为我们的分析对象。Fast-Lio-Change 框架在原版的基础上重构了代码，并且添加了比较详尽的注释，比较适合我这样的初学者阅读。

参数读取模块

在我们的实例对象 FastLioProcess 开始构造时，我们需要先进行一遍基本参数的初始化设置。如果我们打开 FastLioProcess.h 文件可以看到很多参数类型已经提前声明在类里面了。参数读取模块顾名思义就是把我们写在 .yaml 文件里面的参数读取到程序中。SLAM 本身是一个比较工程化的项目，很多框架都依赖一些先验的参数（调参很重要），比如 Lidar 外参、IMU 外参这些东西。所以这里虽然比较臃肿，但还是有必要的。

一般来说这些 SLAM 框架的参数读取部分都大同小异，利用 ROS 的参数服务器进行合理的参数赋值。我在之后如果想要在此基础上进行修改，也不需要从头开始写，只需要添加我自己的参数即可。

代码实现

 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


FastLioProcess::FastLioProcess(ros::NodeHandle &nh)
    : extrinT(3, 0.0),  // 初始化平移向量（3个元素，值为0.0）
      extrinR(9, 0.0)   // 初始化旋转矩阵（9个元素，值为0.0）
{   
    // 从参数服务器读取参数值赋给变量（包括launch文件和launch读取的yaml文件中的参数）
    nh.param<bool>("publish/path_en",path_en, true);  
    nh.param<bool>("publish/scan_publish_en",scan_pub_en, true);   // 是否发布当前正在扫描的点云的topic
    nh.param<bool>("publish/dense_publish_en",dense_pub_en, true);    // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，
    nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);  // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布
    nh.param<int>("max_iteration",max_iterations,4);  // 卡尔曼滤波的最大迭代次数
    nh.param<string>("map_file_path",map_file_path,"");  // 地图保存路径
    nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");  // 激光雷达点云topic名称
    nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");   // IMU的topic名称
    nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
    nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);  // 单帧点云降采样时的体素大小
    nh.param<double>("filter_size_map",filter_size_map_min,0.5);   // VoxelGrid降采样时的体素大小
    nh.param<double>("cube_side_length",cube_len,200);    // 地图的局部区域的长度（FastLio2论文中有解释
    nh.param<float>("mapping/det_range",DET_RANGE,300.f);  // 激光雷达的最大探测范围
    nh.param<double>("mapping/fov_degree",fov_deg,180);  // 视场角  通常用于描述传感器（可能是激光雷达等）的视野范围角度。
    nh.param<double>("mapping/gyr_cov",gyr_cov,0.1);  // IMU陀螺仪的协方差
    nh.param<double>("mapping/acc_cov",acc_cov,0.1);   // IMU加速度计的协方差
    nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);  // IMU陀螺仪偏置的协方差
    nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);   // IMU加速度计偏置的协方差
    nh.param<double>("preprocess/blind", blind, 0.01);  // 最小距离阈值，即过滤掉0～blind范围内的点云
    nh.param<int>("preprocess/scan_line", N_SCANS, 16);   // 激光雷达扫描的线数（livox avia为6线）
    nh.param<int>("point_filter_num", point_filter_num, 2);   // 采样间隔，即每隔point_filter_num个点取1个点
    nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);   // 是否将点云地图保存到PCD文件
    nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
    nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());  // 外参平移向量  IMU 和 LiDAR 之间的相对平移关系
    nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());  // 外参旋转矩阵  IMU 和 LiDAR 之间的旋转关系矩阵
    nh.param<bool>("common/lidar_en", lidar_en, false);
    nh.param<bool>("common/imu_en", imu_en, false);

    // 节点接收器和发布器的定义和初始化
    sub_pcl =nh.subscribe(lid_topic, 200000, &FastLioProcess::livox_pcl_cbk,this);
    sub_imu = nh.subscribe(imu_topic, 200000, &FastLioProcess::imu_cbk,this);
    pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/Odometry", 100000);  // 发布当前里程计信息，topic名字为/Odometry  ，发布机器人位姿的，用于rviz建图
    pubPath = nh.advertise<nav_msgs::Path> ("/path", 100000);  // 发布里程计总的路径，topic名字为/path   发布机器人的路径，用于rviz建图
    pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100000);  // 发布当前正在扫描的点云，topic名字为/cloud_registered
    // 设置 state 参数  外参
    state.offset_R_L_I<<MAT_FROM_ARRAY(extrinR);
    state.offset_T_L_I<<VEC_FROM_ARRAY(extrinT);
    cov_bias_gyr = {b_gyr_cov,b_gyr_cov,b_gyr_cov};
    cov_bias_acc = {b_acc_cov,b_acc_cov,b_acc_cov};
    // 设置点云下采样
    downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
    // 保存点云
    save_root = string(string(ROOT_DIR) + "PCD/");
    createDirectoryIfNotExists(save_root);
    string save_scan_path = string(string(ROOT_DIR) + "PCD/scan.txt");
    fs::remove(save_scan_path);
    fout_cloud.open(save_scan_path, ios::app); 
}

这里参数基本可以分为四大类：

硬件适配类（“不同的车，不同的雷达”）

比如 extrinT, extrinR 这样的外参，除此之外还有激光雷达的线数、各种传感器的协方差等等。每次更换硬件进行 SLAM 处理都需要修改这些参数。
算力与性能平衡类

这些参数一般是我们算法实现的具体超参数。例如 filter_size_surf 是我们的激光点云降采样体素大小， max_iterations 是最大迭代次数。
环境适应类

例如 cube_side_length 和 blind ，分别代表局部地图大小和盲区距离（其实后者更偏向于硬件适配）。我们需要根据具体的环境应用场景来做参数调试校准。
调试与可视化类

publish/scan_publish_en, pcd_save_en 这些，一般是用来 debug 的。

我们重点看发布器和接受器的定义和初始化：

1
2


sub_pcl =nh.subscribe(lid_topic, 200000, &FastLioProcess::livox_pcl_cbk,this);    
sub_imu = nh.subscribe(imu_topic, 200000, &FastLioProcess::imu_cbk,this);

这里在接收到 Lidar 和 IMU 的 topic 之后，ROS 会自动调用下面的两个回调函数： livox_pcl_cbk 和 imu_cbk 。我们可以想像，之后应该是在这两个函数里面进行处理。首先可能要先解包，把 ROS 的数据格式转换为我们自定义的数据格式，然后把它放到我们的指定队列里面，供后续的去畸变模块来处理。

IMU 回调

一句话概括：复制 ROS 传递的 IMU 信息，放到一个 deque 里面。

这里很显然就是把 ROS 的 IMU message 放到我们的指定队列里面。仔细一看，确实，我们用到了一个叫做 imu_buffer 的妙妙工具，它在头文件里面的定义是： deque<sensor_msgs::Imu::ConstPtr> imu_buffer; 所以这个函数实际上放到队列里面的是 IMU 信息的指针。

sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); 这一句深拷贝了 ROS 回调出来的参数，这里需要调用 sensor_msgs::Imu 的拷贝构造函数。为什么不直接转移所有权呢？原因是通常在 ROS 的回调函数中，我们收到的消息是 const 指针（只读的），所以我们要深拷贝一份来作修改和下一步处理。

imu_temp 主要是用来去除异常数据的，可能会有 IMU 数据为 0 或者和上一帧重复之类的事情发生。这里还有一个特别神奇的操作：如果当前IMU的时间戳小于上一个时刻IMU的时间戳，则IMU数据有误，将IMU数据缓存队列清空，重新放数据。估计也是工程实践里面 debug 得到的宝贵教训吧。

既然涉及到多线程存取操作，那就离不开锁了。函数里很奇怪没有用 lock_guard 而是直接用了锁，那涉及到异常就可能会有死锁了，好孩子不要学。之后就是 sig_buffer.notify_all(); 告诉所有线程我执行完了，快来抢锁！

代码实现

 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


void FastLioProcess::imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) {
    if (!imu_en) return;
    sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));   // 单帧imu的数据
    //去除无效数据
    // if (msg->linear_acceleration.x==0 && msg->linear_acceleration.y==0 && msg->linear_acceleration.z==0){
    //     cout<<"0值，跳过imu时间："<<msg_in->header.stamp.toSec()<<endl;
    //   return;
    // } 
    // if (imu_tmp != nullptr && msg->linear_acceleration.x==imu_tmp->linear_acceleration.x && msg->linear_acceleration.y==imu_tmp->linear_acceleration.y && msg->linear_acceleration.z==imu_tmp->linear_acceleration.z){
    //     cout<<"重复，跳过imu时间："<<msg_in->header.stamp.toSec()<<endl;
    //   return;
    // }
    imu_tmp = msg;
    double timestamp = msg->header.stamp.toSec(); // IMU时间戳
    // cout << "imu时间戳："<< timestamp<<endl;
    mtx_buffer.lock();
    // 如果当前IMU的时间戳小于上一个时刻IMU的时间戳，则IMU数据有误，将IMU数据缓存队列清空
    if (timestamp < last_timestamp_imu)
    {
        ROS_WARN("imu loop back, clear buffer");
        imu_buffer.clear();
    }
    last_timestamp_imu = timestamp;
    imu_buffer.push_back(msg);
    mtx_buffer.unlock();
    sig_buffer.notify_all();
}

Lidar 回调

一句话概括：同上，只不过中间可能要穿插一点点云的预处理。

其实 Lidar 回调还有 livox_pcl_cbk ，应该是专门针对的 livox 类型的激光雷达，可能它们的数据类型不太一样？不过这里我主要看常规的雷达回调部分，即 standard_pcl_cbk 。

这里的回调也和刚才的 imu 回调类似，也是要把一个激光雷达的信息塞到一个 deque 类型的容器里面。不过我感觉这里写得很奇怪。

首先又是典中典的加锁和时间戳顺序判断，这里因为如果时间戳错误会有 lidar_time_buffer.clear() 操作，所以加锁一定要放在最前面。之后看情况进行点云的预处理，把点云放到一个 PCL 的点云指针里面（甚至作者懒的写点云的具体处理流程）。最后分别把点云指针和时间放到两个 deque 里面。

代码实现

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22


void FastLioProcess::standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) 
{ // 传感器信息获取
    mtx_buffer.lock(); //加锁
    // 如果当前扫描的激光雷达数据的时间戳比上一次扫描的激光雷达数据的时间戳早，需要将激光雷达数据缓存队列清空
    if (msg->header.stamp.toSec() < last_timestamp_lidar)
    {
        ROS_ERROR("lidar loop back, clear buffer");
        lidar_time_buffer.clear();
    }
    // cout<<"该点云时间戳："<<msg->header.stamp.toSec()<<endl;
    PointCloudXYZI::Ptr  ptr(new PointCloudXYZI());
    // 进行点云预处理 ##########################################
    // 根据情况自己编写
    // p_pre->process(msg, ptr);  // p_pre 是前处理对象的指针
    //  ##########################################
    // cout <<"点云大小："<<ptr->size()<<endl;
    lidar_buffer.push_back(ptr);  //点云预处理  降采样
    lidar_time_buffer.push_back(msg->header.stamp.toSec()); //将时间放入缓冲区
    last_timestamp_lidar = msg->header.stamp.toSec();  //记录最后一个时间
    mtx_buffer.unlock();
    sig_buffer.notify_all();  // 唤醒所有线程
}

Lidar 点云预处理

一句话概括：按线数和回波次序筛选点之后下采样，把符合要求（点之间不能靠太近、点不要离我的机器人太近）的点放到一个 PCL 点云里面。

我们以 livox_avia_process 函数来分析点云的预处理方式。（题外话，看了 Livox 官网，avia 这款雷达要一万，还算便宜了，这些实验室真有钱啊）

函数的输入很简单，一个 ROS 的点云常量指针和一个用于输出的 PCL 类型的点云指针，输出为 void。 pl_surf pl_corn pl_full 是三个点云类型的变量，因为一直要用到所以就提前在类里面初始化了。具体的计算细节在 for 循环里面。

if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) 这里首先约束了点所在的线数要小于N_SCANS，只处理第 0 条线到第 N_SCANS-1 条线的点；其次要求回波次序是 0 或者 1 （妈的，邪恶的位运算技术），也就是说只处理坚硬的障碍物（前两次反射），不关注多次反射之后的结果（例如茂密的植被后面的物体）。

点云的具体排列方式如下：

之后就是等间距的下采样： valid_num % point_filter_num == 0 ，我们把采样之后的点云放到 pl_full 里面。后面的 count_out 和 count_near 感觉是给调试使用的，我们暂时不关注。最后，只有和上一个点相距大于最小阈值，并且不在我们屏蔽范围内（也就是不是小车上面的点），我们才会把它放到 pl_surf ，即最终的输出点云里面（不过这里有一个深拷贝，说实话感觉用 std::move() 更好）

代码实现

 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


void FastLioProcess::livox_avia_process(const livox_ros_driver::CustomMsg::ConstPtr &msg,PointCloudXYZI::Ptr &pcl_out)
{
    pl_surf.clear(); // 清除之前的平面点云缓存
    pl_corn.clear(); // 清除之前的角点云缓存
    pl_full.clear(); // 清除之前的全点云缓存
    double t1 = omp_get_wtime(); // 后面没用到  获取当前时间。用于优化线程的
    int plsize = msg->point_num;  // 这一批中的点云总个数
    // cout<<"plsie: "<<plsize<<endl;

    pl_corn.reserve(plsize);  // 分配空间
    pl_surf.reserve(plsize);  // 分配空间
    pl_full.resize(plsize);   // 分配空间

    uint valid_num = 0;  // 有效的点云数
    // 特征提取（FastLIO2默认不进行特征提取）
    // 分别对每个点云进行处理
    size_t count_out=0;
    size_t count_near=0;
    for(uint i=1; i<plsize; i++)
    {  // 只取线数在0~N_SCANS内并且回波次序为0或者1的点云
        if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
        {
            valid_num ++;  // 有效的点云数
            if (valid_num % point_filter_num == 0)  // 等间隔降采样  // 这里应该做一个判断 如果点数少了则不进行下采样
            {
                pl_full[i].x = msg->points[i].x;
                pl_full[i].y = msg->points[i].y;
                pl_full[i].z = msg->points[i].z;
                pl_full[i].intensity = msg->points[i].reflectivity;
                pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms
                // 只有当当前点和上一点的间距足够大（>1e-7），并且在最小距离阈值之外，才将当前点认为是有用的点，加入到pl_surf队列中
                if (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z <= (blind * blind)){
                count_out+=1;
                }
                if ((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
                && (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
                && (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)){
                count_near+=1;
                }

                if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
                    || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
                    || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
                    && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))
                {
                pl_surf.push_back(pl_full[i]);
                }
            }
        }  
    }
    *pcl_out = pl_surf;
}

点云-IMU的对齐封装

终于，到了激动人心的时间帧对齐环节！在刚才，我们的 IMU 和点云数据都被放到了队列里面，我们还有它们对应的时间帧。这个时候，按我的理解，需要采用合适的方法对齐两者的时间点，然后封装到一个帧里。当然，因为 ROS 的写入和我们的读取过程不在同一个线程内，我们在这里还需要考虑加锁。

💡 2025/12/10 补充
注意，这里的加锁是一个很大的问题，涉及到 FAST-LIO 系列的线程同步和竞态。

首先，代码里面只有 sig_buffer.notify_all() ，没有 sig_buffer.wait() ，认为这里是多余的。

其次，关于为什么插入线程要加锁，弹出线程不加锁。Gemini 和 DeepWiki 给出了不同的答案，但它们都认为代码作者写得有问题。Gemini 认为：代码中并没有调用 ros::AsyncSpinner 或 ros::MultiThreadedSpinner。这意味着整个节点只有一个主线程，多线程的加锁完全没有意义，它是靠轮询来模仿多线程的。DeepWiki 认为：主线程的sync_packages()函数没有加锁主要是因为性能考虑和执行时序的依赖，但这种设计确实存在理论上的竞态条件风险。

我看了一下代码感觉 Gemini 说的更有道理一点，在 run() 函数里面我们是先执行一次 ros::spinOnce() 来处理回调，之后再调用 sync_packages() 。确实从头到尾都在一个线程里面，没必要加锁。

看输入，是一个我们封装好的引用 meas ，具体含义可以去看 common_lib.h 头文件。这代表我们每个时间帧的所有测量结果，我们需要把 IMU 和 Lidar 的信息放进去。成功就 true，反之 return false。

在看下面的实现之前，我们可以先思考一个问题，IMU 的测量是一串离散的数值，那我们的一个数据包里面要包含从哪个时刻到哪个时刻的 IMU 测量才能保证对齐呢？除此之外，我们知道

接下来首先是边界条件，要确定 Lidar 和 IMU 队列里面不是空的。之后我们偷偷看一眼lidar_buffer里面的数据（之所以是偷看，是因为我们不在这里把它 pop_front()）。作者在这里用了一个 flag lidar_pushed 来表示这一帧的雷达数据是否已经放到包里面。如果还没有放进去，那意味着两种情况：

这是第一次扫描。我们多做一个处理来应对边界条件：把 last_lio_update_time 记作 buffer 里面待处理点云的时间，之后执行下面的步骤。
这不是第一次扫描。那我们需要从 lidar_buffer 里面复制待处理点云以及它所在的时间帧。注意到激光雷达的扫描是一个连续的过程，所以我们还要计算点云扫描结束的段时间。

我们的预期实现是 IMU 数据正好覆盖整个 Lidar 扫描过程（这也是我们刚才要计算点云扫描结束时间的原因）。于是，接下来我们等待 imu_buffer 里面的 IMU 的最新时间超过 Lidar 扫描结束的时间，如果 IMU 扫描的数据量不够，那就 return false 等待下一次循环（这里就体现了我们刚才使用 lidatr_pushed 这个 flag 的好处了，之后就不用重复赋值）

接下来，一切准备就绪，我们要执行时间帧对齐（我猜这里应该用球面 slerp ？）。这里又给出了一个奇怪的边界条件检查 lidar_end_time < imu_time ，为了防止出现 Lidar 前面没有 imu 的情况，这种情况这一帧的 Lidar 就废掉了。接下来把 imu_buffer里面的数据一个个放到 meas.imu 里面即可，Lidar 数据也同理。

代码实现

 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


bool FastLioProcess::sync_packages(MeasureGroup &meas){
    if (lidar_buffer.empty() || imu_buffer.empty()) {
        return false;
    }//如果缓存队列中没有数据，则返回false
    // ######################################### 这部分容易出现反复读取数据 
    if(!lidar_pushed){
        if (flg_first_scan) last_lio_update_time=lidar_time_buffer.front();
            lidar = lidar_buffer.front();
            lidar_beg_time = lidar_time_buffer.front();
            lidar_end_time = lidar_beg_time + lidar->points.back().curvature / double(1000);
            lidar_pushed = true;
    }
    
    // #########################################
    // 防止IMU数据没有覆盖雷达时间
    if (last_timestamp_imu <= lidar_end_time)
    {   
        return false;
    }

    // 收集imu数据
    double imu_time = imu_buffer.front()->header.stamp.toSec();
    // 防止雷达前面没有imu
    if (lidar_end_time < imu_time)  
    {   
        lidar_time_buffer.pop_front();
        lidar_buffer.pop_front();
        lidar_pushed = false;
        return false;
    }
    meas.imu.clear();
     // 拿出lidar_beg_time到lidar_end_time之间的所有IMU数据
    while ((!imu_buffer.empty()) && (imu_time <= lidar_end_time))
    {
        imu_time = imu_buffer.front()->header.stamp.toSec();
        if(imu_time > lidar_end_time) break;
        meas.imu.push_back(imu_buffer.front());
        imu_buffer.pop_front();
    }

    //##########################
    // 是否对点云时间进行处理 （也可以放在反向传播中进行处理）
    //##########################
    meas.lidar = lidar;
    meas.lidar_beg_time = lidar_beg_time;
    meas.lidar_end_time = lidar_end_time;
    meas.last_lio_update_time = last_lio_update_time;
    last_lio_update_time = lidar_end_time;
    lidar_pushed = false;  // 等这一批数据整理完后再 返回初始值
    lidar_buffer.pop_front();
    lidar_time_buffer.pop_front();
    return true;
}

线程唤醒

之前已经讨论过， FAST-LIO2 的多线程有点奇怪，实际上使用 ROS1 的 ros::spinOnce() 来轮询模拟多线程。感觉这段代码有点没必要。

代码实现

1
2
3
4
5
6


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

主函数

前面是很正常的 ros::Rate 和循环等参数设置，每次在回调函数执行之后收集每帧数据包，如果数据包收集失败（例如 imu_buffer 里面的数据量不够）就 sleep 一会。

flg_first_scan 是作者对于第一次测量的边界处理，但似乎因为在执行的代码里面已经做过了类似的处理，所以这里只是现实简单的调试信息。之后就是最核心的函数：

前向传播： processImu()

代码实现

  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
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149


void FastLioProcess::run(){
    // 用于设置信号处理函数 可以自定义处理（SIGINT） Ctrl + C 中断信号的行为
    signal(SIGINT, SigHandle); 
    //控制循环频率  在后续的代码中，可以使用 rate.sleep() 函数来使程序休眠一段时间，以达到控制循环频率的目的
    ros::Rate rate(5000); 
    bool status = ros::ok(); //ROS 节点是否被关闭、是否接收到关闭信号
    // 保存位姿差
    string save_imu = string(string(ROOT_DIR) + "PCD/imu_pos.txt");
    fs::remove(save_imu);
    std::ofstream fout_imu_pos;
    fout_imu_pos.open(save_imu, ios::app); 

    string save_state = string(string(ROOT_DIR) + "PCD/state_pos.txt");
    fs::remove(save_state);
    std::ofstream fout_state_pos;
    fout_state_pos.open(save_state, ios::app); 

    while (status){
        // 如果监听到信号则推出循环
        if (flg_exit) break; 
        ros::spinOnce(); 
        if(sync_packages(Measures)) // 将这次的雷达数据以及对应的时间区间的imu数据存入Measures中
        {   
            if (flg_first_scan) // 没看出来这一步的意义
            {
                first_lidar_time = Measures.lidar_beg_time;  // 刚开始默认为0
                cout <<"first_lidar_time: " <<first_lidar_time <<endl;
                flg_first_scan = false;
                continue;
            }
            // 这里前向传播和去畸变操作
            processImu(); 
            // 保存前向传播后的位姿
            if(fout_imu_pos.is_open()){
                fout_imu_pos << std::fixed << std::setprecision(3) << state.pos[0] << " " << state.pos[1]
                        << " " << state.pos[2] << " " << state.rot.coeff(0)<< " " << state.rot.coeff(1) << " " << state.rot.coeff(2)<< " " 
                        <<state.rot.coeff(3)<<" " <<state.rot.coeff(4)<<" " <<state.rot.coeff(5)<<" " <<state.rot.coeff(6)<<" "<<state.rot.coeff(7)
                        <<" " <<state.rot.coeff(8)<<endl;
            }

            int pcl_undistort_size = pcl_undistort->points.size();
            if (pcl_undistort->empty() || (pcl_undistort == NULL))
            {   cout<<"pcl_undistort为空"<<endl;
                ROS_WARN("No point, skip this scan!\n");
                continue;
            }

            pos_lid = state.pos + state.rot * state.offset_T_L_I;
            lasermap_fov_segment(); // 动态调整局部地图

            downSizeFilterSurf.setInputCloud(pcl_undistort);
            downSizeFilterSurf.filter(*feats_down_body);
            feats_down_size = feats_down_body->points.size();  //记录滤波后的点云数量
            // 对于第一帧点云开始构建kd树
            if(ikdtree.Root_Node == nullptr){
                if(feats_down_size > 5)
                {   
                    // 设置ikd tree的降采样参数
                    ikdtree.set_downsample_param(filter_size_map_min);  // 0.5
                    feats_down_world->resize(feats_down_size);  //点云对象将包含 feats_down_size 个点。
                    for(int i = 0; i < feats_down_size; i++)
                    {
                        pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
                    }
                    // 组织ikd tree
                    ikdtree.Build(feats_down_world->points); //构建ikd树
                }
                // 是否保存单帧
                if(false){
                    string root_(string(string(ROOT_DIR) + "PCD/split"));
                    string root_org_(string(string(ROOT_DIR) + "PCD/split_org")); 
                    createDirectoryIfNotExists(root_);
                    createDirectoryIfNotExists(root_org_);
                    string all_points_dir(string(string(ROOT_DIR) + "PCD/split/scans_") + to_string(save_i) + string(".pcd"));
                    pcl::PCDWriter pcd_writer;
                    pcl_undistort_world->resize(pcl_undistort_size);
                    for(int i = 0; i < pcl_undistort_size; i++)
                    {
                        pointBodyToWorld(&(pcl_undistort->points[i]), &(pcl_undistort_world->points[i]));
                    }
                    cout << "init current scan saved to " << all_points_dir<<endl;
                    pcd_writer.writeBinary(all_points_dir, *pcl_undistort_world);


                    string all_points_dir_(string(string(ROOT_DIR) + "PCD/split_org/scans_") + to_string(save_i) + string(".pcd"));
                    pcl::PCDWriter pcd_writer_;
                    cout << "init current scan saved to " << all_points_dir_<<endl;
                    pcl_org_world->resize(Measures.lidar->size());
                    for(int i = 0; i < Measures.lidar->size(); i++)
                    {
                        pointBodyToWorld(&(Measures.lidar->points[i]), &(pcl_org_world->points[i]));
                    }
                    pcd_writer_.writeBinary(all_points_dir_, *pcl_org_world);

                    save_i+=1;
                }
                continue;
            }
            // ################################ 这部分没用
            int featsFromMapNum = ikdtree.validnum(); // 获取 树的有效点树
            kdtree_size_st = ikdtree.size();  // 获取树的大小
            // ################################
            if (feats_down_size < 5)
            {
                ROS_WARN("No point, skip this scan!\n");
                continue;
            } // 在这里跳过了
            // 重置一些属性  迭代时需要
            normvec->resize(feats_down_size);  // 特诊点匹配上的点存在这里面
            feats_down_world->resize(feats_down_size);
            Nearest_Points.resize(feats_down_size);
            state_estimation();  // 迭代优化  ####################################################
            // 保存优化后的位姿
            if(fout_state_pos.is_open()){
                fout_state_pos << std::fixed << std::setprecision(3) << state.pos[0] << " " << state.pos[1]
                        << " " << state.pos[2] << " " << state.rot.coeff(0)<< " " << state.rot.coeff(1) << " " << state.rot.coeff(2)<< " " 
                        <<state.rot.coeff(3)<<" " <<state.rot.coeff(4)<<" " <<state.rot.coeff(5)<<" " <<state.rot.coeff(6)<<" "<<state.rot.coeff(7)
                        <<" " <<state.rot.coeff(8)<<endl;
            }
            
            // 点云转换
            pcl_undistort_world->resize(pcl_undistort_size);
            for(int i = 0; i < pcl_undistort_size; i++)
            {   
                pointBodyToWorld(&(pcl_undistort->points[i]), &(pcl_undistort_world->points[i]));
            }
            feats_down_world->resize(feats_down_size);
            for(int i = 0; i < feats_down_size; i++)
            {
                pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
            }
            // 发布数据  
            euler_cur = RotMtoEuler(state.rot);
            geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));
            pos_lid = state.pos + state.rot * state.offset_T_L_I; 
            publish_odometry();  // 发布位姿 
            // 发布数据
            publish_frame_world();
            // 在kd树中添加新的点云，并进行降采样 重建树
            map_incremental();
            // 保存数据
            save();
            frame_index++;
        }
        status = ros::ok();
        rate.sleep();
    }

}