点云畸变补偿时间插值

Enochzhu

已于 2022-06-04 23:47:08 修改

阅读量288

点赞数

分类专栏： SLAM 面试文章标签：自动驾驶

于 2022-06-04 23:21:47 首次发布

本文链接：https://blog.csdn.net/Enochzhu/article/details/125124076

版权

SLAM 同时被 2 个专栏收录

7 篇文章 0 订阅

订阅专栏

面试

7 篇文章 0 订阅

订阅专栏

本文介绍了如何使用代码实现点云数据的时间插值，通过旋转和平移调整点云，根据给定的起始和结束角度以及扫描周期进行插值计算，调整每个点的位置。该过程涉及欧拉角转换、矩阵运算和时间比例因子的计算，适用于点云处理和三维重建等领域。

摘要由CSDN通过智能技术生成

前言

时间插值

一、深蓝学院代码示例

adjusted_cloud_ptr->points.resize(input_cloud_ptr->points.size());
Eigen::Vector3f point, rotated_point, adjusted_point;
Eigen::Matrix3f current_matrix;
bool half_pased = false;
for (size_t point_index = 0; point_index < input_cloud_ptr->points.size(); point_index ++ ){
    point(0) = input_cloud_ptr->points[point_index].x;
    point(1) = input_cloud_ptr->points[point_index].y;
    point(2) = input_cloud_ptr->points[point_index].z;

    float ori = -atan2(point(1), point(0));
    if(!half_pased){
        if(ori < start_orientation - M_PI / 2)  ori += 2 * M_PI;
        else if(ori > start_orientation + M_PI * 3 / 2) ori -= 2 * M_PI;
        if (ori - start_orientation > M_PI) half_pased = true;
    }
    else{
        ori += 2 * M_PI;
        if ( ori < end_orientation - M_PI * 3 / 2) ori += 2 * M_PI;
        else if (ori > end_orientation + M_PI / 2) ori -= 2 * M_PI;
    }
    float real_time = (ori - start_orientation) / orientation_diff * scan_peirod_;//orientation_diff = 360, scan_period
    current_matrix = UpdateMatrix(real_time);
    rotated_point = current_matrix * point;
    adjusted_point = rotated_point * velocity_ * real_time;
    adjusted_cloud_ptr->points[point_index].x = adjusted_point(0);
    adjusted_cloud_ptr->points[point_index].y = adjusted_point(1);
    adjusted_cloud_ptr->points[point_index].z = adjusted_point(2);
}