激光点云的畸变校正

0.引言

校正由于运动引起的激光点云畸变。

• 参考阅读

点云去畸变的方法包括：

• 纯估计方法（ICP/VICP）
• 传感器辅助方法（IMU/ODOM）
• 融合的方法

这里学习的也是传感器辅助方法。多余的也不再赘述，自己理一下几个坐标关系并做记录。

1.理解几个坐标系的关系

畸变产生的原因如图所示，自己理解也是基于这张图进行理解。

图片来源

这个坐标系关系和后续的代码相对应。

通过线性插值，计算补偿矩阵的原理：

雷达扫描一帧的时间是固定的，通过计算得到每个点的采集时刻，将所有点都统一到同一时刻，**假设选择每完成一帧扫描的结束时刻，**如图所示，直观的理解就是：

• 1.时间越靠近 $t_k$ ，补偿矩阵越近似于 $T_{k+1,k}$
• 2.时间越靠近 $t_{k+1}$ ，补偿矩阵越接近于单位阵，就是不需要补偿

$$T^i=\frac{t_{k+1}-t_i}{t_{k+1}-t_k}{T}_{k+1,k}$$

最后将相应的点云乘上补偿矩阵即可。

2.代码阅读

/**
 * @description:激光雷达运动补偿： 使用里程计数据进行运动补偿
 *              IMU 与 lidar先标定
 *              里程计数据是通过IMU计算所得
 * @param ：Transform &extr： the extr is T_imu_lidar
            LoaderPointcloudPtr org_pc：运动补偿前
            PointCloudXYZNPtr comp_pc：运动补偿后
            double min_time：t_k
            double max_time：t_k+1
            Transform &start_pose：IMU pose t_k
            Transform &end_pose：IMU pose t_k+1
 */
void LidarMotionCompensation(Transform &extr, LoaderPointcloudPtr org_pc,
                             PointCloudXYZNPtr comp_pc, double min_time,
                             double max_time, Transform &start_pose,
                             Transform &end_pose) {
  // the pose is imu pose T_w_imu
  // 将IMU pose转到lidar下
  Transform start_lidar_pose = start_pose * extr;
  Transform end_lidar_pose = end_pose * extr;
  // 两帧之间的相对位姿 T_k+1,k
  Transform relative_pose = end_lidar_pose.inverse() * start_lidar_pose;

  Eigen::Vector3f relative_tran = relative_pose.translation_;
  Eigen::Quaternionf relative_quat = relative_pose.rotation_;

  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_pc(
      new pcl::PointCloud<pcl::PointXYZ>());

  // 使用其他传感器辅助进行运动补偿
  // https://zhuanlan.zhihu.com/p/351109327
  //  temp_pc->resize(org_pc->size());
  double time_range = max_time - min_time;
  // !对每个点计算补偿变换矩阵
  for (int i = 0; i < org_pc->size(); i++) {
    auto point = org_pc->points[i]; // 补偿前的点
    double ratio = (max_time - point.timestamp) / time_range;
    Eigen::Quaternionf identity_; 
    identity_.setIdentity(); // 初始化为单位四元数
    // step 1.计算两个四元数间的球面线性插值，并利用四元数初始化补偿变换矩阵；
    Eigen::Affine3f trans(identity_.slerp(ratio, relative_quat));
    // step 2 平移部分直接进行线性插值
    trans.translation() = ratio * relative_tran;
    Eigen::Vector3f pt(point.x, point.y, point.z);
    Eigen::Vector3f comp_pt = pt;
    // step 3 原始点 * 补偿矩阵
    if (max_time - min_time > 1e-3) {
      comp_pt = trans * pt;
      std::cout << "trans the point" << std::endl;
    }

    // std::cout<<"org pt "<
    // std::cout<<"comp pt "<
    // std::cout<<"trans pose " <

    if (std::isnan(comp_pt[0]) || std::isnan(comp_pt[1]) ||
        std::isnan(comp_pt[2]))
      continue;
    pcl::PointXYZ pc;
    pc.x = comp_pt[0];
    pc.y = comp_pt[1];
    pc.z = comp_pt[2];

    temp_pc->points.push_back(pc);
    // temp_pc->points[i].x = comp_pt[0];
    // temp_pc->points[i].y = comp_pt[1];
    // temp_pc->points[i].z = comp_pt[2];
  }

  // std::cout<<"org pc is "<size()<
  // std::cout<<"temp pc is "<size()<
  // step 4 估计法向量
  pcl::NormalEstimationOMP<pcl::PointXYZ, PointXYZN> norm_est;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(
      new pcl::search::KdTree<pcl::PointXYZ>());

  norm_est.setNumberOfThreads(8);
  norm_est.setInputCloud(temp_pc);
  norm_est.setSearchMethod(kdtree);
  norm_est.setKSearch(20);
  norm_est.compute(*comp_pc);
}