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

一、传感器时间同步

多传感器融合过程中由于传感器之间的采集频率不同，导致无法保证传感器数据同步。这里以激光雷达为核心传感器，每次收到一次雷达数据，便以当前雷达数据采集时刻作为要插入的时间点，该时刻另一传感器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