SLAM中多传感器融合的时间同步问题

最近一直在做深度相机和IMU的数据融合，以期得到更好的位姿。但如果要用多传感器融合，由于传感器频率的差异，必然会遇到时间同步的问题。幸运的是，ROS提供给我们一个时间同步的API以学习，以下就是利用ROS机制实现的时间同步。话不多说，直接上代码。

#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
//#include <message_filters/sync_policies/exact_time.h>
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <rosbag/bag.h>
#include "ctime"
#include