从零入门激光SLAM（二十二）——Fast-lio2代码详解（一） IMU预测与点云去除畸变

已于 2024-06-05 17:42:54 修改
阅读量2k 收藏 28
点赞数 21
分类专栏： 从零入门激光SLAM 文章标签： 自动驾驶 人工智能 SLAM c++ 机器人
于 2024-05-30 12:27:36 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/huashudeyanjing/article/details/139320759
版权

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。

Fast-lio2原理解析见链接
从零入门激光SLAM（二十一）——看不懂FAST-LIO？进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
https://github.com/huashu996/Fast-lio2-Supernote
本代码解析以算法流程的逻辑解析代码，一些简单的函数忽略讲解。

一、初始化卡尔曼滤波

初始化了卡尔曼滤波器的动态共享数据结构。通过设置过程模型、状态转移矩阵雅可比、过程噪声雅可比、测量模型、最大迭代次数和收敛阈值，准备了卡尔曼滤波器所需的所有参数和模型，以便进行状态预测和更新

/*初始化卡尔曼滤波器 get_f：过程模型（Process Model） df_dx: 表示状态转移矩阵的雅可比矩阵 df_dw: 表示过程噪声矩阵的雅可比矩阵 h_share_model: 观测模型 NUM_MAX_ITERATIONS: 表示最大迭代次数 epsi：表示收敛阈值（Convergence Threshold） */
kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
    /*** ROS subscribe initialization ***/
    //sub
    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
        nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
        nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
    ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
    //pub
    ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered", 100000);
    ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered_body", 100000);
    ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_effected", 100000);
    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
            ("/Laser_map", 100000);
    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
            ("/Odometry", 100000);
    ros::Publisher pubPath          = nh.advertise<nav_msgs::Path> 
            ("/path", 100000);

  • 初始化卡尔曼滤波参数

      //接收系统特定的模型及其差异
  //作为特征矩阵的测量，其维数是变化的。
  //通过一个函数(h_dyn_share_in)完成了测量(z)，估计测量(h)，偏微分矩阵(h_x, h_v)和噪声协方差(R)的同时计算。
  void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n])
  {
      //预测模型
      f = f_in;
      //状态转移矩阵
      f_x = f_x_in;
      //过程噪声矩阵
      f_w = f_w_in;
      //动态共享观测模型
      h_dyn_share = h_dyn_share_in;
      //最大迭代次数
      maximum_iter = maximum_iteration;
      for(int i=0; i<n; i++)
      {
          limit[i] = limit_vector[i];
      }

      x_.build_S2_state();
      x_.build_SO3_state();
      x_.build_vect_state();
  }

    二、IMU预测与前向传播

    2.1 整体框架

    当IMU数据与LIDAR数据同步后，初始化系统参数，进入p_imu->Process函数进行对IMU数据处理，获得当前的预测位姿state_point

    signal(SIGINT, SigHandle);
  ros::Rate rate(5000);
  bool status = ros::ok();
  while (status)
  {

      if (flg_exit) break;
      ros::spinOnce();
      if(sync_packages(Measures)) //信息同步
      {
          //如果是第一帧
          if (flg_first_scan)
          {
              first_lidar_time = Measures.lidar_beg_time;
              p_imu->first_lidar_time = first_lidar_time;
              flg_first_scan = false;
              continue;
          }
          auto start = std::chrono::high_resolution_clock::now();
          //初始化参数
          double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;
          match_time = 0;
          kdtree_search_time = 0.0;
          solve_time = 0;
          solve_const_H_time = 0;
          svd_time   = 0;
          t0 = omp_get_wtime();
          // 1. 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播
          p_imu->Process(Measures, kf, feats_undistort);
          // 2.获取kf预测的全局状态（imu）
          state_point = kf.get_x(); //imu先验位姿

    2.2 IMU处理流程

    meas：包含IMU和LiDAR数据的测量组。
    kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
    cur_pcl_un_：当前未去畸变的点云数据，类型为 PointCloudXYZI::Ptr。</state_ikfom,>

  1. 初始化时间：记录开始处理的时间。
  2. 检查IMU数据和LiDAR数据：确保IMU数据不为空，并且LiDAR数据指针有效。
  3. IMU初始化：如果需要初始化IMU，调用 IMU_init 进行初始化，并检查初始化迭代次数，超过最大计数后，调整加速度协方差并标记初始化完成。
  4. 点云去畸变：调用 UndistortPcl 函数对点云数据进行去畸变处理。

  5. 记录时间：记录去畸变处理后的时间，用于后续分析和调试。

    void ImuProcess::Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
//初始化时间测量：
double t1,t2,t3;
t1 = omp_get_wtime();
//检查IMU数据是否为空：
if(meas.imu.empty()) {return;};
//检查LiDAR数据指针是否为NULL：
ROS_ASSERT(meas.lidar != nullptr);
//IMU初始化：
if (imu_need_init_)
{
/// The very first lidar frame
//调用 IMU_init 函数进行初始化
IMU_init(meas, kf_state, init_iter_num);
imu_need_init_ = true;
//保存最新的IMU数据
last_imu_   = meas.imu.back();
//获取滤波器状态 
state_ikfom imu_state = kf_state.get_x();
//如果初始化迭代次数超过最大计数 
if (init_iter_num > MAX_INI_COUNT)
{
//调整加速度协方差 
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
//重置加速度和陀螺仪的协方差比例
cov_acc = cov_acc_scale;
cov_gyr = cov_gyr_scale;
//打印IMU初始化完成信息。
ROS_INFO("IMU Initial Done");
// ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
}

return;
}
//前向传播，点云去畸变
UndistortPcl(meas, kf_state, *cur_pcl_un_);
//记录时间
t2 = omp_get_wtime();
t3 = omp_get_wtime();
// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}
  • IMU初始化
    用于初始化IMU，包括计算重力、陀螺仪偏差、加速度和陀螺仪的协方差，并将加速度测量归一化为单位重力
    meas：包含IMU和LiDAR数据的测量组。
    kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
    N：用于记录初始化过程中IMU数据的计数。</state_ikfom,>
  1. 初始化重力、陀螺仪偏差、加速度和陀螺仪的协方差：初始化当前加速度和陀螺仪数据。
  2. 处理第一帧IMU数据：如果是第一帧数据，重置参数并初始化平均加速度和陀螺仪。
  3. 计算均值和协方差：通过遍历IMU数据，计算加速度和陀螺仪的均值和协方差。
  4. 初始化滤波器状态：根据计算的均值和协方差，初始化滤波器状态，包括重力、陀螺仪偏差和平移旋转偏差。
  5. 初始化滤波器协方差矩阵：设置滤波器协方差矩阵的初始值。
  6. 记录最新IMU数据：保存最后一个IMU数据以备后续使用。
  1. void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity **/
//初始化协方差
V3D cur_acc, cur_gyr;
//处理第一帧IMU数据：
if (b_first_frame_)
{
Reset(); // 重置初始化参数
N = 1; // 设置初始计数
b_first_frame_ = false; // 标记第一帧已经处理
const auto &imu_acc = meas.imu.front()->linear_acceleration; //提取线加速度
const auto &gyr_acc = meas.imu.front()->angular_velocity; //提取角速度
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; // 初始化平均加速度
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; // 初始化平均陀螺仪
first_lidar_time = meas.lidar_beg_time; // 记录第一帧LiDAR时间
}
//计算加速度和陀螺仪的均值和协方差：
for (const auto &imu : meas.imu)
{
const auto &imu_acc = imu->linear_acceleration;
const auto &gyr_acc = imu->angular_velocity;
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
mean_acc      += (cur_acc - mean_acc) / N; // 更新平均加速度
mean_gyr      += (cur_gyr - mean_gyr) / N; // 更新平均陀螺仪
// 更新加速度协方差
cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
// 更新陀螺仪协方差
cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
N ++;
}
//初始化滤波器状态
state_ikfom init_state = kf_state.get_x();
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2); //设置重力方向
//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
init_state.bg  = mean_gyr; // 设置陀螺仪偏差
init_state.offset_T_L_I = Lidar_T_wrt_IMU; // 设置LiDAR相对于IMU的平移偏差
init_state.offset_R_L_I = Lidar_R_wrt_IMU; // 设置LiDAR相对于IMU的旋转偏差
kf_state.change_x(init_state); // 更新滤波器状态
//初始化滤波器协方差矩阵
esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
init_P.setIdentity();  // 初始化协方差为单位矩阵
init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;  // 设置位置协方差
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; // 设置速度协方差
init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; // 设置陀螺仪偏差协方差
init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; // 设置加速度偏差协方差
init_P(21,21) = init_P(22,22) = 0.00001;  // 设置其他状态协方差
kf_state.change_P(init_P); // 更新滤波器协方差矩阵
//记录最新的IMU数据：
last_imu_ = meas.imu.back();
}
  • 点云畸变
    用于对点云数据进行去畸变处理
    meas：包含IMU和LiDAR数据的测量组。
    kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
    pcl_out：输出去畸变后的点云数据，类型为 PointCloudXYZI。</state_ikfom,>
  1. 添加上一帧IMU数据：确保IMU数据连续性。
  2. 排序点云数据：按时间偏移排序点云。
  3. 初始化IMU位姿：获取滤波器状态并初始化IMU位姿列表。
  4. 前向传播每个IMU数据点：遍历IMU数据点，更新滤波器状态并保存IMU位姿。
  5. 计算帧末的位姿预测：根据点云结束时间预测帧末位姿。

  6. 对点云进行去畸变：遍历点云数据，使用IMU数据对点云进行去畸变处理。

详情请见...

从零入门激光SLAM（二十二）——Fast-lio2代码详解（一） IMU预测与点云去除畸变 - 古月居 (guyuehome.com)

桦树无泪
关注
专栏目录
通过imu去雷达畸变
苏凯的博客
08-10 167
【代码】通过imu去雷达畸变。
Fast-Lio2代码及论文解析2
努力努力努力
03-29 1060
点云数据频率较低而IMU为高频数据。
从零入门激光SLAM（二十二）——Fast-lio2代码详解（零） 整体代码框架解析
HUASHUDEYANJING的博客
05-30 1916
FASTLIO2-代码整体框架Fast-lio2原理解析见链接注释版代码完整版见本节对代码的整体框架进行解析，将代码与论文中的公式对应。
FAST-LIO2代码解析(一)
热门推荐
lovely_yoshino的博客
07-18 1万+
现在越来越多的激光雷达方法如雨后春笋一般冒了出来，最近以FAST-LIO为代表的系列渐渐地被大众接受。而FAST-LIO2的工作越来越被大众所熟知与研究。最近作者也在研究与学习FAST-LIO2的相关知识，这里将会以长文的形式来介绍FAST-LIO2.0的学习心得。本文将通过代码的形式与论文结合，直观的来分析论文的核心算法部分。也同时作为自己的学习笔记来留给后来人学习。...
Livox mid360 激光雷达运行 fast-lio2 详细教程
最新发布
m0_55117804的博客
09-29 1261
如果你想之后不想source，就去主目录的.bashrc文件自己加上去（这种ros基操就不在这说了）
fastlio点云去畸变问题
Son_of_the_Bronx的博客
12-13 1711
第一种是速腾雷达,通过配置可以得到点云中每个点的时间,但是时间单位有问题,需要修正才能使去畸变正常工作.第二种是velodyne,不知道是什么原因没法得到点云中点的相对时间,所以需要去补偿时间,才能使去畸变正常工作.吐槽一下velodyne怎么样才能直接从雷达驱动获取点云每个点的时间,不太清楚怎么操作,github上的驱动太不详细了…
fast-lio2代码
05-26
fast-lio2代码，做了调整，调用库，生成头文件。
FAST-LIO2代码解析
lcyangzhanduo的博客
01-19 2972
本文为FAST-LIO2的代码阅读笔记，记录整体流程及相关处理，便于后续回忆。
fast-lio2代码解析
avenger_fang的博客
04-13 2973
代码结构很清晰，从最外层看包含两个文件夹，一个是fast-lio,另外一个是加上scan-context的回环检测与位姿图优化。
FAST-LIO2代码解析(四)
lovely_yoshino的博客
09-04 5974
在讲完前面三节后我们将开始以主函数触发，并分析ESKF和IKD-Tree中对应的算法。其中ESKF作为主要的部分来进行展开介绍。
FAST_LIO:计算有效且强大的LiDAR惯性里程计（LIO）软件包
03-21
注意：不支持Ubuntu 16.04及更低版本 FAST-LIO 2.0 将于2021年3月底发射。 新的功能： 更快更好 更高的频率； 更多的LiDAR支持（Horizo​​n和Ouster 64）； 支持基于ARM的嵌入式平台。 FAST-LIO FAST-LIO （快速LiDAR惯性里程计）是一种计算效率高且功能强大的LiDAR惯性里程计套件。它使用紧密耦合的迭代扩展卡尔曼滤波器将LiDAR特征点与IMU数据融合在一起，从而在发生退化的快速运动，嘈杂或混乱的环境中实现强大的导航能力。我们的软件包解决了许多关键问题： 快速迭代的卡尔曼滤波器，用于里程计优化； 在最稳定的环境下自动初始化； 并行KD-Tree搜索以减少计算量； 强大的特征提取； 开发者 ：激光贴图和姿势优化； ：特征提取。 要了解更多详细信息，请参阅我们的相关文章：） 我们的相关论文：现在可以在arxiv上获得我
LIO-SAM-DetailedNote:LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS
05-08
LIO-SAM-DetailedNote LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS，因子图优化。 LIO-SAM的代码十分轻量，只有四个cpp...对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征
论文精读系列文章——Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry
YangtseJin的博客
03-22 1815
Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry
点云从入门到精通技术详解100篇-雨雾环境下多传感器融合 SLAM 方法
getusushu的博客
12-05 2064
通过滑动窗口的方法，在相邻多个位姿之间建立位姿约束，能够克服较为剧烈的运动场景，仅使用单帧激光点云来获得特征点的深度，虽然摆脱了对运动估计的依赖，但是精度较低。取特征点，在系统的状态量中引入了尺度因子，使其能够在大范围场景下稳定运行，并能。理多个不同传感器及其延迟，获得更加精确的测量估计，同时能够自动校准，在系统运行。的位姿变化初值，提高了定位精度。获得了更高的精度，其回环检测模块能够以最小代价实现重定位，具有更好的鲁棒性。用了基于优化的最大后验估计，在精度上相比前两代有极大的提升，同时实现了极佳的鲁。
点云从入门到精通技术详解100篇-雨雾环境下多传感器融合SLAM方法
getusushu的博客
10-18 305
近年来，随着计算机芯片技术的进步，运算能力得到了极大的提升，为实现无人驾驶 车辆的智能化奠定了坚实的硬件基础。鉴于无人驾驶这一潜在的市场，国内外众多厂商纷 纷开展无人驾驶技术的研究。谷歌的第一个无人驾驶项目Waymo于2009年启动，历经 十多年，于2018年末发布了其第一个无人驾驶载客服务Waymo One，如图1.1。与此同 时，国内的高科技公司纷纷加入无人驾驶市场。百度率先开始了无人驾驶领域的发展，并 于2020年推出了名为“萝卜快跑”的无人驾驶交通服务平台。如图1.2。
点云从入门到精通技术详解100篇-雨雾环境下多传感器融合SLAM方法（续）
getusushu的博客
10-19 248
SLAM通过传感器感知周围环境并获得自身定位，通过传感器数据获得短时间内运 动估计的过程被称为前端。
fastlio点云去畸变问题2
Son_of_the_Bronx的博客
03-06 1125
虽然在雷达10hz情况下，用fastlio内的代码计算点云中的点的相对时间是可行的，但是雷达5hz情况下会出问题，可能是那个时间补偿的代码适用10hz？也可能是5hz的点云不能这样简单地不长时间？具体原因不太清楚。点云中点的时间是非常关键的，最好是用驱动给出的相对时间。提问：为什么点云中靠前的点，驱动给出的相对时间是负数？？
FAST-LIO2代码解析(二)
努力努力努力
04-12 569
在上文中讲述了激光雷达的数据获取以及特征点提取的操作，这一节我们将围绕着IMU_Processing这一节来进行介绍。在ImuProcess.hpp中，一开始就是完成了对ImuProcess类的申明，里面我们可以看到在imu中最主要的还是角速度和加速度这两项特征，这与视觉SLAM的imu融合非常相似。//判断点的时间是否先后颠倒public:// imu参数输出文件//加速度测量协方差//角速度测量协方差//加速度测量协方差//角速度测量协方差。
怎么用imu给激光雷达去畸变
06-12
使用IMU数据对激光雷达进行去畸变的方法称为IMU辅助激光雷达去畸变，其基本思想是通过IMU测量的姿态信息对激光雷达采集的点云数据进行校正。具体的步骤如下： 1. 获取IMU数据：通过IMU传感器获取车辆的加速度和角速度等姿态信息。 2. 根据IMU数据计算姿态：利用IMU数据，通过解算出车辆在三维空间中的姿态信息。 3. 估计激光雷达的位姿：通过激光雷达采集的点云数据和IMU计算出的姿态信息，估计出激光雷达在三维空间中的位姿。 4. 对点云数据进行变换：利用估计出的激光雷达位姿信息，对点云数据进行变换，从而校正出畸变数据。 需要注意的是，IMU辅助激光雷达去畸变的方法需要考虑IMU的精度、噪声等因素，以及对IMU和激光雷达之间的时间同步要求比较高。此外，IMU辅助激光雷达去畸变需要结合激光雷达的标定参数进行使用，以确保校正效果的准确性。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

桦树无泪

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值