激光雷达点云预处理：传感器时间同步、点云去畸变

一、传感器时间同步

多传感器融合过程中由于传感器之间的采集频率不同，导致无法保证传感器数据同步。这里以激光雷达为核心传感器，每次收到一次雷达数据，便以当前雷达数据采集时刻作为要插入的时间点，该时刻另一传感器IMU的数据通过插值获得。这里同样可以参考VINS里相机和IMU时间同步的函数代码getMeasurements（）。

主程序在front_end_flow.cpp文件中的ReadData()函数添加。

ReadData()

1. 从ros缓冲区中传感器数据取出来，并放进XXX_data_buff_容器中
2. 传感器同步，另外三种传感器做插值
3. 如果任意传感器没有插值，剔除该雷达点云帧，找下一个点云帧做融合
   1、读取传感器数据放进XXX_data_buff_容器中
   雷达点云作为主传感器，其他传感器做插值。包含IMU信息(unsynced_imu_)、速度信息(unsynced_velocity_)、GNSS信息(unsynced_gnss_)

// 雷达点云不需要插值，直接放进cloud_data_buff_容器中
    cloud_sub_ptr_->ParseData(cloud_data_buff_);
    // 其他传感器原始数据放进未做同步的临时容器中unsynced_XXX_
    static std::deque<IMUData> unsynced_imu_;
    static std::deque<VelocityData> unsynced_velocity_;
    static std::deque<GNSSData> unsynced_gnss_;
    // 放进XXX_data_buff_容器中
    imu_sub_ptr_->ParseData(unsynced_imu_);
    velocity_sub_ptr_->ParseData(unsynced_velocity_);
    gnss_sub_ptr_->ParseData(unsynced_gnss_);
    // 点云容器不能为空
    if (cloud_data_buff_.size() == 0)
        return false;

以imu的ParseData()函数为例：

ParseData()

读取数据，将新的传感器数据new_imu_data_填充进imu容器imu_data_buff中。

void IMUSubscriber::ParseData(std::deque<IMUData>& imu_data_buff) {
    if (new_imu_data_.size() > 0) {
        imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end());
        new_imu_data_.clear();
    }
}

2、传感器同步，另外三种传感器做插值
cloud_time为主传感器激光雷达的参考时间，然后后面通过SyncData（）函数对三个传感器进行插值实现同步操作。具体就是索引和插值。

 double cloud_time = cloud_data_buff_.front().time;

    bool valid_imu = IMUData::SyncData(unsynced_imu_, imu_data_buff_, cloud_time);
    bool valid_velocity = VelocityData::SyncData(unsynced_velocity_, velocity_data_buff_, cloud_time);
    bool valid_gnss = GNSSData::SyncData(unsynced_gnss_, gnss_data_buff_, cloud_time);

3、如果任意传感器没有插值，剔除该雷达点云帧，找下一个点云帧做融合

 static bool sensor_inited = false;
    if (!sensor_inited) {
        if (!valid_imu || !valid_velocity || !valid_gnss) {
            cloud_data_buff_.pop_front();
            return false;
        }
        sensor_inited = true;
    }

插值SyncData()

插值的流程是先获得主传感器时间，然后根据索引结果获得这一同步时间前后的两帧数据，根据前后两帧数据的采集时刻，以及要插入的时刻，根据比例获得权重得到另一传感器在同步时间的结果。这里以IMU数据为例，三种传感器流程类似。

1. 找到与同步时间相邻的左右两个数据，在UnsyncedData.at(0)和.at(1)之间
2. 根据时间差计算权重线性插值

输入原始数据，输出线性插值后的数据。

bool IMUData::SyncData(std::deque<IMUData>& UnsyncedData, std::deque<IMUData>& SyncedData, double sync_time) {
    // 1.保证数据大于2个，同步时间在两个传感器数据UnsyncedData.at(0)和.at(1)中间
    while (UnsyncedData.size() >= 2) {
        // a. 同步时间sync小于传感器第一个数据,失败
        if (UnsyncedData.front().time > sync_time) 
            return false;
        // b. 同步时间sync大于第二个数据，将第二个设置为.at(0)
        if (UnsyncedData.at(1).time < sync_time) {
            UnsyncedData.pop_front();
            continue;
        }
        // c. 距离前一个时间差值较大，说明数据有丢失，前一个不能用
        if (sync_time - UnsyncedData.front().time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        // d. 距离后一个时间差值较大，说明数据有丢失，后一个不能用
        if (UnsyncedData.at(1).time - sync_time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        break;
    }
    if (UnsyncedData.size() < 2)
        return false;
    // 同步数据synced，前后两帧数据front、back
    IMUData front_data = UnsyncedData.at(0);
    IMUData back_data = UnsyncedData.at(1);
    IMUData synced_data;
    // 2、根据时间差计算权重线性插值
    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;// 同步时间
    // 线速度
    synced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale;
    synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale;
    synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale;
    // 角速度
    synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale;
    synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale;
    synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale;
    // 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大是，二者精度相当
    // 由于是对相邻两时刻姿态插值，姿态差比较小，所以可以用线性插值
    synced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale;
    synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale;
    synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale;
    synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale;
    synced_data.orientation.Normlize();// 线性插值之后要归一化
    
    // 最后插入即可
    SyncedData.push_back(synced_data);

    return true;
}
}

四种同步时间关系如下图所示。

二、点云畸变补偿

激光点数据不是瞬时获得的，激光测量时伴随着机器人的运动，一个数据采集周期内当激光雷达运动到不同位置时，机器人以为自己是在同一个位置测量出来的，所以会出现畸变。

激光雷达扫描过程中采集三维点云数据的过程在之前的博客《激光雷达坐标系、方向角和仰角》
点云数据如下图所示，以Velodyne公司的64线激光雷达HDL_64E为例

• 64线表示64个发射接收机，每一时刻可以得到64个激光点，发射机的发射竖直角度在-24.8~~2度之间，竖直方向分辨率是0.4°
• 一列64个发射接收机绕着竖直方向旋转，采集一圈得到4500列激光点，水平方向上分辨率是0.08°。
• 每一个圆圈代表一个激光束旋转一周产生的数据

解决畸变首先计算采集过程中雷达的运动，然后在每帧激光点上补偿这个运动量。假设采集周期100ms内为匀速运动，可以采用旋转+平移补偿。

程序设计

增加的点云去除畸变操作只是在front_end_flow.cpp中的更新激光里程计函数UpdateLaserOdometry()中位姿估计之前进行了每一帧的点云去畸变操作。具体操作为：

    // imu系转激光雷达系
    current_velocity_data_.TransformCoordinate(imu_to_lidar_);
    // 每帧点云处理畸变：先获得运动信息，再坐标转化
    distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
    distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

第一点，激光雷达扫描过程中采集的数据中提供的速度是IMU所处位置的速度，我们需要将其转化到激光雷达所处位置的速度（线速度+角速度）。

current_velocity_data_.TransformCoordinate(imu_to_lidar_);

这里速度转换，角速度直接根据imu_to_lidar_变换矩阵相乘即可，线速度需要先旋转再加上叉乘结果。
为此在include文件中专门定义了一个类VelocityData，里面包含线速度linear_velocity、角速度angular_velocity。以及类成员函数TransformCoordinate（）

// imu转雷达坐标系：线速度、角速度
void VelocityData::TransformCoordinate(Eigen::Matrix4f transform_matrix) {
    // 杆臂矩阵
    Eigen::Matrix4d matrix = transform_matrix.cast<double>();
    Eigen::Matrix3d t_R = matrix.block<3,3>(0,0);

    // 角速度更新
    Eigen::Vector3d w(angular_velocity.x, angular_velocity.y, angular_velocity.z);
    w = t_R * w;

    // 线速度先旋转
    Eigen::Vector3d v(linear_velocity.x, linear_velocity.y, linear_velocity.z);
    v = t_R * v;
    // 再加上叉乘结果，杆臂速度=角速度 叉乘 杆臂
    Eigen::Vector3d r(matrix(0,3), matrix(1,3), matrix(2,3));
    Eigen::Vector3d delta_v;
    delta_v(0) = w(1) * r(2) - w(2) * r(1);
    delta_v(1) = w(2) * r(0) - w(0) * r(2);
    delta_v(2) = w(1) * r(1) - w(1) * r(0);
    v = v + delta_v;

    angular_velocity.x = w(0);
    angular_velocity.y = w(1);
    angular_velocity.z = w(2);
    linear_velocity.x = v(0);
    linear_velocity.y = v(1);
    linear_velocity.z = v(2);
}

将线速度和角速度从IMU系转化到激光雷达坐标系之后，需要进行点云去畸变操作。为此在models文件夹下专门建立一个文件夹scan_adjust，里面包含两个重要函数：获得运动信息SetMotionInfo（）和去畸变AdjustCloud（）。

// 每帧点云处理畸变：先获得运动信息，再坐标转化
    distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
    distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

去畸变scan_adjust

1.获取运动信息SetMotionInfo（）

获取载体运动信息，角速度angular_rate_ 、速度velocity_分别用来计算相对角度和相对位移。

    // 获得运动信息:线速度、角速度
void DistortionAdjust::SetMotionInfo(float scan_period, VelocityData velocity_data) {
    scan_period_ = scan_period;
    velocity_ << velocity_data.linear_velocity.x, velocity_data.linear_velocity.y, velocity_data.linear_velocity.z;
    angular_rate_ << velocity_data.angular_velocity.x, velocity_data.angular_velocity.y, velocity_data.angular_velocity.z;
}

2.去除畸变AdjustCloud（）

在已知载体运动信息，角速度和线速度之后，接下来要对每个扫描周期内激光点云进行补偿。载体运动过程中，每一列激光点的基准坐标系不同，所以我们需要在得到每个激光束接收时刻后，将激光点云转换到相对与初始时刻的相对运动即可，让此时激光点坐标乘上这个相对转换，就转换成了在初始坐标系下的激光点。
函数输入原始点云，输出去除畸变的点云。

bool DistortionAdjust::AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr)

0、预处理
初始化输入点云和输出点云。

 // 输入、输出点云
    CloudData::CLOUD_PTR origin_cloud_ptr(new CloudData::CLOUD(*input_cloud_ptr));// 原始点云是输入点云
    output_cloud_ptr->points.clear();

    float orientation_space = 2.0 * M_PI;// 方向角一圈 2*pi
    float delete_space = 5.0 * M_PI / 180.0; // 过滤正负5度激光点

1、初始点云先旋转
通过atan2(y/x)第一个激光点的方位角，并以Z轴为旋转角，旋转弧度为方位角，得到旋转矩阵rotate_matrix。
对初始点云进行旋转pcl::transformPointCloud
更新线速度、角速度（旋转）

    // 1.初始点云先旋转
    // 起始点旋转矩阵
    float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x);// 起始点方位角（欧拉角） atan2(y/x)
    Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ()); // 旋转向量，以Z轴为旋转轴，旋转方位角弧度
    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_;

2、后续点云去除畸变

• 获得每个激光点的方位角orientation并预处理
• 根据顺序扫描时间real_time对点云进行去畸变处理（旋转+平移）
• 点云去除畸变后旋转pcl::transformPointCloud

    for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {
        // 2.1 获得方位角及预处理
        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)// 过滤了+-5度的点
            continue;

        // 2.2 根据顺序扫描时间对点云进行去畸变处理（旋转+平移）
        // 减去scan_period/2，把点云转换到该帧采集的中间时刻上去
        // bag里点云的时刻是该帧点云起始时刻和终止时刻的平均值
        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);
    }
    // 2.3 点云去除畸变后旋转
    pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());
    return true;
}

获得当前旋转矩阵UpdateMatrix（）
由顺序扫描时间和角速度 获得当前扫描的角度，然后通过旋转向量Z-Y-X，最后转化为旋转矩阵。

// 已知运行时间获得旋转矩阵
Eigen::Matrix3f DistortionAdjust::UpdateMatrix(float real_time) {
    // 角度=角速度*运行时间
    Eigen::Vector3f angle = angular_rate_ * real_time;
    // 旋转向量z-y-x
    Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf t_V;
    t_V = t_Vz * t_Vy * t_Vx;
    // 转为旋转矩阵
    return t_V.matrix();
}

Eigen库中各种形式的表示如下：

旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1）:Eigen::AngleAxisd
四元数（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d

仿射变换Affine3f进行点云变换。

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
 
  // 在 X 轴上定义一个 2.5 米的平移.
  transform_2.translation() << 2.5, 0.0, 0.0;
  // 和前面一样的旋转; Z 轴上旋转 theta 弧度
  transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));

  // 执行变换，并将结果保存在新创建的‎‎ transformed_cloud ‎‎中
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  // pcl库实现点云变化
  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

获得AngleAxisd旋转向量

//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
    AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
    cout << "Rotation_vector1" << endl << V1.matrix() << endl;

参考：
https://zhuanlan.zhihu.com/p/109379384