雷达点云去畸变

参考链接：从零开始做自动驾驶定位(八): 点云畸变补偿 - 知乎

一、点云去畸变大致过程：

1. 创建一个原始点云的副本(origin_cloud_ptr)。
2. 计算起始点的方向(start_orientation)，并基于该方向创建一个旋转矩阵(rotate_matrix)。然后使用这个旋转矩阵将原始点云进行变换，以使起始点的方向对齐到X轴。
3. 将速度(velocity_)和角速度(angular_rate_)也通过旋转矩阵进行变换。
4. 对原始点云中的每个点进行遍历，计算其方向(orientation)并判断是否需要删除。如果不需要删除，则根据方向计算真实时间(real_time)，并根据当前时间更新点的位置。
5. 将调整后的点添加到输出点云(output_cloud_ptr)中。
6. 最后，将输出点云还原回到原始坐标系中。

代码实现

bool DistortionAdjust::AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr) {
    CloudData::CLOUD_PTR origin_cloud_ptr(new CloudData::CLOUD(*input_cloud_ptr));
    output_cloud_ptr.reset(new CloudData::CLOUD());

    float orientation_space = 2.0 * M_PI;
    float delete_space = 5.0 * M_PI / 180.0;
    float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x);

    Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());
    Eigen::Matrix3f rotate_matrix = t_V.matrix();
    Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
    transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();
    pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);

    velocity_ = rotate_matrix * velocity_;
    angular_rate_ = rotate_matrix * angular_rate_;

    for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {
        //-PI，PI
        float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x);
        if (orientation < 0.0)
            orientation += 2.0 * M_PI;
        
        if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)
            continue;

        float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0;

        Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x,
                                     origin_cloud_ptr->points[point_index].y,
                                     origin_cloud_ptr->points[point_index].z);

        Eigen::Matrix3f current_matrix = UpdateMatrix(real_time);
        Eigen::Vector3f rotated_point = current_matrix * origin_point;
        Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time;
        CloudData::POINT point;
        point.x = adjusted_point(0);
        point.y = adjusted_point(1);
        point.z = adjusted_point(2);
        output_cloud_ptr->points.push_back(point);
    }

    pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());
    return true;
}

二、时间对齐

kitti数据集，bag里点云的时刻是该帧点云起始时刻和终止时刻的平均值，所以应该往前移半个周期

float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0;