slam定位学习笔记(四)

本篇是学习任乾大佬，这篇文章的学习笔记。主要内容是将不同传感器数据使用时间进行同步。

在多传感器融合的工程设计中，最初的一步就是将各个传感器的时间进行同步。多传感器数据的时间同步是多传感器信息融合的必备条件之一，但由于各个传感器独立封装并按照自己的时钟基准运行，往往采样频率也不尽不同，这些将导致采样数据时间上不同步。针对这一问题有两个解决方案。分别是硬同步和软同步。

硬同步是指定制传感器，使得多个传感器能在同一时刻触发采样，例如多个传感器安装于同一个晶振下工作。主要使用gnss进行授时，传感器自身集成了GPS通信接口，可以接收一些数据和信号。目前使用比较多的是使用PTP协议解决各传感器设备时间同步的问题。

软件同步是指通过统一的主机给各个传感器提供基准时间，使得传感器每帧数据同步到统一的时间戳上。

详细的解释可以参考这篇文章，本文主要针对软件同步。

一、kitti同步设置

kitti数据集采用GNSS进行了硬件同步，它的lidar和imu都是10hz，但lidar每次采集的时间都要比imu慢几十毫秒，所以要获得lidar时刻的时候，imu的数据并不是相同时刻的。所以需要我们通过插值来获得同一时刻的等效值。

二、IMU插值方法

这篇文章总结的很详细，对于四元数的插值方法进行总结：有线性插值、球面线性插值等，本文主要采用最简单的线性插值。

总结主要的流程：索引这一时刻前后的两帧数据，然后再进行插值计算。

三、程序分析

从sensor_data开始：

sensor_data添加了一个源码，velocity_data.hpp，主要是设定速度信息的数据格式。然后其他的源码主要以cloud_data为对齐传感器，所以它的代码没有改动，而gnss_data和imu_data都添加了一个对其函数SyncData()，具体看源码分析。

关于velocity_data.hpp:

#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_

#include <deque>
#include <Eigen/Dense>

namespace lidar_localization {
class VelocityData {
  public:
    struct LinearVelocity {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    struct AngularVelocity {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    double time = 0.0;
    LinearVelocity linear_velocity;
    AngularVelocity angular_velocity;
  
  public:
    static bool SyncData(std::deque<VelocityData>& UnsyncedData, std::deque<VelocityData>& SyncedData, double sync_time);
};
}
#endif

主要是设置了一个数据类VelocityData，它里面有两个struct，VelocityData和AngularVelocity，分别是线速度和角速度。还有一个处理是时间同步的函数SyncData。

velocity_data.cpp:
 

#include "lidar_localization/sensor_data/velocity_data.hpp"

#include "glog/logging.h"

namespace lidar_localization {
bool VelocityData::SyncData(std::deque<VelocityData>& UnsyncedData, std::deque<VelocityData>& SyncedData, double sync_time) {
    // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置
    // 即找到与同步时间相邻的左右两个数据
    // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值
    while (UnsyncedData.size() >= 2) {
        if (UnsyncedData.front().time > sync_time)
            return false;
        if (UnsyncedData.at(1).time < sync_time) {
            UnsyncedData.pop_front();
            continue;
        }
        if (sync_time - UnsyncedData.front().time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        if (UnsyncedData.at(1).time - sync_time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        break;
    }
    if (UnsyncedData.size() < 2)
        return false;

    VelocityData front_data = UnsyncedData.at(0);
    VelocityData back_data = UnsyncedData.at(1);
    VelocityData synced_data;

    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_velocity.x = front_data.linear_velocity.x * front_scale + back_data.linear_velocity.x * back_scale;
    synced_data.linear_velocity.y = front_data.linear_velocity.y * front_scale + back_data.linear_velocity.y * back_scale;
    synced_data.linear_velocity.z = front_data.linear_velocity.z * front_scale + back_data.linear_velocity.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;

    SyncedData.push_back(synced_data);

    return true;
}
}

主要内容是对于SyncData函数的定义，现在来逐行分析：

第一个参数是未经过时间同步的原始数据队列：std::deque<VelocityData> & UnsyncedData

第二个参数是经过时间同步的数据队列：std::deque<VelocityData>& SyncedData

第三个参数是同步的时间：double sync_time

    while (UnsyncedData.size() >= 2) {
        if (UnsyncedData.front().time > sync_time)
            return false;
        if (UnsyncedData.at(1).time < sync_time) {
            UnsyncedData.pop_front();
            continue;
        }
        if (sync_time - UnsyncedData.front().time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        if (UnsyncedData.at(1).time - sync_time > 0.2) {
            UnsyncedData.pop_front();
            break;
        }
        break;
    }

所有的操作都是将两个原始数据等效计算成一个所有必须判断原始数据的数量是否大于两个。

第一个if判断是第一个数据如果大于标准时间就是在标准时刻的后面就直接返回false，因为数据同步是要求两个数据的时刻正好将标准时刻夹在中间。

第二个if判断是第二个数据如果小于标准时刻就先将地一个数据推出deque然后跳出这个if判断。

第三个if判断是确定了第一个数据在标准时刻之前，或者是经过第二个判断后的新的第一个数据，当标准时刻落后第一个时刻的时间大于0.2秒的时候，就说明所有数据丢失，直接将第一个数据推出丢弃，然后跳出while。

第四个if判断是对于第二个数据的处理，如果第二个数据与标准时刻的时间差大于0.2也将第一个数据推出丢弃，并退出while循环。

因为imu和lidar的频率都是10hz，所以说是0.1s，设置为0.2秒就一定是丢数据了。

感觉代码写的有问题，这个while的主要作用是保证得到：

UnsyncedData.at(0).time < sync_time < UnsyncedData.at(1).time

如果有三个数据，前两个数据将标准时间包含，但符合第三个if，所以直接break，后面还是会继续运行，但就不符合while的要求。

在评论区有纠正的方法就是将最后两个if的break更改为return false。

    VelocityData front_data = UnsyncedData.at(0);
    VelocityData back_data = UnsyncedData.at(1);
    VelocityData synced_data;

这里就是将两个数据进行赋值，并创建一个新的数据synced_data用来保存线性插值后得到的新的数据。

    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_velocity.x = front_data.linear_velocity.x * front_scale + back_data.linear_velocity.x * back_scale;
    synced_data.linear_velocity.y = front_data.linear_velocity.y * front_scale + back_data.linear_velocity.y * back_scale;
    synced_data.linear_velocity.z = front_data.linear_velocity.z * front_scale + back_data.linear_velocity.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;

    SyncedData.push_back(synced_data);

这里就是线性插值的具体实现过程，最开始看这个对于front_scale和back_scale的定义有点不明白，然后看到评论区的解释就很清楚了。

设置有两个变量a和b，a的时刻是0，b的时刻是1。现在要在a和b之间进行插值，时间为t。

插值应该是a*(1 - t)/(1 - 0) + b*t/(1 - 0)，t取0到1之间的值。

相当与front_scale就是(1 - t)/1(1 - 0)，back_scale就是t/(1 - 0) ，

back_data.tme - front_data.time就是(1 - 0)，

back_data.time - sync_time = 1 - t，

sync_time - front_data.time = t，

然后a就是front_data，b是back_data。

关于gnss和imu数据的处理都是一样的。

subscriber文件：

新设立了velocity_subscriber.hpp和velocity_subscriber.cpp文件，具体格式和其他的都是一样的，不做具体分析。

其他文件没有很大的变化，最后只有在front_end_flow.cpp里面的ReadData函数变化很大：

bool FrontEndFlow::ReadData() {
    cloud_sub_ptr_->ParseData(cloud_data_buff_);

    static std::deque<IMUData> unsynced_imu_;
    static std::deque<VelocityData> unsynced_velocity_;
    static std::deque<GNSSData> unsynced_gnss_;

    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;

    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);

    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;
    }

    return true;
}

在这部分，我在ParseData这里产生了怀疑，之前的数据队列是怎么处理。仔细的把整段代码看了之后发现了，是利用了构造函数机制，它把订阅模块放在了构造函数里面，每当利用这个生成一个对象，就会自动启动构造函数，然后就会生成订阅到的数据，就可以调用ParseData函数。

具体的在这里：

    cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    imu_sub_ptr_ = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    velocity_sub_ptr_ = std::make_shared<VelocitySubscriber>(nh, "/kitti/oxts/gps/vel", 1000000);
    gnss_sub_ptr_ = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
    lidar_to_imu_ptr_ = std::make_shared<TFListener>(nh, "imu_link", "velo_link");

整个系统设计的很巧妙，值得学习。

但后来还是很疑惑，如果说处理是返回的是false怎么办，就会在Run函数哪里直接返回false，然后在main函数里面，使用的是while(ros::ok())这里应该是说如果数据错误，就直接采用下一个了。

//front_end_node.cpp    
while (ros::ok()) {
        ros::spinOnce();

        _front_end_flow_ptr->Run();

        rate.sleep();
    }

还是对于ros相关订阅、发布的每一秒的数据有所疑问，准备写一个练习，把它搞明白。

四、总结

主要是对于多个传感器时间进行统一，以lidar时间戳为基准，主要改变是索引和插值两个模块。这两个模块的实现全部集成在里每一个传感器的数据格式bool函数里面。最后在front_end_flow.cpp模块里面的ReadData函数进行了调用，流程很清晰。