一、傳感器時間同步

多傳感器融合過程中由於傳感器之間的採集頻率不同，導致無法保證傳感器數據同步。這裏以激光雷達爲核心傳感器，每次收到一次雷達數據，便以當前雷達數據採集時刻作爲要插入的時間點，該時刻另一傳感器IMU的數據通過插值獲得。這裏同樣可以參考VINS裏相機和IMU時間同步的函數代碼getMeasurements（）。

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

ReadData()

從ros緩衝區中傳感器數據取出來，並放進XXX_data_buff_容器中
傳感器同步，另外三種傳感器做插值
如果任意傳感器沒有插值，剔除該雷達點雲幀，找下一個點雲幀做融合
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數據爲例，三種傳感器流程類似。

找到與同步時間相鄰的左右兩個數據，在UnsyncedData.at(0)和.at(1)之間
根據時間差計算權重線性插值

輸入原始數據，輸出線性插值後的數據。

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

激光雷達點雲預處理：傳感器時間同步、點雲去畸變

一、傳感器時間同步

ReadData()

ParseData()

插值SyncData()

二、點雲畸變補償

程序設計

去畸變scan_adjust

1.獲取運動信息SetMotionInfo（）

2.去除畸變AdjustCloud（）

激光雷達點雲預處理：傳感器時間同步、點雲去畸變

基於NDT的前端里程計框架優化

基於NDT的前端里程計代碼解析

LOAM源碼解析4——transformMaintenance

LOAM源碼解析3——laserMapping

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結