不同硬件传感器数据之间的时间同步问题

主要分为硬同步和软同步。
硬同步就是用一个硬件触发器，直接通过物理信号，触发相机和LIDAR记录一个data frame
软同步提供一个相同的时间源（一般都是主控电脑utc时间）给相机和LIDAR，相机和LIDAR记录的数据中打上时间戳，因为周期不同，相机和LIDAR之间的数据帧不是完全对应的。

我们在使用ROS的时候，例如激光雷达、小觅相机之类的，在获取其传感器数据并用ros节点以话题发布的时候都会根据电脑的时钟来为数据打上时间戳，以利用不同数据间的时间戳进行数据同步，但是该时间戳不一定准确，尤其是对于没有做过硬件同步的不同数据源来讲，可能会存在延迟存在，我们需要找寻其他方法来消除时间戳之间的时间延迟td。

---

另外一个关于传感器网络中时间同步问题的文章可以带来思考：
知乎-传感器网络中时间同步
什么是无线传感器网络？如上图，无线传感器网络一般由数量众多的无线传感器节点组成，这些节点大多硬件成本低廉且非常适合大量部署。网关设备（又称为基站节点）是无线传感器网络与互联网沟通的接口，通过网关设备可以将无线传感器网络收集的数据传递到互联网服务器中存储分析。

FTSP：The Flooding Time Synchronization Protocol，是在无线传感器网络中被广泛使用的时间同步协议，主要解决了3个问题：

1、 两个设备之间的时间对齐：时间戳同步

2、 两个设备之间晶体振荡器的对齐

3、 同步网络中所有的设备

---

从零开始做自动驾驶定位(六): 传感器时间同步

本文章配套源代码地址：https://github.com/Little-Potato-1990/localization_in_auto_driving

测试数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取码: n9ys

本篇文章对应的代码Tag为 6.0

代码在后续可能会有调整，如和文章有出入，以实际代码为准

========================================

一、概述
只要牵扯到多个传感器，那么时间同步就是一件必做的事情。

时间同步包括两个步骤：

1）首先保证时钟源是一致的
因为时钟源都有钟漂，而且每个时钟源钟漂不同，所以即使把各个传感器时间戳在初始时刻对齐，运行一段时间之后，之前对齐的结果就会偏离。解决这个问题的办法就是在硬件上把时钟源统一，常见的做法是做一个脉冲发生器，所有传感器都被这个脉冲触发，每次触发都校正一次自己的时钟，这样就可以消除时钟源的累计误差。

在自动驾驶的传感器配置里，GNSS是一个必备的传感器，它自带秒脉冲发生器，所以可以直接使用。而且GNSS信号能够达到定位要求时，自身时钟也会受到卫星上原子钟的校正，从而进一步提高精度。大家可能对GNSS的定位功能比较熟悉，其实它的授时功能是和定位同等重要的功能，现在很多系统里都已经改变说法，不再把这类东西只称作定位系统，而是称作定位授时系统。感兴趣的可以自行去搜一些资料看看。

2）获取同一时刻的信息
在解决硬件同步以后，我们只能保证时间差没有累计漂移了，但是各个传感器的采集时刻并不是相同的。比如，在kitti数据集了，雷达和IMU都是10HZ，也就是100ms的周期，但是雷达每次采集的时间要比IMU慢个几十毫秒，当我们想获得在雷达采集时刻的车体角速度和加速度信息时，就要根据雷达前后时刻的IMU信息，通过插值计算出一个等效值，这就是获取同一时刻信息的含义。

由于kitti已经做好了第一步，所以我们本次的工作就是围绕第二步来做的。

二、同步方法
按照刚才所说，同步就是插值。

关于插入时刻问题，因为我们是以雷达作为核心传感器，所以每收到一次雷达数据，就以当前雷达数据采集时刻作为要插入的时间点，从而获得除雷达以外的其他传感器的同一时刻等效信息。

在动手做之前，我们可以拆解一下任务，想插值获取某一时间点的等效信息，就需要索引获得这一时间点前后的两帧数据，有了前后两帧数据的采集时刻，以及要插入的时刻，那么就是一个比例计算的问题了。所以一共被拆解成了两步：索引和插值计算。

1. 程序结构设计
   我们在front_end_flow.cpp文件中的ReadData函数里添加这段功能。

首先从ros的缓冲区里把所有传感器数据取出来，雷达不需要做插值，所以直接放在cloud_data_buff_容器里，其他传感器原始数据放在未做同步的临时容器里，包括IMU信息(unsynced_imu_)、速度信息(unsynced_velocity_)、GNSS信息(unsynced_gnss_)

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

接下来要做的就是做同步了，由于每个传感器都要做一次同步，如果都放在front_end_flow.cpp文件中，会显得繁琐，所以我们把每个传感器的同步功能放在sensor_data文件夹下对应的传感器类文件中，ReadData函数只是调用这个功能函数，这样在ReadData函数下只需要实现三行程序

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

他们分别是IMU信息、速度信息和GNSS信息的时间同步的实现。每个传感器的类都包含一个SyncData函数，在函数内部实现该传感器对应的同步操作，具体讲就是索引和插值操作。

2. 时间索引
   由于数据在容器里都是按照时间先后排列的，每个容器相当于构建了一条时间线。索引要做的就是把雷达采集时刻在其他传感器的时间线里找到对应的位置，位置锁定，前后两帧的数据就自然有了。

在索引时需要考虑一些异常情况，因为传感器难免会有一些丢帧或者时间戳出问题的情况，要避免其对程序功能产生影响

我们以IMU数据的索引为例来介绍具体步骤，其他传感器步骤一致。

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();
        return false;
    }
    
    if (UnsyncedData.at(1).time - sync_time > 0.2) {
        UnsyncedData.pop_front();
        return false;
    }
    break;
}

上面这段就是索引所需要的四个步骤，核心思想是让容器第一个数据时间比插入时刻早，第二个数据时间比插入时刻晚：

1）如果第一个数据时间比雷达时间还要靠后，即插入时刻的前面没有数据，那么就无从插入，直接退出

2）如果第一个数据比插入时刻早，第二个数据也比插入时刻早，那么第一个时刻的数据是没意义的，应该接着往下找，并删除第一个数据

3）如果雷达采集时刻已经处在前两个数据的中间了，但是第一个数据时刻与雷达采集时刻时间差过大，那么中间肯定丢数据了，退出

4）同样，如果第二个数据时刻与雷达采集时刻时间差过大，那么也是丢数据了，也退出

以上四个限制条件如果都通过了，那么就算是找到对应位置了。

3. 数据插值
   线性插值大家都懂，有两个数据a和b，时刻分别是0和1，那么时间t（0<t<1）时刻的插值就是a*(1-t)+b*t。

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

上面是IMU数据插值的例子。其他传感器插值原理一致。

---

ROS中提供的方法：

ROS之订阅多个话题并对其进行同步处理(多传感器融合)
文章目录
1.引言
2.方法一:利用全局变量TimeSynchronizer
3. CMakeLists.txt 和 packages.xml添加ROS包
4.方法二: 利用类成员message_filters::Synchronizer
1.引言
本小节针对在ROS节点中需要订阅两个及两个以上的话题时，需要保持对这两个话题数据的同步，且需要同时接收数据一起处理然后当做参数传入到另一个函数中。
研究背景：realsenseT265 和 realsense D435 都有IMU数据，但是这两个传感器都将imu的数据拆开进行发布了，区分了线加速度和角加速，而在有一些场合我们需要合并使用。

2.方法一:利用全局变量TimeSynchronizer
1 . message_filter ::subscriber 分别订阅不同的输入topic

2 . TimeSynchronizer<Image,CameraInfo> 定义时间同步器;

3 . sync.registerCallback 同步回调

4 . void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数

5 .相关API:http://docs.ros.org/api/message_filters/html/c++/classmessage__filters_1_1TimeSynchronizer.html

	#include <message_filters/subscriber.h>
	#include <message_filters/synchronizer.h>
	#include <message_filters/sync_policies/approximate_time.h>
	#include <boost/thread/thread.hpp>
	
	using namespace message_filters;
	
	void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg_accel, const sensor_msgs::ImuConstPtr &imu_msg_gyro)
	{
		    double t = imu_msg_accel->header.stamp.toSec();
		    double dx = imu_msg_accel->linear_acceleration.x;
		    double dy = imu_msg_accel->linear_acceleration.y;
		    double dz = imu_msg_accel->linear_acceleration.z;
		    double rx = imu_msg_gyro->angular_velocity.x;
		    double ry = imu_msg_gyro->angular_velocity.y;
		    double rz = imu_msg_gyro->angular_velocity.z;
				Vector3d gyr(rx, ry, rz);
			Vector3d acc(dx, dy, dz);
		   /**
		  	处理函数 ......
		   */
	}
	
	int main(int argc, char** argv)
	{
		// 需要用message_filter容器对两个话题的数据发布进行初始化，这里不能指定回调函数
	    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_accel(n,IMU_TOPIC_ACCEL,2000,ros::TransportHints().tcpNoDelay());
	    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_gyro(n,IMU_TOPIC_GYRO,2000,ros::TransportHints().tcpNoDelay());
  		
  		// 将两个话题的数据进行同步
		typedef sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::Imu> syncPolicy;
	  	Synchronizer<syncPolicy> sync(syncPolicy(10), sub_imu_accel, sub_imu_gyro);  
	  	// 指定一个回调函数，就可以实现两个话题数据的同步获取
	  	sync.registerCallback(boost::bind(&imu_callback, _1, _2));
	  	
	  	ros::spin();
	  	return 0;
 		}

3. CMakeLists.txt 和 packages.xml添加ROS包

CMakeLists.txt下添加：
find_package(catkin REQUIRED COMPONENTS
  ....
  image_transport
  ....
)
package.xml下添加：
 <build_depend>image_transport</build_depend>
 <exec_depend>image_transport</exec_depend>

参考连接:http://wiki.ros.org/message_filters

补充:用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式

4.方法二: 利用类成员message_filters::Synchronizer
1 . 头文件

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

2 . 定义消息同步机制和成员变量

typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;
message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ;             // topic1 输入
message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入
message_filters::Synchronizer<slamSyncPolicy>* sync_;

3.类构造函数中开辟空间new

 odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);
 img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);
   
 sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_);
 sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));

4.类成员函数回调处理

void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg)  //回调中包含多个消息
{
    //TODO
    fStampAll<<pOdom->header.stamp<<"    "<<pImg->header.stamp<<endl;
    getOdomData(pOdom);                   //
    is_img_update_ = getImgData(pImg);    // 像素值
    cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
         << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
    fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
          << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
    pixDataToMetricData();
    static bool FINISH_INIT_ODOM_STATIC = false;
    if(FINISH_INIT_ODOM_STATIC)
    {
        ekfslam(robot_odom_);
    }
    else if(is_img_update_)
    {
        if(addInitVectorFull())
        {
            computerCoordinate();
            FINISH_INIT_ODOM_STATIC = true;
        }
    }
}

总结：

解决的问题
多传感器数据融合的时候，由于各个传感器采集数据的频率的不同，例如odom 50Hz、Imu 100Hz、camera 25Hz，需要将传感器数据进行时间同步后才能进行融合。
融合的原理：
分别订阅不同的需要融合的传感器的主题，通过TimeSynchronizer 统一接收多个主题，并产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。
注意
只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。
当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率，一般会很低。实际测试订阅两个需要同步的主题，odom 50Hz、imu 50Hz，而回调函数的频率只有24Hz左右。