fast-lio代码解析(一）

标签：acc mathbf text lio fast init imu pcl 解析

文章目录

一.lasermap_fov_segment
二. IMU_init
三. UndistortPcl

一.lasermap_fov_segment

void lasermap_fov_segment()
{
    cub_needrm.clear(); // 清空需要移除的立方体集合
    kdtree_delete_counter = 0; // 初始化删除点计数器
    kdtree_delete_time = 0.0;  // 初始化删除时间

    // 计算机体坐标到世界坐标的转换
    pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
    V3D pos_LiD = pos_lid; // LiDAR当前位姿

    // 初始化局部地图范围
    if (!Localmap_Initialized) {
        for (int i = 0; i < 3; i++) {
            LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; // 最小边界
            LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; // 最大边界
        }
        Localmap_Initialized = true; // 设置初始化标志
        return;
    }

    // 检查是否需要移动局部地图
    float dist_to_map_edge[3][2]; // 当前位置到边界的距离
    bool need_move = false;       // 是否需要移动标志
    for (int i = 0; i < 3; i++) {
        dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); // 到最小边界距离
        dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); // 到最大边界距离
        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || 
            dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {
            need_move = true; // 如果接近边界，设置移动标志
        }
    }

    // 如果无需移动局部地图，直接返回
    if (!need_move) return;

    // 计算新的局部地图范围
    BoxPointType New_LocalMap_Points, tmp_boxpoints;
    New_LocalMap_Points = LocalMap_Points;
    float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, 
                         double(DET_RANGE * (MOV_THRESHOLD - 1)));

    for (int i = 0; i < 3; i++) {
        tmp_boxpoints = LocalMap_Points;
        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) {
            New_LocalMap_Points.vertex_max[i] -= mov_dist; // 更新最大边界
            New_LocalMap_Points.vertex_min[i] -= mov_dist; // 更新最小边界
            tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
            cub_needrm.push_back(tmp_boxpoints); // 添加需要移除的立方体
        } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {
            New_LocalMap_Points.vertex_max[i] += mov_dist; // 更新最大边界
            New_LocalMap_Points.vertex_min[i] += mov_dist; // 更新最小边界
            tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
            cub_needrm.push_back(tmp_boxpoints); // 添加需要移除的立方体
        }
    }

    // 更新局部地图范围
    LocalMap_Points = New_LocalMap_Points;

    // 清理冗余点
    points_cache_collect(); // 收集点缓存
    double delete_begin = omp_get_wtime(); // 开始计时
    if (cub_needrm.size() > 0) 
        kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); // 调用KD树清理点云
    kdtree_delete_time = omp_get_wtime() - delete_begin; // 记录清理耗时
}

该函数的核心功能是通过动态更新局部地图的范围，保持激光雷达（LiDAR）位于地图中心附近，并清理超出范围的点云数据。以下是关键过程公式化描述：

1. 初始化局部地图

局部地图的最小边界和最大边界初始化为：
v min , i = x i − L 2 \mathbf{v}_{\text{min},i} = x_i - \frac{L}{2} vmin,i=xi−2L
v max , i = x i + L 2 \mathbf{v}_{\text{max},i} = x_i + \frac{L}{2} vmax,i=xi+2L
其中：

v min , i \mathbf{v}_{\text{min},i} vmin,i 和 v max , i \mathbf{v}_{\text{max},i} vmax,i 表示局部地图在第 i i i 维的边界。
x i x_i xi 表示激光雷达在第 i i i 维的当前位置。
L L L 是局部地图的边长。

2. 计算距离并判断是否需要移动

计算当前激光雷达位置到局部地图边界的距离：
d min , i = ∣ x i − v min , i ∣ d_{\text{min},i} = |x_i - \mathbf{v}_{\text{min},i}| dmin,i=∣xi−vmin,i∣
d max , i = ∣ x i − v max , i ∣ d_{\text{max},i} = |x_i - \mathbf{v}_{\text{max},i}| dmax,i=∣xi−vmax,i∣

如果任意维度满足以下条件之一，则需要移动局部地图：
d min , i ≤ γ ⋅ R d_{\text{min},i} \leq \gamma \cdot R dmin,i≤γ⋅R
d max , i ≤ γ ⋅ R d_{\text{max},i} \leq \gamma \cdot R dmax,i≤γ⋅R
其中：

γ \gamma γ 是边界移动的阈值比例（MOV_THRESHOLD）。
R R R 是检测范围（DET_RANGE）。

    // 检查是否需要移动局部地图
    float dist_to_map_edge[3][2]; // 当前位置到边界的距离
    bool need_move = false;       // 是否需要移动标志
    for (int i = 0; i < 3; i++) {
        dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); // 到最小边界距离
        dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); // 到最大边界距离
        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || 
            dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {
            need_move = true; // 如果接近边界，设置移动标志
        }
    }

3. 计算地图边界的移动距离

地图边界的移动距离计算为：
Δ d = max ⁡ ( L − 2 γ R 2 ⋅ 0.9 , R ⋅ ( γ − 1 ) ) \Delta d = \max\left(\frac{L - 2 \gamma R}{2} \cdot 0.9, R \cdot (\gamma - 1)\right) Δd=max(2L−2γR⋅0.9,R⋅(γ−1))

    // 计算新的局部地图范围
    BoxPointType New_LocalMap_Points, tmp_boxpoints;
    New_LocalMap_Points = LocalMap_Points;
    float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, 
                         double(DET_RANGE * (MOV_THRESHOLD - 1)));

4. 更新局部地图边界

当激光雷达位置接近最小边界时：
v min , i ← v min , i − Δ d \mathbf{v}_{\text{min},i} \gets \mathbf{v}_{\text{min},i} - \Delta d vmin,i←vmin,i−Δd
v max , i ← v max , i − Δ d \mathbf{v}_{\text{max},i} \gets \mathbf{v}_{\text{max},i} - \Delta d vmax,i←vmax,i−Δd

当激光雷达位置接近最大边界时：
v min , i ← v min , i + Δ d \mathbf{v}_{\text{min},i} \gets \mathbf{v}_{\text{min},i} + \Delta d vmin,i←vmin,i+Δd
v max , i ← v max , i + Δ d \mathbf{v}_{\text{max},i} \gets \mathbf{v}_{\text{max},i} + \Delta d vmax,i←vmax,i+Δd

   for (int i = 0; i < 3; i++) {
        tmp_boxpoints = LocalMap_Points;
        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) {
            New_LocalMap_Points.vertex_max[i] -= mov_dist; // 更新最大边界
            New_LocalMap_Points.vertex_min[i] -= mov_dist; // 更新最小边界
            tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
            cub_needrm.push_back(tmp_boxpoints); // 添加需要移除的立方体
        } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {
            New_LocalMap_Points.vertex_max[i] += mov_dist; // 更新最大边界
            New_LocalMap_Points.vertex_min[i] += mov_dist; // 更新最小边界
            tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
            cub_needrm.push_back(tmp_boxpoints); // 添加需要移除的立方体
        }
    }

5. 清理超出范围的点云数据

通过 KD 树方法移除超出范围的点云：
N deleted = KDTree.Delete_Boxes ( C remove ) N_{\text{deleted}} = \text{KDTree.Delete\_Boxes}(\mathbf{C}_{\text{remove}}) Ndeleted=KDTree.Delete_Boxes(Cremove)

  // 清理冗余点
    points_cache_collect(); // 收集点缓存
    double delete_begin = omp_get_wtime(); // 开始计时
    if (cub_needrm.size() > 0) 
        kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); // 调用KD树清理点云
    kdtree_delete_time = omp_get_wtime() - delete_begin; // 记录清理耗时

二. IMU_init

void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
  // 1. 初始化重力、陀螺仪偏差、加速度计和陀螺仪的协方差
  // 2. 将加速度测量值归一化为单位重力
  
  V3D cur_acc, cur_gyr;  // 当前加速度和陀螺仪测量值
  
  // 如果是第一帧数据
  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;  // 记录第一帧激光雷达开始时间
  }

  // 遍历所有IMU数据
  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);
  
  // 初始化陀螺仪偏差
  init_state.bg = mean_gyr;
  // 初始化激光雷达相对于IMU的位移和旋转
  init_state.offset_T_L_I = Lidar_T_wrt_IMU;
  init_state.offset_R_L_I = Lidar_R_wrt_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();
}

IMU_init 函数初始化惯性测量单元（IMU）的状态，包括计算加速度和角速度的均值与协方差、初始化滤波器状态，并将初始IMU状态用于后续滤波计算。以下是关键过程公式化描述：

1. 加速度与角速度均值计算

设 N N N 为当前已处理的IMU样本数，每次迭代更新加速度均值 μ a \mathbf{\mu}_a μa 和角速度均值 μ g \mathbf{\mu}_g μg：
μ a = μ a + a cur − μ a N \mathbf{\mu}_a = \mathbf{\mu}_a + \frac{\mathbf{a}_{\text{cur}} - \mathbf{\mu}_a}{N} μa=μa+Nacur−μa
μ g = μ g + g cur − μ g N \mathbf{\mu}_g = \mathbf{\mu}_g + \frac{\mathbf{g}_{\text{cur}} - \mathbf{\mu}_g}{N} μg=μg+Ngcur−μg
其中：

μ a \mathbf{\mu}_a μa：当前加速度均值。
μ g \mathbf{\mu}_g μg：当前角速度均值。
a cur \mathbf{a}_{\text{cur}} acur 和 g cur \mathbf{g}_{\text{cur}} gcur：当前IMU加速度和角速度。

 // 遍历所有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;

2. 加速度与角速度协方差更新

每次迭代更新加速度协方差 Σ a \mathbf{\Sigma}_a Σa 和角速度协方差 Σ g \mathbf{\Sigma}_g Σg：
Σ a = N − 1 N Σ a + N − 1 N 2 ( a cur − μ a ) ∘ ( a cur − μ a ) \mathbf{\Sigma}_a = \frac{N-1}{N} \mathbf{\Sigma}_a + \frac{N-1}{N^2} (\mathbf{a}_{\text{cur}} - \mathbf{\mu}_a) \circ (\mathbf{a}_{\text{cur}} - \mathbf{\mu}_a) Σa=NN−1Σa+N2N−1(acur−μa)∘(acur−μa)
Σ g = N − 1 N Σ g + N − 1 N 2 ( g cur − μ g ) ∘ ( g cur − μ g ) \mathbf{\Sigma}_g = \frac{N-1}{N} \mathbf{\Sigma}_g + \frac{N-1}{N^2} (\mathbf{g}_{\text{cur}} - \mathbf{\mu}_g) \circ (\mathbf{g}_{\text{cur}} - \mathbf{\mu}_g) Σg=NN−1Σg+N2N−1(gcur−μg)∘(gcur−μg)
其中：

Σ a \mathbf{\Sigma}_a Σa 和 Σ g \mathbf{\Sigma}_g Σg：分别为加速度和角速度的协方差矩阵。
∘ \circ ∘ 表示逐元素相乘。

    // 更新加速度计和陀螺仪的协方差
    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);

3. 初始化重力方向

根据归一化后的加速度均值 μ a \mathbf{\mu}_a μa 初始化重力方向 g \mathbf{g} g：
g = − μ a ∥ μ a ∥ ⋅ g \mathbf{g} = -\frac{\mathbf{\mu}_a}{\|\mathbf{\mu}_a\|} \cdot g g=−∥μa∥μa⋅g
其中：

g g g 为标准重力加速度，通常取 9.81 m/s 2 9.81 \, \text{m/s}^2 9.81m/s2。

  // 获取滤波器的初始状态
  state_ikfom init_state = kf_state.get_x();
  // 初始化重力向量，方向为平均加速度的反方向，大小为标准重力加速度
  init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);

4. 滤波器状态初始化

状态向量 x \mathbf{x} x 的初始值设置为：
x init = [ g μ g T L → I R L → I ] \mathbf{x}_{\text{init}} = \begin{bmatrix} \mathbf{g} \\ \mathbf{\mu}_g \\ \mathbf{T}_{L \to I} \\ \mathbf{R}_{L \to I} \end{bmatrix} xinit= gμgTL→IRL→I
其中：

T L → I \mathbf{T}_{L \to I} TL→I 和 R L → I \mathbf{R}_{L \to I} RL→I 为LiDAR相对于IMU的位移和旋转。

协方差矩阵 P \mathbf{P} P 的初始值：
P = diag ( σ g 2 , σ b g 2 , σ T L I 2 , σ R L I 2 ) \mathbf{P} = \text{diag}( \sigma_g^2, \sigma_{bg}^2, \sigma_{TLI}^2, \sigma_{RLI}^2 ) P=diag(σg2,σbg2,σTLI2,σRLI2)
其中各分量的值为：
σ g 2 = 0.00001 σ b g 2 = 0.0001 σ T L I 2 = 0.001 σ R L I 2 = 0.00001 \begin{aligned} \sigma_g^2 &= 0.00001 \\ \sigma_{bg}^2 &= 0.0001 \\ \sigma_{TLI}^2 &= 0.001 \\ \sigma_{RLI}^2 &= 0.00001 \end{aligned} σg2σbg2σTLI2σRLI2=0.00001=0.0001=0.001=0.00001

  // 获取滤波器的初始协方差矩阵
  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;

三. UndistortPcl

UndistortPcl 函数的核心功能是利用IMU数据对LiDAR点云进行去畸变处理（Undistortion），以矫正点云因激光扫描期间的运动引起的失真。主要过程包括IMU前向传播、后向点云补偿以及预测帧尾姿态。

void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
  // 将上一帧尾部的IMU数据添加到当前帧头部
  auto v_imu = meas.imu;
  v_imu.push_front(last_imu_);
  const double &imu_beg_time = v_imu.front()->header.stamp.toSec();  // IMU数据的开始时间
  const double &imu_end_time = v_imu.back()->header.stamp.toSec();  // IMU数据的结束时间

  double pcl_beg_time = meas.lidar_beg_time;  // 激光雷达数据的开始时间
  double pcl_end_time = meas.lidar_end_time;  // 激光雷达数据的结束时间

  // 如果是MARSIM类型，调整激光雷达时间范围
  if (lidar_type == MARSIM) {
      pcl_beg_time = last_lidar_end_time_;
      pcl_end_time = meas.lidar_beg_time;
  }

  // 按照时间戳对点云进行排序
  pcl_out = *(meas.lidar);
  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
  // 输出调试信息
  // cout << "[ IMU Process ]: Process lidar from " << pcl_beg_time << " to " << pcl_end_time << ", " \
  //          << meas.imu.size() << " imu msgs from " << imu_beg_time << " to " << imu_end_time << endl;

  // 初始化IMU姿态
  state_ikfom imu_state = kf_state.get_x();
  IMUpose.clear();
  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));

  // 前向传播每个IMU点
  V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
  M3D R_imu;

  double dt = 0;

  input_ikfom in;
  for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
  {
    auto &&head = *(it_imu);
    auto &&tail = *(it_imu + 1);
    
    if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue;
    
    // 计算角速度和加速度的平均值
    angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                  0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                  0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
    acc_avr   << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                  0.5 * (head->linear_acceleration.y + tail->angular_velocity.y),
                  0.5 * (head->linear_acceleration.z + tail->angular_velocity.z);

    // 归一化加速度
    acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;

    // 计算时间间隔
    if (head->header.stamp.toSec() < last_lidar_end_time_)
    {
      dt = tail->header.stamp.toSec() - last_lidar_end_time_;
    }
    else
    {
      dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
    }
    
    in.acc = acc_avr;
    in.gyro = angvel_avr;
    Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
    Q.block<3, 3>(3, 3).diagonal() = cov_acc;
    Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
    Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
    kf_state.predict(dt, Q, in);

    // 保存每个IMU测量的姿态
    imu_state = kf_state.get_x();
    angvel_last = angvel_avr - imu_state.bg;
    acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba);
    for (int i = 0; i < 3; i++)
    {
      acc_s_last[i] += imu_state.grav[i];
    }
    double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
    IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
  }

  // 计算帧末的位置和姿态预测
  double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
  dt = note * (pcl_end_time - imu_end_time);
  kf_state.predict(dt, Q, in);
  
  imu_state = kf_state.get_x();
  last_imu_ = meas.imu.back();
  last_lidar_end_time_ = pcl_end_time;

  // 对每个激光雷达点进行畸变校正（反向传播）
  if (pcl_out.points.begin() == pcl_out.points.end()) return;

  if (lidar_type != MARSIM) {
      auto it_pcl = pcl_out.points.end() - 1;
      for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
      {
          auto head = it_kp - 1;
          auto tail = it_kp;
          R_imu << MAT_FROM_ARRAY(head->rot);
          vel_imu << VEC_FROM_ARRAY(head->vel);
          pos_imu << VEC_FROM_ARRAY(head->pos);
          acc_imu << VEC_FROM_ARRAY(tail->acc);
          angvel_avr << VEC_FROM_ARRAY(tail->gyr);

          for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--)
          {
              dt = it_pcl->curvature / double(1000) - head->offset_time;

              // 只使用旋转将点转换到“末”帧
              // 注意：补偿方向与帧移动方向相反
              // 因此，如果要将时间戳为i的点补偿到帧e
              // P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei)，其中T_ei表示在全局坐标系中
              M3D R_i(R_imu * Exp(angvel_avr, dt));

              V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
              V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
              V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I); // 不够准确！

              // 保存校正后的点和它们的旋转
              it_pcl->x = P_compensate(0);
              it_pcl->y = P_compensate(1);
              it_pcl->z = P_compensate(2);

              if (it_pcl == pcl_out.points.begin()) break;
          }
      }
  }
}

1. IMU前向传播

IMU前向传播用于根据IMU测量值预测每个IMU时间点的姿态与速度。

1.1 平均角速度与加速度

每对连续的IMU测量值，计算平均角速度 ω avg \boldsymbol{\omega}_{\text{avg}} ωavg 和加速度 a avg \mathbf{a}_{\text{avg}} aavg：
ω avg = ω head + ω tail 2 \boldsymbol{\omega}_{\text{avg}} = \frac{\boldsymbol{\omega}_{\text{head}} + \boldsymbol{\omega}_{\text{tail}}}{2} ωavg=2ωhead+ωtail
a avg = a head + a tail 2 \mathbf{a}_{\text{avg}} = \frac{\mathbf{a}_{\text{head}} + \mathbf{a}_{\text{tail}}}{2} aavg=2ahead+atail
其中：

ω head \boldsymbol{\omega}_{\text{head}} ωhead 和 ω tail \boldsymbol{\omega}_{\text{tail}} ωtail：前后IMU角速度。
a head \mathbf{a}_{\text{head}} ahead 和 a tail \mathbf{a}_{\text{tail}} atail：前后IMU加速度。

    // 计算角速度和加速度的平均值
    angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                  0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                  0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
    acc_avr   << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                  0.5 * (head->linear_acceleration.y + tail->angular_velocity.y),
                  0.5 * (head->linear_acceleration.z + tail->angular_velocity.z);

1.2 加速度归一化

归一化加速度以补偿重力影响：
a norm = a avg ⋅ g ∥ a mean ∥ \mathbf{a}_{\text{norm}} = \mathbf{a}_{\text{avg}} \cdot \frac{g}{\|\mathbf{a}_{\text{mean}}\|} anorm=aavg⋅∥amean∥g

    // 归一化加速度
    acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;

1.3 IMU状态更新

IMU状态通过预测时间 d t dt dt 更新：
x imu ( t + Δ t ) = f ( x imu ( t ) , u imu , Δ t ) \mathbf{x}_{\text{imu}}(t+\Delta t) = f(\mathbf{x}_{\text{imu}}(t), \mathbf{u}_{\text{imu}}, \Delta t) ximu(t+Δt)=f(ximu(t),uimu,Δt)
其中 x imu \mathbf{x}_{\text{imu}} ximu 包括：

p \mathbf{p} p：位置。
v \mathbf{v} v：速度。
R \mathbf{R} R：旋转矩阵。

  // 计算帧末的位置和姿态预测
  double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
  dt = note * (pcl_end_time - imu_end_time);
  kf_state.predict(dt, Q, in);

2. 点云去畸变补偿

通过IMU的姿态和速度信息，对每个激光点进行去畸变补偿。

2.1 点云的时间偏移

激光点相对于帧开始的时间偏移为：
Δ t = t point − t frame_start \Delta t = t_{\text{point}} - t_{\text{frame\_start}} Δt=tpoint−tframe_start

2.2 点云坐标变换

将点 P i P_i Pi 的坐标转换至帧尾：
P comp = R frame_end ⊤ ⋅ ( R imu ⋅ P i + T comp ) P_{\text{comp}} = \mathbf{R}_{\text{frame\_end}}^\top \cdot \left(\mathbf{R}_{\text{imu}} \cdot P_i + \mathbf{T}_{\text{comp}}\right) Pcomp=Rframe_end⊤⋅(Rimu⋅Pi+Tcomp)
其中：

T comp \mathbf{T}_{\text{comp}} Tcomp 是从IMU的位移和加速度推导出的位移补偿：
T comp = v ⋅ Δ t + 1 2 a ⋅ ( Δ t ) 2 \mathbf{T}_{\text{comp}} = \mathbf{v} \cdot \Delta t + \frac{1}{2} \mathbf{a} \cdot (\Delta t)^2 Tcomp=v⋅Δt+21a⋅(Δt)2

3. 帧尾姿态预测

在点云采集结束时，利用IMU预测帧尾姿态：
x imu_end = f ( x imu_prev , u imu , t end − t prev ) \mathbf{x}_{\text{imu\_end}} = f(\mathbf{x}_{\text{imu\_prev}}, \mathbf{u}_{\text{imu}}, t_{\text{end}} - t_{\text{prev}}) ximu_end=f(ximu_prev,uimu,tend−tprev)

 // 对每个激光雷达点进行畸变校正（反向传播）
  if (pcl_out.points.begin() == pcl_out.points.end()) return;

  if (lidar_type != MARSIM) {
      auto it_pcl = pcl_out.points.end() - 1;
      for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
      {
          auto head = it_kp - 1;
          auto tail = it_kp;
          R_imu << MAT_FROM_ARRAY(head->rot);
          vel_imu << VEC_FROM_ARRAY(head->vel);
          pos_imu << VEC_FROM_ARRAY(head->pos);
          acc_imu << VEC_FROM_ARRAY(tail->acc);
          angvel_avr << VEC_FROM_ARRAY(tail->gyr);

          for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--)
          {
              dt = it_pcl->curvature / double(1000) - head->offset_time;

              // 只使用旋转将点转换到“末”帧
              // 注意：补偿方向与帧移动方向相反
              // 因此，如果要将时间戳为i的点补偿到帧e
              // P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei)，其中T_ei表示在全局坐标系中
              M3D R_i(R_imu * Exp(angvel_avr, dt));

              V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
              V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
              V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I); // 不够准确！

              // 保存校正后的点和它们的旋转
              it_pcl->x = P_compensate(0);
              it_pcl->y = P_compensate(1);
              it_pcl->z = P_compensate(2);

              if (it_pcl == pcl_out.points.begin()) break;
          }
      }
  }

标签：acc,mathbf,text,lio,fast,init,imu,pcl,解析
From： https://blog.csdn.net/weixin_41331879/article/details/145105953