├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── config ├── event_stereo_visualizer.yaml ├── event_visualizer.yaml ├── message_topic.yaml └── raw_message_topic.yaml ├── include └── utility.hpp ├── launch ├── bag2rawfile.launch ├── bag_merger.launch ├── bag_splitter.launch ├── data_validation.launch ├── event_stereo_visualization.launch ├── event_visualization.launch └── timeline_reconstruction.launch ├── msg ├── Event.msg └── EventArray.msg ├── package.xml ├── script ├── bag2hdf5.py └── csv2tum.py └── src ├── bag2rawfile.cpp ├── bag_merger.cpp ├── bag_splitter.cpp ├── data_validator.cpp ├── event_visualizer.cpp └── timeline_reconstructor.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mpl_dataset_toolbox) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cv_bridge 8 | geometry_msgs 9 | message_generation 10 | # prophesee_event_msgs 11 | pcl_conversions 12 | pcl_ros 13 | rosbag 14 | roscpp 15 | rospy 16 | sensor_msgs 17 | std_msgs 18 | ) 19 | 20 | find_package(OpenCV REQUIRED) 21 | find_package(PCL REQUIRED) 22 | 23 | add_message_files( 24 | FILES 25 | Event.msg 26 | EventArray.msg 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs 32 | ) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | DEPENDS PCL 37 | ) 38 | 39 | include_directories( 40 | include 41 | ${catkin_INCLUDE_DIRS} 42 | ${OpenCV_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ) 45 | 46 | link_directories( 47 | include 48 | ${OpenCV_LIBRARY_DIRS} 49 | ${PCL_LIBRARY_DIRS} 50 | ) 51 | 52 | add_executable(bag2rawfile src/bag2rawfile.cpp) 53 | add_executable(bag_merger src/bag_merger.cpp) 54 | add_executable(bag_splitter src/bag_splitter.cpp) 55 | add_executable(data_validator src/data_validator.cpp) 56 | add_executable(event_visualizer src/event_visualizer.cpp) 57 | add_executable(timeline_reconstructor src/timeline_reconstructor.cpp) 58 | 59 | target_link_libraries(bag2rawfile ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 60 | target_link_libraries(bag_merger ${catkin_LIBRARIES}) 61 | target_link_libraries(bag_splitter ${catkin_LIBRARIES}) 62 | target_link_libraries(data_validator ${catkin_LIBRARIES}) 63 | target_link_libraries(event_visualizer ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 64 | target_link_libraries(timeline_reconstructor ${catkin_LIBRARIES}) 65 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | The Clear BSD License 2 | 3 | Copyright (c) 2022 Ling Gao 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted (subject to the limitations in the disclaimer 8 | below) provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from this 19 | software without specific prior written permission. 20 | 21 | NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY 22 | THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 23 | CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 25 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 26 | CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 27 | EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 28 | PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 29 | BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 30 | IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPL Dataset Toolbox 2 | 3 | [VECtor Benchmark](https://star-datasets.github.io/vector/) is the first complete set of benchmark datasets captured with a multi-sensor setup containing an event-based stereo camera, a regular stereo camera, multiple depth sensors, and an inertial measurement unit. The setup is fully hardware-synchronized and underwent accurate extrinsic calibration. All sequences come with ground truth data captured by highly accurate external reference devices such as a motion capture system. Individual sequences include both small and large-scale environments, and cover the specific challenges targeted by dynamic vision sensors. 4 | 5 | This toolbox is a ROS workspace integrating with a set of easy-to-use dataset functions, including: 6 | 7 | - [Event Visualizer](https://github.com/mgaoling/mpl_dataset_toolbox#event-visualizer): convert raw event stream into accumulated event frames. 8 | - [Data Validator](https://github.com/mgaoling/mpl_dataset_toolbox#data-validator): validate the downloaded rosbag or the merged rosbag for its completeness. 9 | - [Bag Merger](https://github.com/mgaoling/mpl_dataset_toolbox#bag-merger): merge multiple, single-topic rosbags chronologically into one complete rosbag. 10 | - [Bag Splitter](https://github.com/mgaoling/mpl_dataset_toolbox#bag-splitter): split one multi-topic rosbag into a few compressed, single-topic rosbags. 11 | - [Timeline Reconstructor](https://github.com/mgaoling/mpl_dataset_toolbox#tool-kit): reconstruct raw recording's timeline for each data channel. 12 | - [Bag-to-HDF5 Converter](https://github.com/mgaoling/mpl_dataset_toolbox#tool-kit): convert event stream(s) into compressed HDF5 file(s) under [DSEC format](https://dsec.ifi.uzh.ch/data-format/). 13 | - [Bag-to-Rawfile Converter](https://github.com/mgaoling/mpl_dataset_toolbox#tool-kit): convert all other data stream(s) into raw file(s). 14 | - [CSV-to-TUM Converter](https://github.com/mgaoling/mpl_dataset_toolbox#tool-kit): convert OptiTrack readings into text file under [TUM format](https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats). 15 | 16 | # License and Citation 17 | 18 | This toolbox, together with the [MPL Calibration Toolbox](https://github.com/mgaoling/mpl_calibration_toolbox) and the [k4a Projector](https://github.com/greatoyster/k4a_projector), is available as open-source under the terms of the [BSD-3-Clause-Clear License](https://github.com/mgaoling/mpl_dataset_toolbox/blob/main/LICENSE.txt). If you use this toolbox in an academic context, please cite the [publication](https://star-datasets.github.io/vector/assets/pdf/VECtor.pdf) as follows: 19 | 20 | ```bibtex 21 | @Article{gao2022vector, 22 | author = {Gao, Ling and Liang, Yuxuan and Yang, Jiaqi and Wu, Shaoxun and Wang, Chenyu and Chen, Jiaben and Kneip, Laurent}, 23 | title = {{VECtor}: A Versatile Event-Centric Benchmark for Multi-Sensor SLAM}, 24 | journal = {IEEE Robotics and Automation Letters}, 25 | pages = {8217--8224}, 26 | volume = {7}, 27 | number = {3}, 28 | year = {2022}, 29 | doi = {10.1109/LRA.2022.3186770} 30 | } 31 | ``` 32 | 33 | # Getting Started 34 | 35 | The following instructions are tested on (A) [Ubuntu 18.04](https://ubuntu.com/download/desktop) with [ROS Melodic](http://wiki.ros.org/ROS/Installation), and (B) [Ubuntu 20.04](https://ubuntu.com/download/desktop) with [ROS Noetic](http://wiki.ros.org/ROS/Installation). A ROS **desktop-full installation** is required. On top of that, the following libraries ([Eigen 3](https://eigen.tuxfamily.org/index.php?title=Main_Page), [OpenCV](https://opencv.org/releases/), [Point Cloud Library](https://pointclouds.org/downloads/)) have to be installed: 36 | 37 | ``` 38 | sudo apt-get update 39 | sudo apt-get install libeigen3-dev libopencv-dev libpcl-dev 40 | ln -s /usr/include/eigen3/Eigen /usr/include/Eigen 41 | ``` 42 | 43 | After that, enter your catkin workspace and the build can be triggered with the following command: 44 | 45 | ``` 46 | cd ~/catkin_ws/src 47 | git clone https://github.com/mgaoling/mpl_dataset_toolbox.git 48 | cd .. 49 | catkin_make 50 | source ~/catkin_ws/devel/setup.bash 51 | ``` 52 | 53 | # Event Visualizer 54 | 55 | The event visualizer converts raw event stream into accumulated event frames. It can handle at most two different event streams. 56 | 57 | - For monocular setup, check and modify the parameters in the `config/event_visualizer.yaml`, then launch the visualization by: 58 | 59 | ``` 60 | roslaunch mpl_dataset_toolbox event_visualization.launch 61 | ``` 62 | 63 | - For stereo setup, check and modify the parameters in the `config/event_stereo_visualizer.yaml`, then launch the visualization by: 64 | 65 | ``` 66 | roslaunch mpl_dataset_toolbox event_stereo_visualization.launch 67 | ``` 68 | 69 | - To visualize the rosbag, run `rqt` or `rqt_image_view` in another terminal to display the accumulated event frames. 70 | 71 | # Data Validator 72 | 73 | The data validator helps to check the correctness and completeness of a downloaded, single-topic rosbag or a merged rosbag. It also calculates the Mean Event Rate if the event stream exists. 74 | 75 | - Launch the validation in one terminal by: 76 | 77 | ``` 78 | roslaunch mpl_dataset_toolbox data_validation.launch 79 | ``` 80 | 81 | - Play the rosbag in another terminal by: 82 | 83 | ``` 84 | rosbag play [data.bag] 85 | ``` 86 | 87 | - Terminate the validation once the playback is over. 88 | 89 | **Note:** Dropped frames on Kinect stream are to be expected, owing to the singularity of the depth calculation during recording. We have deliberately left a blank on the depth stream during the timeline reconstruction, to maintain the overall correctness. 90 | 91 | # Bag Merger 92 | 93 | Bag merger helps to merge different single-topic rosbags chronologically. Note that data sequences with different prefixes can be safely stored under the same directory. **DO NOT MODIFY THE FILENAME!** 94 | 95 | - Put all single-topic rosbags under one folder, then launch the bag merger by: 96 | 97 | ``` 98 | roslaunch mpl_dataset_toolbox bag_merger.launch directory_path:=[path_to_directory] 99 | ``` 100 | 101 | **Note:** Here, only an absolute path without `~`, or a relative path that is relative to this toolbox is allowed for the directory path. 102 | 103 | # Bag Splitter 104 | 105 | Bag Splitter helps to split a multi-topic rosbag into a few single-topic rosbags, with a compression option. 106 | 107 | - Launch the bag splitter by: 108 | 109 | ``` 110 | roslaunch mpl_dataset_toolbox bag_splitter.launch bag_path:=[data.bag] need_compression:=[true_or_false] 111 | ``` 112 | 113 | **Note:** Here, only an absolute path without `~`, or a relative path that is relative to this toolbox is allowed for the rosbag path. 114 | 115 | # Tool Kit 116 | 117 | ### Timeline Reconstructor 118 | 119 | ``` 120 | roslaunch mpl_dataset_toolbox timeline_reconstruction.launch bag_path:=[data.bag] gt_path:=[ground_truth.csv] 121 | ``` 122 | 123 | ### Bag-to-HDF5 Converter 124 | 125 | ``` 126 | roscd mpl_dataset_toolbox/script 127 | python3 bag2hdf5.py [data.bag] 128 | ``` 129 | 130 | ### Bag-to-Rawfile Converter 131 | 132 | ``` 133 | roslaunch mpl_dataset_toolbox bag2rawfile.launch bag_path:=[data.bag] 134 | ``` 135 | 136 | ### CSV-to-TUM Converter 137 | 138 | ``` 139 | roscd mpl_dataset_toolbox/script 140 | python3 csv2tum.py [ground_truth.csv] [data.bag] 141 | ``` 142 | -------------------------------------------------------------------------------- /config/event_stereo_visualizer.yaml: -------------------------------------------------------------------------------- 1 | in_event_stream_topic: [ 2 | "/prophesee/left/events", 3 | "/prophesee/right/events" 4 | ] 5 | out_event_frame_topic: [ 6 | "/prophesee/left/event_frame", # <-- follow the same order as above 7 | "/prophesee/right/event_frame" 8 | ] 9 | out_frequency: 30 # [Hz] 10 | -------------------------------------------------------------------------------- /config/event_visualizer.yaml: -------------------------------------------------------------------------------- 1 | in_event_stream_topic: [ 2 | "/prophesee/camera/cd_events_buffer" 3 | ] 4 | out_event_frame_topic: [ 5 | "/prophesee/event_frame" 6 | ] 7 | out_frequency: 30 # [Hz] 8 | -------------------------------------------------------------------------------- /config/message_topic.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/imu/data" 2 | event_left_topic: "/prophesee/left/events" 3 | event_right_topic: "/prophesee/right/events" 4 | camera_left_topic: "/camera/left/image_mono" 5 | camera_right_topic: "/camera/right/image_mono" 6 | kinect_color_topic: "/kinect/color_image" 7 | kinect_depth_topic: "/kinect/depth_image" 8 | lidar_topic: "/ouster/point_cloud" 9 | ground_truth_topic: "/gt/pose" 10 | event_left_depth_topic: "/prophesee/left/depth_image_undistort" 11 | event_right_depth_topic: "/prophesee/right/depth_image_undistort" 12 | camera_left_depth_topic: "/camera/left/depth_image_undistort" 13 | camera_right_depth_topic: "/camera/right/depth_image_undistort" 14 | camera_left_undistort_topic: "/camera/left/image_mono_undistort" 15 | camera_right_undistort_topic: "/camera/right/image_mono_undistort" 16 | -------------------------------------------------------------------------------- /config/raw_message_topic.yaml: -------------------------------------------------------------------------------- 1 | in_imu_topic: "/imu/data" 2 | in_event_left_topic: "/prophesee/camera_left/cd_events_buffer_reconstructed" 3 | in_event_right_topic: "/prophesee/camera_right/cd_events_buffer_reconstructed" 4 | in_camera_left_topic: "/stereo/left/image_mono" 5 | in_camera_right_topic: "/stereo/right/image_mono" 6 | in_kinect_color_topic: "/rgb/image_raw" 7 | in_kinect_depth_topic: "/depth/image_raw" 8 | in_lidar_topic: "/os_cloud_node/points" -------------------------------------------------------------------------------- /include/utility.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DATASET_TOOLBOX_UTILITY_HPP_ 2 | #define DATASET_TOOLBOX_UTILITY_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | // #include 9 | // #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define IMU_PERIOD 0.005000000 // 200Hz 18 | #define CAM_PERIOD 0.033333333 // 30Hz 19 | #define KINECT_PERIOD 0.033333333 // 30Hz 20 | #define LIDAR_PERIOD 0.100000000 // 10Hz 21 | #define GT_PERIOD 0.008333333 // 120Hz (small-scale) or 10Hz (large-scale) 22 | 23 | using IMU = sensor_msgs::Imu; 24 | using Event = mpl_dataset_toolbox::Event; 25 | using EventArray = mpl_dataset_toolbox::EventArray; 26 | // using Event = prophesee_event_msgs::Event; 27 | // using EventArray = prophesee_event_msgs::EventArray; 28 | using Image = sensor_msgs::Image; 29 | using PointCloud = sensor_msgs::PointCloud2; 30 | using PoseStamped = geometry_msgs::PoseStamped; 31 | 32 | // ------------------------------------------------------------------------- // 33 | 34 | namespace colorful_char { 35 | 36 | std::string info(std::string input_str) { 37 | return "\033[1;32m>> " + input_str + " \033[0m"; 38 | } 39 | 40 | std::string warning(std::string input_str) { 41 | return "\033[1;35m>> WARNING: " + input_str + " \033[0m"; 42 | } 43 | 44 | std::string error(std::string input_str) { 45 | return "\033[1;31m>> ERROR: " + input_str + " \033[0m"; 46 | } 47 | 48 | } // namespace colorful_char 49 | 50 | // ------------------------------------------------------------------------- // 51 | 52 | namespace fs = boost::filesystem; 53 | 54 | // Check whether the directory/file path is absolute path or relative path, as well as its validness. 55 | bool directory_path_check(std::string & path) { 56 | if (path.back() != '/') path += '/'; 57 | if (!fs::exists(path)) { 58 | if (path.front() != '/') path = '/' + path; 59 | path = ros::package::getPath("mpl_dataset_toolbox") + path; 60 | } 61 | if (!fs::exists(path)) { 62 | ROS_ERROR("%s", colorful_char::error("Invalid directory path: " + path).c_str()); 63 | return false; 64 | } 65 | return true; 66 | } 67 | 68 | bool file_path_check(std::string & path) { 69 | if (!fs::exists(path)) { 70 | if (path.front() != '/') path = '/' + path; 71 | path = ros::package::getPath("mpl_dataset_toolbox") + path; 72 | } 73 | if (!fs::exists(path)) { 74 | ROS_ERROR("%s", colorful_char::error("Invalid file path: " + path).c_str()); 75 | return false; 76 | } 77 | return true; 78 | } 79 | 80 | #endif // DATASET_TOOLBOX_UTILITY_HPP_ 81 | -------------------------------------------------------------------------------- /launch/bag2rawfile.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/bag_merger.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/bag_splitter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/data_validation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/event_stereo_visualization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/event_visualization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/timeline_reconstruction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /msg/Event.msg: -------------------------------------------------------------------------------- 1 | # This message is copied from Prophesee ROS Wrapper. 2 | # A single event message 3 | uint16 x 4 | uint16 y 5 | time ts 6 | bool polarity 7 | -------------------------------------------------------------------------------- /msg/EventArray.msg: -------------------------------------------------------------------------------- 1 | # This message is copied from Prophesee ROS Wrapper. 2 | # This message contains an array of events 3 | # (0, 0) is at top-left corner of image 4 | # 5 | 6 | Header header 7 | 8 | uint32 height # image height, that is, number of rows 9 | uint32 width # image width, that is, number of columns 10 | 11 | # an array of events 12 | Event[] events 13 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpl_dataset_toolbox 4 | 2.1.0 5 | The MPL Dataset Toolbox 6 | Ling Gao 7 | BSD-3-Clause-Clear 8 | https://star-datasets.github.io/vector/download/ 9 | 10 | catkin 11 | cv_bridge 12 | geometry_msgs 13 | message_generation 14 | 15 | opencv 16 | pcl_conversions 17 | pcl_ros 18 | rosbag 19 | roscpp 20 | rospy 21 | sensor_msgs 22 | std_msgs 23 | 24 | -------------------------------------------------------------------------------- /script/bag2hdf5.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import h5py 3 | import hdf5plugin 4 | import progressbar 5 | from prophesee_event_msgs.msg import EventArray 6 | import rosbag 7 | import numpy as np 8 | 9 | if __name__ == "__main__": 10 | parser = argparse.ArgumentParser(description='Convert ROS bag to hdf5 for event stream') 11 | parser.add_argument('topic', type=str, help='ROS topic for event stream') 12 | parser.add_argument('input_path', type=str, help='Input ROS bag path') 13 | parser.add_argument('output_path', type=str, help='Output HDF5 file path') 14 | args = parser.parse_args() 15 | 16 | event_topic = args.topic 17 | input_data_path = args.input_path 18 | bag = rosbag.Bag(input_data_path, 'r') 19 | output_data_path = args.output_path 20 | event_file = h5py.File(output_data_path, 'w') 21 | 22 | event_curr_idx = 0 23 | event_ms_idx = 0 24 | chunk_size = 100000 25 | receive_event = False 26 | event_x = [] 27 | event_y = [] 28 | event_p = [] 29 | event_t = [] 30 | event_ms_to_idx = [] 31 | bar = progressbar.ProgressBar(maxval=bag.size).start() 32 | for topic, msg, t in bag.read_messages(event_topic): 33 | bar.update(bag._file.tell()) 34 | if (topic == event_topic): 35 | # Add this event into a temporary numpy holder 36 | if not receive_event: 37 | receive_event = True 38 | event_t_offset = msg.events[0].ts 39 | for event in msg.events: 40 | event_x.append(event.x) 41 | event_y.append(event.y) 42 | event_p.append(event.polarity) 43 | event_t.append(int((event.ts.to_nsec() - event_t_offset.to_nsec()) / 1e3)) 44 | while (event_t[-1] >= 1000 * event_ms_idx): 45 | event_ms_to_idx.append(event_curr_idx) 46 | event_ms_idx += 1 47 | event_curr_idx += 1 48 | # Flush the holder temporarily 49 | if (len(event_x) >= chunk_size): 50 | if ('events/t' not in event_file): 51 | event_file.create_dataset('events/x', dtype='u2', maxshape=(None, ), data=np.array(event_x, dtype='u2'), **hdf5plugin.Zstd()) 52 | event_file.create_dataset('events/y', dtype='u2', maxshape=(None, ), data=np.array(event_y, dtype='u2'), **hdf5plugin.Zstd()) 53 | event_file.create_dataset('events/p', dtype='u1', maxshape=(None, ), data=np.array(event_p, dtype='u1'), **hdf5plugin.Zstd()) 54 | event_file.create_dataset('events/t', dtype='u4', maxshape=(None, ), data=np.array(event_t, dtype='u4'), **hdf5plugin.Zstd()) 55 | else: 56 | old_size = event_file['events/t'].shape[0] 57 | new_size = len(event_t) + old_size 58 | event_file['events/x'].resize((new_size, )) 59 | event_file['events/x'][old_size:] = data=np.array(event_x, dtype='u2') 60 | event_file['events/y'].resize((new_size, )) 61 | event_file['events/y'][old_size:] = data=np.array(event_y, dtype='u2') 62 | event_file['events/p'].resize((new_size, )) 63 | event_file['events/p'][old_size:] = data=np.array(event_p, dtype='u1') 64 | event_file['events/t'].resize((new_size, )) 65 | event_file['events/t'][old_size:] = data=np.array(event_t, dtype='u4') 66 | event_x.clear() 67 | event_y.clear() 68 | event_p.clear() 69 | event_t.clear() 70 | # Flush the remaining data on holder 71 | old_size = event_file['events/t'].shape[0] 72 | new_size = len(event_t) + old_size 73 | event_file['events/x'].resize((new_size, )) 74 | event_file['events/x'][old_size:] = data=np.array(event_x, dtype='u2') 75 | event_file['events/y'].resize((new_size, )) 76 | event_file['events/y'][old_size:] = data=np.array(event_y, dtype='u2') 77 | event_file['events/p'].resize((new_size, )) 78 | event_file['events/p'][old_size:] = data=np.array(event_p, dtype='u1') 79 | event_file['events/t'].resize((new_size, )) 80 | event_file['events/t'][old_size:] = data=np.array(event_t, dtype='u4') 81 | event_file.create_dataset('ms_to_idx', dtype='u8', data=np.array(event_ms_to_idx, dtype='u8'), **hdf5plugin.Zstd()) 82 | event_file.create_dataset('t_offset', shape = (1,), dtype='i8', data=int(event_t_offset.to_nsec() / 1e3), **hdf5plugin.Zstd()) 83 | bar.finish() 84 | event_file.close() 85 | -------------------------------------------------------------------------------- /script/csv2tum.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import numpy as np 3 | import rosbag 4 | 5 | if __name__ == "__main__": 6 | parser = argparse.ArgumentParser(description='Convert OptiTrack raw csv file to txt in tum format') 7 | parser.add_argument('csv_path', type=str, help='csv file path') 8 | parser.add_argument('bag_path', type=str, help='ROS bag path') 9 | args = parser.parse_args() 10 | 11 | csv_path = args.csv_path 12 | bag_path = args.bag_path 13 | 14 | tum_path = csv_path[:-3] + "tum" 15 | bag_name = csv_path[:-4].split("/")[-1] 16 | bag = rosbag.Bag(bag_path, "r") 17 | ts_offset = bag.get_start_time() 18 | 19 | with open(csv_path, encoding = "utf-8") as gt: 20 | data = np.loadtxt(gt, delimiter=',', skiprows=7, usecols=[0,6,7,8,2,3,4,5]) 21 | data[:,0] = data[:,0] / 120 + ts_offset 22 | # data[:,4:8] = data[:,4:8]jnp.linalg.norm(data[:,4:8], axis=1)[:,np.newaxis] 23 | 24 | np.savetxt(tum_path, data, fmt = "%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f", 25 | header = "ground truth for " + bag_name + "\ntimestamp tx ty tz qx qy qz qw") 26 | -------------------------------------------------------------------------------- /src/bag2rawfile.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // ------------------------------------------------------------------------- // 11 | 12 | int main(int argc, char ** argv) { 13 | ros::init(argc, argv, "bag2rawfile"); 14 | ros::NodeHandle nh; 15 | 16 | bool receive_imu = false; 17 | bool receive_camera_left = false; 18 | bool receive_camera_right = false; 19 | bool receive_kinect_depth = false; 20 | bool receive_lidar = false; 21 | bool receive_event_left_depth = false; 22 | bool receive_event_right_depth = false; 23 | bool receive_camera_left_depth = false; 24 | bool receive_camera_right_depth = false; 25 | bool receive_camera_left_undistort = false; 26 | bool receive_camera_right_undistort = false; 27 | 28 | std::string bag_in_path, imu_topic, camera_left_topic, camera_right_topic, kinect_depth_topic, lidar_topic, event_left_depth_topic, 29 | event_right_depth_topic, camera_left_depth_topic, camera_right_depth_topic, camera_left_undistort_topic, camera_right_undistort_topic; 30 | ros::param::get("/bag_path", bag_in_path); 31 | ros::param::get("/imu_topic", imu_topic); 32 | ros::param::get("/camera_left_topic", camera_left_topic); 33 | ros::param::get("/camera_right_topic", camera_right_topic); 34 | ros::param::get("/kinect_depth_topic", kinect_depth_topic); 35 | ros::param::get("/lidar_topic", lidar_topic); 36 | ros::param::get("/event_left_depth_topic", event_left_depth_topic); 37 | ros::param::get("/event_right_depth_topic", event_right_depth_topic); 38 | ros::param::get("/camera_left_depth_topic", camera_left_depth_topic); 39 | ros::param::get("/camera_right_depth_topic", camera_right_depth_topic); 40 | ros::param::get("/camera_left_undistort_topic", camera_left_undistort_topic); 41 | ros::param::get("/camera_right_undistort_topic", camera_right_undistort_topic); 42 | if (!file_path_check(bag_in_path)) { 43 | ros::shutdown(); 44 | return -1; 45 | } 46 | rosbag::Bag bag_in; 47 | bag_in.open(bag_in_path, rosbag::bagmode::Read); 48 | std::ofstream imu_txt, camera_left_txt, camera_right_txt, camera_left_undistort_txt, camera_right_undistort_txt; 49 | 50 | std::string folder_path, bag_name, out_path; 51 | folder_path = bag_in_path.substr(0, bag_in_path.find_last_of("/\\") + 1); 52 | bag_name = bag_in_path.substr(bag_in_path.find_last_of("/\\") + 1, bag_in_path.size() - bag_in_path.find_last_of("/\\") - 5); 53 | out_path = folder_path + "raw_data/"; 54 | fs::create_directory(out_path); 55 | 56 | uint32_t msg_idx = 0; 57 | uint32_t msg_size = rosbag::View(bag_in).size(); 58 | for (rosbag::MessageInstance const msg : rosbag::View(bag_in)) { 59 | if (msg.getTopic() == imu_topic) { 60 | IMU::Ptr imu_msg = msg.instantiate(); 61 | if (!receive_imu) { 62 | receive_imu = true; 63 | imu_txt = std::ofstream(out_path + bag_name + ".imu.txt", std::ofstream::out); 64 | imu_txt << "# IMU data for " << bag_name << std::endl 65 | << "# Readings from gyroscope[rad/s], accelerometer[m/s^2], and magnetometer[quaternion]" << std::endl 66 | << "# timestamp gx gy gz ax ay az qx qy qz qw" << std::endl; 67 | ROS_INFO("%s", colorful_char::info("Receives data from the IMU!").c_str()); 68 | } 69 | imu_txt << std::to_string(imu_msg->header.stamp.toSec()) << " " << imu_msg->angular_velocity.x << " " << imu_msg->angular_velocity.y 70 | << " " << imu_msg->angular_velocity.z << " " << imu_msg->linear_acceleration.x << " " << imu_msg->linear_acceleration.y << " " 71 | << imu_msg->linear_acceleration.z << " " << imu_msg->orientation.x << " " << imu_msg->orientation.y << " " 72 | << imu_msg->orientation.z << " " << imu_msg->orientation.w << std::endl; 73 | } 74 | 75 | else if (msg.getTopic() == camera_left_topic) { 76 | Image::Ptr img_msg = msg.instantiate(); 77 | if (!receive_camera_left) { 78 | receive_camera_left = true; 79 | fs::create_directory(out_path + bag_name + ".left_camera/"); 80 | camera_left_txt = std::ofstream(out_path + bag_name + ".left_camera/timestamp.txt", std::ofstream::out); 81 | camera_left_txt << "# Left Regular Camera's timestamp data for " << bag_name << std::endl 82 | << "# start and end of the exposure time[s] for each image" << std::endl; 83 | ROS_INFO("%s", colorful_char::info("Receives data from the Left Regular Camera!").c_str()); 84 | } 85 | camera_left_txt << std::to_string(img_msg->header.stamp.toSec()) << " " << std::to_string(img_msg->header.stamp.toSec() + 0.010) 86 | << std::endl; 87 | cv::imwrite(out_path + bag_name + ".left_camera/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 88 | cv_bridge::toCvCopy(img_msg)->image); 89 | } 90 | 91 | else if (msg.getTopic() == camera_right_topic) { 92 | Image::Ptr img_msg = msg.instantiate(); 93 | if (!receive_camera_right) { 94 | receive_camera_right = true; 95 | fs::create_directory(out_path + bag_name + ".right_camera/"); 96 | camera_right_txt = std::ofstream(out_path + bag_name + ".right_camera/timestamp.txt", std::ofstream::out); 97 | camera_right_txt << "# Right Regular Camera's timestamp data for " << bag_name << std::endl 98 | << "# start and end of the exposure time[s] for each image" << std::endl; 99 | ROS_INFO("%s", colorful_char::info("Receives data from the Right Regular Camera!").c_str()); 100 | } 101 | camera_right_txt << std::to_string(img_msg->header.stamp.toSec()) << " " << std::to_string(img_msg->header.stamp.toSec() + 0.010) 102 | << std::endl; 103 | cv::imwrite(out_path + bag_name + ".right_camera/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 104 | cv_bridge::toCvCopy(img_msg)->image); 105 | } 106 | 107 | else if (msg.getTopic() == kinect_depth_topic) { 108 | Image::Ptr img_msg = msg.instantiate(); 109 | if (!receive_kinect_depth) { 110 | receive_kinect_depth = true; 111 | fs::create_directory(out_path + bag_name + ".kinect_depth/"); 112 | ROS_INFO("%s", colorful_char::info("Receives data from the Kinect Depth Camera!").c_str()); 113 | } 114 | cv::imwrite(out_path + bag_name + ".kinect_depth/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 115 | cv_bridge::toCvCopy(img_msg)->image); 116 | } 117 | 118 | else if (msg.getTopic() == lidar_topic) { 119 | PointCloud::Ptr cloud_msg = msg.instantiate(); 120 | if (!receive_lidar) { 121 | receive_lidar = true; 122 | fs::create_directory(out_path + bag_name + ".lidar/"); 123 | ROS_INFO("%s", colorful_char::info("Receives data from the LiDAR!").c_str()); 124 | } 125 | pcl::io::savePCDFile(out_path + bag_name + ".lidar/" + std::to_string(cloud_msg->header.stamp.toSec()) + ".pcd", *cloud_msg, 126 | Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), true); 127 | } 128 | 129 | else if (msg.getTopic() == event_left_depth_topic) { 130 | Image::Ptr img_msg = msg.instantiate(); 131 | if (!receive_event_left_depth) { 132 | receive_event_left_depth = true; 133 | fs::create_directory(out_path + bag_name + ".left_event_depth/"); 134 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Event Camera!").c_str()); 135 | } 136 | cv::imwrite(out_path + bag_name + ".left_event_depth/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 137 | cv_bridge::toCvCopy(img_msg)->image); 138 | } 139 | 140 | else if (msg.getTopic() == event_right_depth_topic) { 141 | Image::Ptr img_msg = msg.instantiate(); 142 | if (!receive_event_right_depth) { 143 | receive_event_right_depth = true; 144 | fs::create_directory(out_path + bag_name + ".right_event_depth/"); 145 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Event Camera!").c_str()); 146 | } 147 | cv::imwrite(out_path + bag_name + ".right_event_depth/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 148 | cv_bridge::toCvCopy(img_msg)->image); 149 | } 150 | 151 | else if (msg.getTopic() == camera_left_depth_topic) { 152 | Image::Ptr img_msg = msg.instantiate(); 153 | if (!receive_camera_left_depth) { 154 | receive_camera_left_depth = true; 155 | fs::create_directory(out_path + bag_name + ".left_camera_depth/"); 156 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Regular Camera!").c_str()); 157 | } 158 | cv::imwrite(out_path + bag_name + ".left_camera_depth/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 159 | cv_bridge::toCvCopy(img_msg)->image); 160 | } 161 | 162 | else if (msg.getTopic() == camera_right_depth_topic) { 163 | Image::Ptr img_msg = msg.instantiate(); 164 | if (!receive_camera_right_depth) { 165 | receive_camera_right_depth = true; 166 | fs::create_directory(out_path + bag_name + ".right_camera_depth/"); 167 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Regular Camera!").c_str()); 168 | } 169 | cv::imwrite(out_path + bag_name + ".right_camera_depth/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 170 | cv_bridge::toCvCopy(img_msg)->image); 171 | } 172 | 173 | else if (msg.getTopic() == camera_left_undistort_topic) { 174 | Image::Ptr img_msg = msg.instantiate(); 175 | if (!receive_camera_left_undistort) { 176 | receive_camera_left_undistort = true; 177 | fs::create_directory(out_path + bag_name + ".left_camera_undistort/"); 178 | camera_left_undistort_txt = std::ofstream(out_path + bag_name + ".left_camera_undistort/timestamp.txt", std::ofstream::out); 179 | camera_left_undistort_txt << "# Left Regular Camera's timestamp data for " << bag_name << std::endl 180 | << "# start and end of the exposure time[s] for each undistorted image" << std::endl; 181 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Left Regular Camera!").c_str()); 182 | } 183 | camera_left_undistort_txt << std::to_string(img_msg->header.stamp.toSec()) << " " 184 | << std::to_string(img_msg->header.stamp.toSec() + 0.010) << std::endl; 185 | cv::imwrite(out_path + bag_name + ".left_camera_undistort/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 186 | cv_bridge::toCvCopy(img_msg)->image); 187 | } 188 | 189 | else if (msg.getTopic() == camera_right_undistort_topic) { 190 | Image::Ptr img_msg = msg.instantiate(); 191 | if (!receive_camera_right_undistort) { 192 | receive_camera_right_undistort = true; 193 | fs::create_directory(out_path + bag_name + ".right_camera_undistort/"); 194 | camera_right_undistort_txt = std::ofstream(out_path + bag_name + ".right_camera_undistort/timestamp.txt", std::ofstream::out); 195 | camera_right_undistort_txt << "# Right Regular Camera's timestamp data for " << bag_name << std::endl 196 | << "# start and end of the exposure time[s] for each undistorted image" << std::endl; 197 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Right Regular Camera!").c_str()); 198 | } 199 | camera_right_undistort_txt << std::to_string(img_msg->header.stamp.toSec()) << " " 200 | << std::to_string(img_msg->header.stamp.toSec() + 0.010) << std::endl; 201 | cv::imwrite(out_path + bag_name + ".right_camera_undistort/" + std::to_string(img_msg->header.stamp.toSec()) + ".png", 202 | cv_bridge::toCvCopy(img_msg)->image); 203 | } 204 | 205 | msg_idx++; 206 | if (msg_idx % 10000 == 0) ROS_INFO_STREAM(msg_idx << "/" << msg_size << " messages have been processed!"); 207 | } 208 | ROS_INFO("%s", colorful_char::info("All messages have been processed!").c_str()); 209 | 210 | if (receive_imu) imu_txt.close(); 211 | if (receive_camera_left) camera_left_txt.close(); 212 | if (receive_camera_right) camera_right_txt.close(); 213 | if (receive_camera_left_undistort) camera_left_undistort_txt.close(); 214 | if (receive_camera_right_undistort) camera_right_undistort_txt.close(); 215 | 216 | ros::shutdown(); 217 | return 0; 218 | } 219 | -------------------------------------------------------------------------------- /src/bag_merger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // ------------------------------------------------------------------------- // 6 | 7 | int BagNameParser(const std::string & file_name, std::string & prefix) { 8 | bool last_dot = false; 9 | int last_dot_idx = -1; 10 | for (int idx = file_name.size() - 1; idx >= 0; --idx) { 11 | if (!last_dot && file_name[idx] == '.') { 12 | last_dot = true; 13 | last_dot_idx = idx; 14 | } else if (last_dot && file_name[idx] == '.') { 15 | prefix = file_name.substr(0, idx); 16 | std::string topic_name = file_name.substr(idx + 1, last_dot_idx - idx - 1); 17 | if (topic_name == "imu") return 1; 18 | else if (topic_name == "left_event") 19 | return 2; 20 | else if (topic_name == "right_event") 21 | return 3; 22 | else if (topic_name == "left_camera") 23 | return 4; 24 | else if (topic_name == "right_camera") 25 | return 5; 26 | else if (topic_name == "kinect_color") 27 | return 6; 28 | else if (topic_name == "kinect_depth") 29 | return 7; 30 | else if (topic_name == "lidar") 31 | return 8; 32 | else if (topic_name == "gt") 33 | return 9; 34 | else if (topic_name == "depth_reprojection") 35 | return 10; 36 | else if (topic_name == "left_event_depth") 37 | return 11; 38 | else if (topic_name == "right_event_depth") 39 | return 12; 40 | else if (topic_name == "left_camera_depth") 41 | return 13; 42 | else if (topic_name == "right_camera_depth") 43 | return 14; 44 | else if (topic_name == "left_camera_undistort") 45 | return 15; 46 | else if (topic_name == "right_camera_undistort") 47 | return 16; 48 | else 49 | return 0; 50 | } 51 | } 52 | return 0; 53 | } 54 | 55 | void PrintDirectoryInfo(std::map> & out_bags_info) { 56 | ROS_INFO("%s", colorful_char::info("The following valid rosbags have been detected: ").c_str()); 57 | for (auto bag_it = out_bags_info.begin(); bag_it != out_bags_info.end(); ++bag_it) { 58 | std::sort(bag_it->second.begin(), bag_it->second.end()); 59 | std::string topic_list = ""; 60 | for (int topic_id : bag_it->second) { 61 | switch (topic_id) { 62 | case 1 : topic_list += "IMU"; break; 63 | case 2 : topic_list += "Left Event Camera"; break; 64 | case 3 : topic_list += "Right Event Camera"; break; 65 | case 4 : topic_list += "Left Regular Camera"; break; 66 | case 5 : topic_list += "Right Regular Camera"; break; 67 | case 6 : topic_list += "Kinect Color Camera"; break; 68 | case 7 : topic_list += "Kinect Depth Camera"; break; 69 | case 8 : topic_list += "LiDAR"; break; 70 | case 9 : topic_list += "Ground Truth Signal"; break; 71 | case 10 : topic_list += "Full Depth Reprojection"; break; 72 | case 11 : topic_list += "Left Event Camera (Depth)"; break; 73 | case 12 : topic_list += "Right Event Camera (Depth)"; break; 74 | case 13 : topic_list += "Left Regular Camera (Depth)"; break; 75 | case 14 : topic_list += "Right Regular Camera (Depth)"; break; 76 | case 15 : topic_list += "Left Regular Camera (Undistorted)"; break; 77 | case 16 : topic_list += "Right Regular Camera (Undistorted)"; break; 78 | } 79 | if (topic_id != bag_it->second.back()) topic_list += " | "; 80 | } 81 | if (bag_it->second.size() == 1) { 82 | ROS_INFO_STREAM("Data Sequence Name: \033[1;36m" << bag_it->first << "\033[0m"); 83 | ROS_INFO_STREAM("Data Sequence Topic: \033[1;36m" << topic_list << "\033[1;31m --> Single bag cannot be merged! \033[0m"); 84 | } else { 85 | ROS_INFO_STREAM("Data Sequence Name: \033[1;36m" << bag_it->first << "\033[0m"); 86 | ROS_INFO_STREAM("Data Sequence Topics: \033[1;36m" << topic_list << "\033[0m"); 87 | } 88 | } 89 | } 90 | 91 | // ------------------------------------------------------------------------- // 92 | 93 | int main(int argc, char ** argv) { 94 | ros::init(argc, argv, "bag_merger"); 95 | ros::NodeHandle nh; 96 | 97 | // Check the directory path and create a directory for merged bags under same path. 98 | std::string in_dir_path, out_dir_path; 99 | ros::param::get("/directory_path", in_dir_path); 100 | if (!directory_path_check(in_dir_path)) { 101 | ros::shutdown(); 102 | return -1; 103 | }; 104 | out_dir_path = in_dir_path + "merged_bags/"; 105 | fs::create_directory(out_dir_path); 106 | 107 | // Read each rosbag's absolute path from the input directory and classify by their prefix. 108 | fs::path dir(in_dir_path); 109 | fs::directory_iterator bag_path_it(dir); 110 | fs::directory_iterator end_it; 111 | std::map> out_bags_info; 112 | for (; bag_path_it != end_it; ++bag_path_it) { 113 | if (fs::is_regular_file(bag_path_it->status()) && bag_path_it->path().extension().string() == ".bag") { 114 | std::string bag_name = bag_path_it->path().filename().string(); 115 | std::string prefix; 116 | int topic_id = BagNameParser(bag_name, prefix); 117 | if (topic_id == 0) continue; 118 | out_bags_info[prefix].emplace_back(topic_id); 119 | } 120 | } 121 | if (out_bags_info.size() == 0) { 122 | ROS_ERROR("%s", colorful_char::error("Found no rosbag under the directory path: " + in_dir_path).c_str()); 123 | ros::shutdown(); 124 | return -1; 125 | } 126 | PrintDirectoryInfo(out_bags_info); 127 | 128 | // Merge all bags with the same prefix. 129 | for (auto bag_it = out_bags_info.begin(); bag_it != out_bags_info.end(); ++bag_it) { 130 | if (bag_it->second.size() == 1) continue; 131 | rosbag::Bag bag_out; 132 | bag_out.open(out_dir_path + bag_it->first + ".merged.bag", rosbag::bagmode::Write); 133 | 134 | // Read each single-topic bag. 135 | size_t topic_num = bag_it->second.size(); 136 | size_t msg_size = 0; 137 | rosbag::Bag bags_in[topic_num]; 138 | rosbag::View bags_views[topic_num]; 139 | std::vector bags_msg_ptr; 140 | std::vector are_bags_valid; 141 | for (size_t idx = 0; idx < bag_it->second.size(); ++idx) { 142 | switch (bag_it->second[idx]) { 143 | case 1 : bags_in[idx].open(in_dir_path + bag_it->first + ".imu.bag", rosbag::bagmode::Read); break; 144 | case 2 : bags_in[idx].open(in_dir_path + bag_it->first + ".left_event.bag", rosbag::bagmode::Read); break; 145 | case 3 : bags_in[idx].open(in_dir_path + bag_it->first + ".right_event.bag", rosbag::bagmode::Read); break; 146 | case 4 : bags_in[idx].open(in_dir_path + bag_it->first + ".left_camera.bag", rosbag::bagmode::Read); break; 147 | case 5 : bags_in[idx].open(in_dir_path + bag_it->first + ".right_camera.bag", rosbag::bagmode::Read); break; 148 | case 6 : bags_in[idx].open(in_dir_path + bag_it->first + ".kinect_color.bag", rosbag::bagmode::Read); break; 149 | case 7 : bags_in[idx].open(in_dir_path + bag_it->first + ".kinect_depth.bag", rosbag::bagmode::Read); break; 150 | case 8 : bags_in[idx].open(in_dir_path + bag_it->first + ".lidar.bag", rosbag::bagmode::Read); break; 151 | case 9 : bags_in[idx].open(in_dir_path + bag_it->first + ".gt.bag", rosbag::bagmode::Read); break; 152 | case 10 : bags_in[idx].open(in_dir_path + bag_it->first + ".depth_reprojection.bag", rosbag::bagmode::Read); break; 153 | case 11 : bags_in[idx].open(in_dir_path + bag_it->first + ".left_event_depth.bag", rosbag::bagmode::Read); break; 154 | case 12 : bags_in[idx].open(in_dir_path + bag_it->first + ".right_event_depth.bag", rosbag::bagmode::Read); break; 155 | case 13 : bags_in[idx].open(in_dir_path + bag_it->first + ".left_camera_depth.bag", rosbag::bagmode::Read); break; 156 | case 14 : bags_in[idx].open(in_dir_path + bag_it->first + ".right_camera_depth.bag", rosbag::bagmode::Read); break; 157 | case 15 : bags_in[idx].open(in_dir_path + bag_it->first + ".left_camera_undistort.bag", rosbag::bagmode::Read); break; 158 | case 16 : bags_in[idx].open(in_dir_path + bag_it->first + ".right_camera_undistort.bag", rosbag::bagmode::Read); break; 159 | } 160 | bags_views[idx].addQuery(bags_in[idx]); 161 | msg_size += bags_views[idx].size(); 162 | bags_msg_ptr.emplace_back(bags_views[idx].begin()); 163 | are_bags_valid.emplace_back(true); 164 | } 165 | 166 | // Merge all messages from each single-topic bag chronologically. 167 | size_t msg_idx = 0; 168 | while (topic_num != 0) { 169 | int earliest_idx; 170 | double earliest_ts = DBL_MAX; 171 | for (size_t idx = 0; idx < bag_it->second.size(); ++idx) { 172 | if (are_bags_valid[idx] && earliest_ts > bags_msg_ptr[idx]->getTime().toSec()) { 173 | earliest_idx = idx; 174 | earliest_ts = bags_msg_ptr[idx]->getTime().toSec(); 175 | } 176 | } 177 | bag_out.write(bags_msg_ptr[earliest_idx]->getTopic(), bags_msg_ptr[earliest_idx]->getTime(), *bags_msg_ptr[earliest_idx]); 178 | ++bags_msg_ptr[earliest_idx]; 179 | if (bags_msg_ptr[earliest_idx] == bags_views[earliest_idx].end()) { 180 | are_bags_valid[earliest_idx] = false; 181 | --topic_num; 182 | bags_in[earliest_idx].close(); 183 | } 184 | 185 | ++msg_idx; 186 | if (msg_idx % 100000 == 0) 187 | ROS_INFO_STREAM(msg_idx << "/" << msg_size << " messages from data sequence \"" << bag_it->first << "\" have been processed."); 188 | } 189 | ROS_INFO("%s", colorful_char::info("All \"" + bag_it->first + "\"-related bags have been merged into a new bag! Enjoy!").c_str()); 190 | bag_out.close(); 191 | } 192 | 193 | ros::shutdown(); 194 | return 0; 195 | } 196 | -------------------------------------------------------------------------------- /src/bag_splitter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // ------------------------------------------------------------------------- // 6 | 7 | int main(int argc, char ** argv) { 8 | ros::init(argc, argv, "bag_splitter"); 9 | ros::NodeHandle nh; 10 | 11 | bool receive_imu = false; 12 | bool receive_event_left = false; 13 | bool receive_event_right = false; 14 | bool receive_camera_left = false; 15 | bool receive_camera_right = false; 16 | bool receive_kinect_color = false; 17 | bool receive_kinect_depth = false; 18 | bool receive_lidar = false; 19 | bool receive_gt = false; 20 | bool receive_event_left_depth = false; 21 | bool receive_event_right_depth = false; 22 | bool receive_camera_left_depth = false; 23 | bool receive_camera_right_depth = false; 24 | bool receive_camera_left_undistort = false; 25 | bool receive_camera_right_undistort = false; 26 | 27 | std::string bag_in_path, imu_topic, event_left_topic, event_right_topic, camera_left_topic, camera_right_topic, kinect_color_topic, 28 | kinect_depth_topic, lidar_topic, gt_topic, event_left_depth_topic, event_right_depth_topic, camera_left_depth_topic, 29 | camera_right_depth_topic, camera_left_undistort_topic, camera_right_undistort_topic; 30 | bool need_compression; 31 | ros::param::get("/bag_path", bag_in_path); 32 | ros::param::get("/need_compression", need_compression); 33 | ros::param::get("/imu_topic", imu_topic); 34 | ros::param::get("/event_left_topic", event_left_topic); 35 | ros::param::get("/event_right_topic", event_right_topic); 36 | ros::param::get("/camera_left_topic", camera_left_topic); 37 | ros::param::get("/camera_right_topic", camera_right_topic); 38 | ros::param::get("/kinect_color_topic", kinect_color_topic); 39 | ros::param::get("/kinect_depth_topic", kinect_depth_topic); 40 | ros::param::get("/lidar_topic", lidar_topic); 41 | ros::param::get("/ground_truth_topic", gt_topic); 42 | ros::param::get("/event_left_depth_topic", event_left_depth_topic); 43 | ros::param::get("/event_right_depth_topic", event_right_depth_topic); 44 | ros::param::get("/camera_left_depth_topic", camera_left_depth_topic); 45 | ros::param::get("/camera_right_depth_topic", camera_right_depth_topic); 46 | ros::param::get("/camera_left_undistort_topic", camera_left_undistort_topic); 47 | ros::param::get("/camera_right_undistort_topic", camera_right_undistort_topic); 48 | if (!file_path_check(bag_in_path)) { 49 | ros::shutdown(); 50 | return -1; 51 | } 52 | rosbag::Bag bag_in, imu_bag_out, event_left_bag_out, event_right_bag_out, camera_left_bag_out, camera_right_bag_out, kinect_color_bag_out, 53 | kinect_depth_bag_out, lidar_bag_out, gt_bag_out, event_left_depth_bag_out, event_right_depth_bag_out, camera_left_depth_bag_out, 54 | camera_right_depth_bag_out, camera_left_undistort_bag_out, camera_right_undistort_bag_out; 55 | bag_in.open(bag_in_path, rosbag::bagmode::Read); 56 | std::string bag_path_without_extension = bag_in_path.substr(0, bag_in_path.size() - 4); 57 | 58 | // For every message read in rosbag, re-write it into another bag entitled with its topic name. 59 | uint32_t msg_idx = 0; 60 | uint32_t msg_size = rosbag::View(bag_in).size(); 61 | for (rosbag::MessageInstance const msg : rosbag::View(bag_in)) { 62 | if (msg.getTopic() == imu_topic) { 63 | if (!receive_imu) { 64 | receive_imu = true; 65 | imu_bag_out.open(bag_path_without_extension + ".imu.bag", rosbag::bagmode::Write); 66 | if (need_compression) imu_bag_out.setCompression(rosbag::compression::BZ2); 67 | ROS_INFO("%s", colorful_char::info("Receives data from the IMU!").c_str()); 68 | } 69 | imu_bag_out.write(imu_topic, msg.getTime(), msg); 70 | } 71 | 72 | else if (msg.getTopic() == event_left_topic) { 73 | if (!receive_event_left) { 74 | receive_event_left = true; 75 | event_left_bag_out.open(bag_path_without_extension + ".left_event.bag", rosbag::bagmode::Write); 76 | if (need_compression) event_left_bag_out.setCompression(rosbag::compression::BZ2); 77 | ROS_INFO("%s", colorful_char::info("Receives data from the Left Event Camera!").c_str()); 78 | } 79 | event_left_bag_out.write(event_left_topic, msg.getTime(), msg); 80 | } 81 | 82 | else if (msg.getTopic() == event_right_topic) { 83 | if (!receive_event_right) { 84 | receive_event_right = true; 85 | event_right_bag_out.open(bag_path_without_extension + ".right_event.bag", rosbag::bagmode::Write); 86 | if (need_compression) event_right_bag_out.setCompression(rosbag::compression::BZ2); 87 | ROS_INFO("%s", colorful_char::info("Receives data from the Right Event Camera!").c_str()); 88 | } 89 | event_right_bag_out.write(event_right_topic, msg.getTime(), msg); 90 | } 91 | 92 | else if (msg.getTopic() == camera_left_topic) { 93 | if (!receive_camera_left) { 94 | receive_camera_left = true; 95 | camera_left_bag_out.open(bag_path_without_extension + ".left_camera.bag", rosbag::bagmode::Write); 96 | if (need_compression) camera_left_bag_out.setCompression(rosbag::compression::BZ2); 97 | ROS_INFO("%s", colorful_char::info("Receives data from the Left Regular Camera!").c_str()); 98 | } 99 | camera_left_bag_out.write(camera_left_topic, msg.getTime(), msg); 100 | } 101 | 102 | else if (msg.getTopic() == camera_right_topic) { 103 | if (!receive_camera_right) { 104 | receive_camera_right = true; 105 | camera_right_bag_out.open(bag_path_without_extension + ".right_camera.bag", rosbag::bagmode::Write); 106 | if (need_compression) camera_right_bag_out.setCompression(rosbag::compression::BZ2); 107 | ROS_INFO("%s", colorful_char::info("Receives data from the Right Regular Camera!").c_str()); 108 | } 109 | camera_right_bag_out.write(camera_right_topic, msg.getTime(), msg); 110 | } 111 | 112 | else if (msg.getTopic() == kinect_color_topic) { 113 | if (!receive_kinect_color) { 114 | receive_kinect_color = true; 115 | kinect_color_bag_out.open(bag_path_without_extension + ".kinect_color.bag", rosbag::bagmode::Write); 116 | if (need_compression) kinect_color_bag_out.setCompression(rosbag::compression::BZ2); 117 | ROS_INFO("%s", colorful_char::info("Receives data from the Kinect Color Camera!").c_str()); 118 | } 119 | kinect_color_bag_out.write(kinect_color_topic, msg.getTime(), msg); 120 | } 121 | 122 | else if (msg.getTopic() == kinect_depth_topic) { 123 | if (!receive_kinect_depth) { 124 | receive_kinect_depth = true; 125 | kinect_depth_bag_out.open(bag_path_without_extension + ".kinect_depth.bag", rosbag::bagmode::Write); 126 | if (need_compression) kinect_depth_bag_out.setCompression(rosbag::compression::BZ2); 127 | ROS_INFO("%s", colorful_char::info("Receives data from the Kinect Depth Camera!").c_str()); 128 | } 129 | kinect_depth_bag_out.write(kinect_depth_topic, msg.getTime(), msg); 130 | } 131 | 132 | else if (msg.getTopic() == lidar_topic) { 133 | if (!receive_lidar) { 134 | receive_lidar = true; 135 | lidar_bag_out.open(bag_path_without_extension + ".lidar.bag", rosbag::bagmode::Write); 136 | if (need_compression) lidar_bag_out.setCompression(rosbag::compression::BZ2); 137 | ROS_INFO("%s", colorful_char::info("Receives data from the LiDAR!").c_str()); 138 | } 139 | lidar_bag_out.write(lidar_topic, msg.getTime(), msg); 140 | } 141 | 142 | else if (msg.getTopic() == gt_topic) { 143 | if (!receive_gt) { 144 | receive_gt = true; 145 | gt_bag_out.open(bag_path_without_extension + ".gt.bag", rosbag::bagmode::Write); 146 | if (need_compression) gt_bag_out.setCompression(rosbag::compression::BZ2); 147 | ROS_INFO("%s", colorful_char::info("Receives data from the Ground Truth Signal!").c_str()); 148 | } 149 | gt_bag_out.write(gt_topic, msg.getTime(), msg); 150 | } 151 | 152 | else if (msg.getTopic() == event_left_depth_topic) { 153 | if (!receive_event_left_depth) { 154 | receive_event_left_depth = true; 155 | event_left_depth_bag_out.open(bag_path_without_extension + ".left_event_depth.bag", rosbag::bagmode::Write); 156 | if (need_compression) event_left_depth_bag_out.setCompression(rosbag::compression::BZ2); 157 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Event Camera!").c_str()); 158 | } 159 | event_left_depth_bag_out.write(event_left_depth_topic, msg.getTime(), msg); 160 | } 161 | 162 | else if (msg.getTopic() == event_right_depth_topic) { 163 | if (!receive_event_right_depth) { 164 | receive_event_right_depth = true; 165 | event_right_depth_bag_out.open(bag_path_without_extension + ".right_event_depth.bag", rosbag::bagmode::Write); 166 | if (need_compression) event_right_depth_bag_out.setCompression(rosbag::compression::BZ2); 167 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Event Camera!").c_str()); 168 | } 169 | event_right_depth_bag_out.write(event_right_depth_topic, msg.getTime(), msg); 170 | } 171 | 172 | else if (msg.getTopic() == camera_left_depth_topic) { 173 | if (!receive_camera_left_depth) { 174 | receive_camera_left_depth = true; 175 | camera_left_depth_bag_out.open(bag_path_without_extension + ".left_camera_depth.bag", rosbag::bagmode::Write); 176 | if (need_compression) camera_left_depth_bag_out.setCompression(rosbag::compression::BZ2); 177 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Regular Camera!").c_str()); 178 | } 179 | camera_left_depth_bag_out.write(camera_left_depth_topic, msg.getTime(), msg); 180 | } 181 | 182 | else if (msg.getTopic() == camera_right_depth_topic) { 183 | if (!receive_camera_right_depth) { 184 | receive_camera_right_depth = true; 185 | camera_right_depth_bag_out.open(bag_path_without_extension + ".right_camera_depth.bag", rosbag::bagmode::Write); 186 | if (need_compression) camera_right_depth_bag_out.setCompression(rosbag::compression::BZ2); 187 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Regular Camera!").c_str()); 188 | } 189 | camera_right_depth_bag_out.write(camera_right_depth_topic, msg.getTime(), msg); 190 | } 191 | 192 | else if (msg.getTopic() == camera_left_undistort_topic) { 193 | if (!receive_camera_left_undistort) { 194 | receive_camera_left_undistort = true; 195 | camera_left_undistort_bag_out.open(bag_path_without_extension + ".left_camera_undistort.bag", rosbag::bagmode::Write); 196 | if (need_compression) camera_left_undistort_bag_out.setCompression(rosbag::compression::BZ2); 197 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Left Regular Camera!").c_str()); 198 | } 199 | camera_left_undistort_bag_out.write(camera_left_undistort_topic, msg.getTime(), msg); 200 | } 201 | 202 | else if (msg.getTopic() == camera_right_undistort_topic) { 203 | if (!receive_camera_right_undistort) { 204 | receive_camera_right_undistort = true; 205 | camera_right_undistort_bag_out.open(bag_path_without_extension + ".right_camera_undistort.bag", rosbag::bagmode::Write); 206 | if (need_compression) camera_right_undistort_bag_out.setCompression(rosbag::compression::BZ2); 207 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Right Regular Camera!").c_str()); 208 | } 209 | camera_right_undistort_bag_out.write(camera_right_undistort_topic, msg.getTime(), msg); 210 | } 211 | 212 | msg_idx++; 213 | if (msg_idx % 10000 == 0) ROS_INFO_STREAM(msg_idx << "/" << msg_size << " messages have been processed!"); 214 | } 215 | ROS_INFO("%s", colorful_char::info("All messages have been processed!").c_str()); 216 | 217 | if (receive_imu) imu_bag_out.close(); 218 | if (receive_event_left) event_left_bag_out.close(); 219 | if (receive_event_right) event_right_bag_out.close(); 220 | if (receive_camera_left) camera_left_bag_out.close(); 221 | if (receive_camera_right) camera_right_bag_out.close(); 222 | if (receive_kinect_color) kinect_color_bag_out.close(); 223 | if (receive_kinect_depth) kinect_depth_bag_out.close(); 224 | if (receive_lidar) lidar_bag_out.close(); 225 | if (receive_gt) gt_bag_out.close(); 226 | if (receive_event_left_depth) event_left_depth_bag_out.close(); 227 | if (receive_event_right_depth) event_right_depth_bag_out.close(); 228 | if (receive_camera_left_depth) camera_left_depth_bag_out.close(); 229 | if (receive_camera_right_depth) camera_right_depth_bag_out.close(); 230 | if (receive_camera_left_undistort) camera_left_undistort_bag_out.close(); 231 | if (receive_camera_right_undistort) camera_right_undistort_bag_out.close(); 232 | 233 | ros::shutdown(); 234 | return 0; 235 | } 236 | -------------------------------------------------------------------------------- /src/data_validator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define TS_DIFF_THLD 0.01 // 99% - 101% 4 | #define EVENT_SKIP_TS 5.0 // [s] 5 | 6 | // ------------------------------------------------------------------------- // 7 | 8 | bool receive_imu = false; 9 | bool receive_event_left = false; 10 | bool receive_event_right = false; 11 | bool receive_camera_left = false; 12 | bool receive_camera_right = false; 13 | bool receive_kinect_color = false; 14 | bool receive_kinect_depth = false; 15 | bool receive_lidar = false; 16 | bool receive_gt = false; 17 | bool receive_event_left_depth = false; 18 | bool receive_event_right_depth = false; 19 | bool receive_camera_left_depth = false; 20 | bool receive_camera_right_depth = false; 21 | bool receive_camera_left_undistort = false; 22 | bool receive_camera_right_undistort = false; 23 | 24 | double prev_imu_ts = 0; 25 | uint64_t prev_event_left_ts = 0; 26 | double prev_event_left_array_ts = 0; 27 | uint64_t prev_event_right_ts = 0; 28 | double prev_event_right_array_ts = 0; 29 | double prev_camera_left_ts = 0; 30 | double prev_camera_right_ts = 0; 31 | double prev_kinect_color_ts = 0; 32 | double prev_kinect_depth_ts = 0; 33 | double prev_lidar_ts = 0; 34 | double prev_gt_ts = 0; 35 | double prev_event_left_depth_ts = 0; 36 | double prev_event_right_depth_ts = 0; 37 | double prev_camera_left_depth_ts = 0; 38 | double prev_camera_right_depth_ts = 0; 39 | double prev_camera_left_undistort_ts = 0; 40 | double prev_camera_right_undistort_ts = 0; 41 | 42 | std::vector event_left_ts; 43 | std::vector event_left_size; 44 | std::vector event_right_ts; 45 | std::vector event_right_size; 46 | 47 | // ------------------------------------------------------------------------- // 48 | 49 | // For every sensor handler, it contains the following features: 50 | // (1) Check whether the bag contains this type of sensor message 51 | // (2) Check whether there is an unusual frequency on the stream (i.e. some message is lost) 52 | // (3) Check whether there is a chronological error caused by synchronization 53 | // (4*) For event handler, count the number of events for future calculation of the Mean Event Rate. 54 | 55 | void ImuHandler(const IMU::ConstPtr & imu_message) { 56 | if (!receive_imu) { 57 | receive_imu = true; 58 | prev_imu_ts = imu_message->header.stamp.toSec(); 59 | ROS_INFO("%s", colorful_char::info("Receives data from the IMU!").c_str()); 60 | } else { 61 | double imu_ts_diff = imu_message->header.stamp.toSec() - prev_imu_ts; 62 | if (imu_ts_diff < (1 - TS_DIFF_THLD) * IMU_PERIOD || imu_ts_diff > (1 + TS_DIFF_THLD) * IMU_PERIOD) 63 | ROS_WARN("%s", colorful_char::warning("IMU stream detects an unusual frequency!").c_str()); 64 | if (imu_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("IMU stream detects a chronological error!").c_str()); 65 | prev_imu_ts = imu_message->header.stamp.toSec(); 66 | } 67 | } 68 | 69 | void EventLeftHandler(const EventArray::ConstPtr & event_array_message) { 70 | if (!receive_event_left) { 71 | receive_event_left = true; 72 | prev_event_left_array_ts = event_array_message->header.stamp.toSec(); 73 | ROS_INFO("%s", colorful_char::info("Receives data from the Left Event Camera!").c_str()); 74 | } else { 75 | double left_event_array_ts_diff = event_array_message->header.stamp.toSec() - prev_event_left_array_ts; 76 | if (left_event_array_ts_diff < 0) 77 | ROS_WARN("%s", colorful_char::warning("Left Event Array stream detects a chronological error!").c_str()); 78 | prev_event_left_array_ts = event_array_message->header.stamp.toSec(); 79 | } 80 | for (auto & event : event_array_message->events) { 81 | if (event.ts.toNSec() >= prev_event_left_ts) prev_event_left_ts = event.ts.toNSec(); 82 | else 83 | ROS_WARN_ONCE("%s", colorful_char::warning("Left Event stream detects a chronological error!").c_str()); 84 | } 85 | event_left_ts.emplace_back(event_array_message->header.stamp.toSec()); 86 | event_left_size.emplace_back(event_array_message->events.size()); 87 | } 88 | 89 | void EventRightHandler(const EventArray::ConstPtr & event_array_message) { 90 | if (!receive_event_right) { 91 | receive_event_right = true; 92 | prev_event_right_array_ts = event_array_message->header.stamp.toSec(); 93 | ROS_INFO("%s", colorful_char::info("Receives data from the Right Event Camera!").c_str()); 94 | } else { 95 | double right_event_array_ts_diff = event_array_message->header.stamp.toSec() - prev_event_right_array_ts; 96 | if (right_event_array_ts_diff < 0) 97 | ROS_WARN("%s", colorful_char::warning("Right Event Array stream detects a chronological error!").c_str()); 98 | prev_event_right_array_ts = event_array_message->header.stamp.toSec(); 99 | } 100 | for (auto & event : event_array_message->events) { 101 | if (event.ts.toNSec() >= prev_event_right_ts) prev_event_right_ts = event.ts.toNSec(); 102 | else 103 | ROS_WARN_ONCE("%s", colorful_char::warning("Right Event stream detects a chronological error!").c_str()); 104 | } 105 | event_right_ts.emplace_back(event_array_message->header.stamp.toSec()); 106 | event_right_size.emplace_back(event_array_message->events.size()); 107 | } 108 | 109 | void CameraLeftHandler(const Image::ConstPtr & image_message) { 110 | if (!receive_camera_left) { 111 | receive_camera_left = true; 112 | prev_camera_left_ts = image_message->header.stamp.toSec(); 113 | ROS_INFO("%s", colorful_char::info("Receives data from the Left Regular Camera!").c_str()); 114 | } else { 115 | double left_camera_ts_diff = image_message->header.stamp.toSec() - prev_camera_left_ts; 116 | if (left_camera_ts_diff < (1 - TS_DIFF_THLD) * CAM_PERIOD || left_camera_ts_diff > (1 + TS_DIFF_THLD) * CAM_PERIOD) 117 | ROS_WARN("%s", colorful_char::warning("Left Image stream detects an unusual frequency!").c_str()); 118 | if (left_camera_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("Left Image stream detects a chronological error!").c_str()); 119 | prev_camera_left_ts = image_message->header.stamp.toSec(); 120 | } 121 | } 122 | 123 | void CameraRightHandler(const Image::ConstPtr & image_message) { 124 | if (!receive_camera_right) { 125 | receive_camera_right = true; 126 | prev_camera_right_ts = image_message->header.stamp.toSec(); 127 | ROS_INFO("%s", colorful_char::info("Receives data from the Right Regular Camera!").c_str()); 128 | } else { 129 | double right_camera_ts_diff = image_message->header.stamp.toSec() - prev_camera_right_ts; 130 | if (right_camera_ts_diff < (1 - TS_DIFF_THLD) * CAM_PERIOD || right_camera_ts_diff > (1 + TS_DIFF_THLD) * CAM_PERIOD) 131 | ROS_WARN("%s", colorful_char::warning("Right Image stream detects an unusual frequency!").c_str()); 132 | if (right_camera_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("Right Image stream detects a chronological error!").c_str()); 133 | prev_camera_right_ts = image_message->header.stamp.toSec(); 134 | } 135 | } 136 | 137 | void KinectColorHandler(const Image::ConstPtr & image_message) { 138 | if (!receive_kinect_color) { 139 | receive_kinect_color = true; 140 | prev_kinect_color_ts = image_message->header.stamp.toSec(); 141 | ROS_INFO("%s", colorful_char::info("Receives data from the Kinect Color Camera!").c_str()); 142 | } else { 143 | double kinect_color_ts_diff = image_message->header.stamp.toSec() - prev_kinect_color_ts; 144 | if (kinect_color_ts_diff > (2 - TS_DIFF_THLD) * KINECT_PERIOD && kinect_color_ts_diff < (2 + TS_DIFF_THLD) * KINECT_PERIOD) 145 | ROS_WARN("%s", colorful_char::warning("Kinect Color stream detects one lost frame!").c_str()); 146 | else if (kinect_color_ts_diff > (3 - TS_DIFF_THLD) * KINECT_PERIOD && kinect_color_ts_diff < (3 + TS_DIFF_THLD) * KINECT_PERIOD) 147 | ROS_WARN("%s", colorful_char::warning("Kinect Color stream detects two lost frame!").c_str()); 148 | else if (kinect_color_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || kinect_color_ts_diff > (1 + TS_DIFF_THLD) * KINECT_PERIOD) 149 | ROS_WARN("%s", colorful_char::warning("Kinect Color stream detects an unusual frequency!").c_str()); 150 | if (kinect_color_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("Kinect Color stream detects a chronological error!").c_str()); 151 | prev_kinect_color_ts = image_message->header.stamp.toSec(); 152 | } 153 | } 154 | 155 | void KinectDepthHandler(const Image::ConstPtr & image_message) { 156 | if (!receive_kinect_depth) { 157 | receive_kinect_depth = true; 158 | prev_kinect_depth_ts = image_message->header.stamp.toSec(); 159 | ROS_INFO("%s", colorful_char::info("Receives data from the Kinect Depth Camera!").c_str()); 160 | } else { 161 | double kinect_depth_ts_diff = image_message->header.stamp.toSec() - prev_kinect_depth_ts; 162 | if (kinect_depth_ts_diff > (2 - TS_DIFF_THLD) * KINECT_PERIOD && kinect_depth_ts_diff < (2 + TS_DIFF_THLD) * KINECT_PERIOD) 163 | ROS_WARN("%s", colorful_char::warning("Kinect Depth stream detects one lost frame!").c_str()); 164 | else if (kinect_depth_ts_diff > (3 - TS_DIFF_THLD) * KINECT_PERIOD && kinect_depth_ts_diff < (3 + TS_DIFF_THLD) * KINECT_PERIOD) 165 | ROS_WARN("%s", colorful_char::warning("Kinect Depth stream detects two lost frame!").c_str()); 166 | else if (kinect_depth_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || kinect_depth_ts_diff > (1 + TS_DIFF_THLD) * KINECT_PERIOD) 167 | ROS_WARN("%s", colorful_char::warning("Kinect Depth stream detects an unusual frequency!").c_str()); 168 | if (kinect_depth_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("Kinect Depth stream detects a chronological error!").c_str()); 169 | prev_kinect_depth_ts = image_message->header.stamp.toSec(); 170 | } 171 | } 172 | 173 | void LidarHandler(const PointCloud::ConstPtr & point_cloud_message) { 174 | if (!receive_lidar) { 175 | receive_lidar = true; 176 | prev_lidar_ts = point_cloud_message->header.stamp.toSec(); 177 | ROS_INFO("%s", colorful_char::info("Receives data from the LiDAR!").c_str()); 178 | } else { 179 | double lidar_ts_diff = point_cloud_message->header.stamp.toSec() - prev_lidar_ts; 180 | // 98%-102% confidence here! 181 | if (lidar_ts_diff < (1 - 2 * TS_DIFF_THLD) * LIDAR_PERIOD || lidar_ts_diff > (1 + 2 * TS_DIFF_THLD) * LIDAR_PERIOD) 182 | ROS_WARN("%s", colorful_char::warning("LiDAR stream detects an unusual frequency!").c_str()); 183 | if (lidar_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("LiDAR stream detects a chronological error!").c_str()); 184 | prev_lidar_ts = point_cloud_message->header.stamp.toSec(); 185 | } 186 | } 187 | 188 | void GroundTruthHandler(const PoseStamped::ConstPtr & pose_stamped_message) { 189 | if (!receive_gt) { 190 | receive_gt = true; 191 | prev_gt_ts = pose_stamped_message->header.stamp.toSec(); 192 | ROS_INFO("%s", colorful_char::info("Receives data from the Ground Truth Signal!").c_str()); 193 | } else { 194 | double gt_ts_diff = pose_stamped_message->header.stamp.toSec() - prev_gt_ts; 195 | if (gt_ts_diff < (1 - TS_DIFF_THLD) * GT_PERIOD || gt_ts_diff > (1 + TS_DIFF_THLD) * GT_PERIOD) 196 | ROS_WARN("%s", colorful_char::warning("Ground Truth stream detects an unusual frequency!").c_str()); 197 | if (gt_ts_diff < 0) ROS_WARN("%s", colorful_char::warning("Ground Truth stream detects a chronological error!").c_str()); 198 | prev_gt_ts = pose_stamped_message->header.stamp.toSec(); 199 | } 200 | } 201 | 202 | void EventLeftDepthHandler(const Image::ConstPtr & image_message) { 203 | if (!receive_event_left_depth) { 204 | receive_event_left_depth = true; 205 | prev_event_left_depth_ts = image_message->header.stamp.toSec(); 206 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Event Camera!").c_str()); 207 | } else { 208 | double event_left_depth_ts_diff = image_message->header.stamp.toSec() - prev_event_left_depth_ts; 209 | if (event_left_depth_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || event_left_depth_ts_diff > (3 + TS_DIFF_THLD) * KINECT_PERIOD) 210 | ROS_WARN("%s", colorful_char::warning("Left Event Camera's depth stream detects an unusual frequency!").c_str()); 211 | if (event_left_depth_ts_diff < 0) 212 | ROS_WARN("%s", colorful_char::warning("Left Event Camera's depth stream detects a chronological error!").c_str()); 213 | prev_event_left_depth_ts = image_message->header.stamp.toSec(); 214 | } 215 | } 216 | 217 | void EventRightDepthHandler(const Image::ConstPtr & image_message) { 218 | if (!receive_event_right_depth) { 219 | receive_event_right_depth = true; 220 | prev_event_right_depth_ts = image_message->header.stamp.toSec(); 221 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Event Camera!").c_str()); 222 | } else { 223 | double event_right_depth_ts_diff = image_message->header.stamp.toSec() - prev_event_right_depth_ts; 224 | if (event_right_depth_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || event_right_depth_ts_diff > (3 + TS_DIFF_THLD) * KINECT_PERIOD) 225 | ROS_WARN("%s", colorful_char::warning("Right Event Camera's depth stream detects an unusual frequency!").c_str()); 226 | if (event_right_depth_ts_diff < 0) 227 | ROS_WARN("%s", colorful_char::warning("Right Event Camera's depth stream detects a chronological error!").c_str()); 228 | prev_event_right_depth_ts = image_message->header.stamp.toSec(); 229 | } 230 | } 231 | 232 | void CameraLeftDepthHandler(const Image::ConstPtr & image_message) { 233 | if (!receive_camera_left_depth) { 234 | receive_camera_left_depth = true; 235 | prev_camera_left_depth_ts = image_message->header.stamp.toSec(); 236 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Left Regular Camera!").c_str()); 237 | } else { 238 | double camera_left_depth_ts_diff = image_message->header.stamp.toSec() - prev_camera_left_depth_ts; 239 | if (camera_left_depth_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || camera_left_depth_ts_diff > (3 + TS_DIFF_THLD) * KINECT_PERIOD) 240 | ROS_WARN("%s", colorful_char::warning("Left Regular Camera's depth stream detects an unusual frequency!").c_str()); 241 | if (camera_left_depth_ts_diff < 0) 242 | ROS_WARN("%s", colorful_char::warning("Left Regular Camera's depth stream detects a chronological error!").c_str()); 243 | prev_camera_left_depth_ts = image_message->header.stamp.toSec(); 244 | } 245 | } 246 | 247 | void CameraRightDepthHandler(const Image::ConstPtr & image_message) { 248 | if (!receive_camera_right_depth) { 249 | receive_camera_right_depth = true; 250 | prev_camera_right_depth_ts = image_message->header.stamp.toSec(); 251 | ROS_INFO("%s", colorful_char::info("Receives reprojected depth data from the Right Regular Camera!").c_str()); 252 | } else { 253 | double camera_right_depth_ts_diff = image_message->header.stamp.toSec() - prev_camera_right_depth_ts; 254 | if (camera_right_depth_ts_diff < (1 - TS_DIFF_THLD) * KINECT_PERIOD || camera_right_depth_ts_diff > (3 + TS_DIFF_THLD) * KINECT_PERIOD) 255 | ROS_WARN("%s", colorful_char::warning("Right Regular Camera's depth stream detects an unusual frequency!").c_str()); 256 | if (camera_right_depth_ts_diff < 0) 257 | ROS_WARN("%s", colorful_char::warning("Right Regular Camera's depth stream detects a chronological error!").c_str()); 258 | prev_camera_right_depth_ts = image_message->header.stamp.toSec(); 259 | } 260 | } 261 | 262 | void CameraLeftUndistotrtHandler(const Image::ConstPtr & image_message) { 263 | if (!receive_camera_left_undistort) { 264 | receive_camera_left_undistort = true; 265 | prev_camera_left_undistort_ts = image_message->header.stamp.toSec(); 266 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Left Regular Camera!").c_str()); 267 | } else { 268 | double camera_left_undistort_ts_diff = image_message->header.stamp.toSec() - prev_camera_left_undistort_ts; 269 | if (camera_left_undistort_ts_diff < (1 - TS_DIFF_THLD) * CAM_PERIOD || camera_left_undistort_ts_diff > (1 + TS_DIFF_THLD) * CAM_PERIOD) 270 | ROS_WARN("%s", colorful_char::warning("Right Image stream detects an unusual frequency!").c_str()); 271 | if (camera_left_undistort_ts_diff < 0) 272 | ROS_WARN("%s", colorful_char::warning("Left Regular Camera's depth stream detects a chronological error!").c_str()); 273 | prev_camera_left_undistort_ts = image_message->header.stamp.toSec(); 274 | } 275 | } 276 | 277 | void CameraRightUndistotrtHandler(const Image::ConstPtr & image_message) { 278 | if (!receive_camera_right_undistort) { 279 | receive_camera_right_undistort = true; 280 | prev_camera_right_undistort_ts = image_message->header.stamp.toSec(); 281 | ROS_INFO("%s", colorful_char::info("Receives undistorted data from the Right Regular Camera!").c_str()); 282 | } else { 283 | double camera_right_undistort_ts_diff = image_message->header.stamp.toSec() - prev_camera_right_undistort_ts; 284 | if (camera_right_undistort_ts_diff < (1 - TS_DIFF_THLD) * CAM_PERIOD 285 | || camera_right_undistort_ts_diff > (1 + TS_DIFF_THLD) * CAM_PERIOD) 286 | ROS_WARN("%s", colorful_char::warning("Right Image stream detects an unusual frequency!").c_str()); 287 | if (camera_right_undistort_ts_diff < 0) 288 | ROS_WARN("%s", colorful_char::warning("Right Regular Camera's depth stream detects a chronological error!").c_str()); 289 | prev_camera_right_undistort_ts = image_message->header.stamp.toSec(); 290 | } 291 | } 292 | 293 | // ------------------------------------------------------------------------- // 294 | 295 | int main(int argc, char ** argv) { 296 | ros::init(argc, argv, "data_validator"); 297 | ros::NodeHandle nh; 298 | 299 | std::string imu_topic, event_left_topic, event_right_topic, camera_left_topic, camera_right_topic, kinect_color_topic, kinect_depth_topic, 300 | lidar_topic, gt_topic, event_left_depth_topic, event_right_depth_topic, camera_left_depth_topic, camera_right_depth_topic, 301 | camera_left_undistort_topic, camera_right_undistort_topic; 302 | ros::param::get("/imu_topic", imu_topic); 303 | ros::param::get("/event_left_topic", event_left_topic); 304 | ros::param::get("/event_right_topic", event_right_topic); 305 | ros::param::get("/camera_left_topic", camera_left_topic); 306 | ros::param::get("/camera_right_topic", camera_right_topic); 307 | ros::param::get("/kinect_color_topic", kinect_color_topic); 308 | ros::param::get("/kinect_depth_topic", kinect_depth_topic); 309 | ros::param::get("/lidar_topic", lidar_topic); 310 | ros::param::get("/ground_truth_topic", gt_topic); 311 | ros::param::get("/event_left_depth_topic", event_left_depth_topic); 312 | ros::param::get("/event_right_depth_topic", event_right_depth_topic); 313 | ros::param::get("/camera_left_depth_topic", camera_left_depth_topic); 314 | ros::param::get("/camera_right_depth_topic", camera_right_depth_topic); 315 | ros::param::get("/camera_left_undistort_topic", camera_left_undistort_topic); 316 | ros::param::get("/camera_right_undistort_topic", camera_right_undistort_topic); 317 | 318 | ros::Subscriber sub_imu = nh.subscribe(imu_topic, 1000, &ImuHandler); 319 | ros::Subscriber sub_event_left = nh.subscribe(event_left_topic, 100000, &EventLeftHandler); 320 | ros::Subscriber sub_event_right = nh.subscribe(event_right_topic, 100000, &EventRightHandler); 321 | ros::Subscriber sub_camera_left = nh.subscribe(camera_left_topic, 1000, &CameraLeftHandler); 322 | ros::Subscriber sub_camera_right = nh.subscribe(camera_right_topic, 1000, &CameraRightHandler); 323 | ros::Subscriber sub_kinect_color = nh.subscribe(kinect_color_topic, 1000, &KinectColorHandler); 324 | ros::Subscriber sub_kinect_depth = nh.subscribe(kinect_depth_topic, 1000, &KinectDepthHandler); 325 | ros::Subscriber sub_lidar = nh.subscribe(lidar_topic, 1000, &LidarHandler); 326 | ros::Subscriber sub_gt = nh.subscribe(gt_topic, 1000, &GroundTruthHandler); 327 | ros::Subscriber sub_event_left_depth = nh.subscribe(event_left_depth_topic, 1000, &EventLeftDepthHandler); 328 | ros::Subscriber sub_event_right_depth = nh.subscribe(event_right_depth_topic, 1000, &EventRightDepthHandler); 329 | ros::Subscriber sub_camera_left_depth = nh.subscribe(camera_left_depth_topic, 1000, &CameraLeftDepthHandler); 330 | ros::Subscriber sub_camera_right_depth = nh.subscribe(camera_right_depth_topic, 1000, &CameraRightDepthHandler); 331 | ros::Subscriber sub_camera_left_undistort = nh.subscribe(camera_left_undistort_topic, 1000, &CameraLeftUndistotrtHandler); 332 | ros::Subscriber sub_camera_right_undistort = nh.subscribe(camera_right_undistort_topic, 1000, &CameraRightUndistotrtHandler); 333 | ros::spin(); 334 | 335 | // By cutting off the first and last X seconds, calculate the Mean Event Rate for each event streaem. 336 | if (!ros::ok() && receive_event_left) { 337 | double event_left_start_ts = event_left_ts.front() + EVENT_SKIP_TS; 338 | double event_left_end_ts = event_left_ts.back() - EVENT_SKIP_TS; 339 | double event_left_duration = event_left_end_ts - event_left_start_ts; 340 | double event_left_total_size = 0; 341 | for (size_t idx = 0; idx < event_left_ts.size(); ++idx) 342 | if (event_left_ts[idx] >= event_left_start_ts && event_left_ts[idx] <= event_left_end_ts) 343 | event_left_total_size += event_left_size[idx]; 344 | std::cout << colorful_char::info("The Mean Event Rate for the Left Event stream is " 345 | + std::to_string(event_left_total_size / event_left_duration / 1000000) 346 | + " million events per second.") 347 | << std::endl; 348 | } 349 | if (!ros::ok() && receive_event_right) { 350 | double event_right_start_ts = event_right_ts.front() + EVENT_SKIP_TS; 351 | double event_right_end_ts = event_right_ts.back() - EVENT_SKIP_TS; 352 | double event_right_duration = event_right_end_ts - event_right_start_ts; 353 | double event_right_total_size = 0; 354 | for (size_t idx = 0; idx < event_right_ts.size(); ++idx) { 355 | if (event_right_ts[idx] >= event_right_start_ts && event_right_ts[idx] <= event_right_end_ts) 356 | event_right_total_size += event_right_size[idx]; 357 | } 358 | std::cout << colorful_char::info("The Mean Event Rate for the Right Event stream is " 359 | + std::to_string(event_right_total_size / event_right_duration / 1000000) 360 | + " million events per second.") 361 | << std::endl; 362 | } 363 | 364 | ros::shutdown(); 365 | return 0; 366 | } 367 | -------------------------------------------------------------------------------- /src/event_visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // ------------------------------------------------------------------------- // 7 | 8 | std::mutex mtx1, mtx2; 9 | std::vector event_buffer1, event_buffer2; 10 | int cam1_width, cam1_height, cam2_width, cam2_height; 11 | 12 | // ------------------------------------------------------------------------- // 13 | 14 | void Event1Callback(const EventArray::ConstPtr & message, const std::string & topic_name) { 15 | ROS_INFO_ONCE("%s", colorful_char::info("Receive event messages under topic name: " + topic_name).c_str()); 16 | mtx1.lock(); 17 | for (const Event & event : message->events) event_buffer1.emplace_back(event); 18 | mtx1.unlock(); 19 | cam1_width = message->width; 20 | cam1_height = message->height; 21 | } 22 | 23 | void Event2Callback(const EventArray::ConstPtr & message, const std::string & topic_name) { 24 | ROS_INFO_ONCE("%s", colorful_char::info("Receive event messages under topic name: " + topic_name).c_str()); 25 | mtx2.lock(); 26 | for (const Event & event : message->events) event_buffer2.emplace_back(event); 27 | mtx2.unlock(); 28 | cam2_width = message->width; 29 | cam2_height = message->height; 30 | } 31 | 32 | // ------------------------------------------------------------------------- // 33 | 34 | int main(int argc, char ** argv) { 35 | ros::init(argc, argv, "event_visualizer"); 36 | ros::NodeHandle nh; 37 | 38 | // Read and validate the input parameters from config. 39 | std::vector in_topics, out_topics; 40 | int out_freq; 41 | ros::param::get("in_event_stream_topic", in_topics); 42 | ros::param::get("out_event_frame_topic", out_topics); 43 | ros::param::get("out_frequency", out_freq); 44 | if (in_topics.size() != out_topics.size()) { 45 | ROS_ERROR("%s", colorful_char::error("The in and out topic number do not match with each other.").c_str()); 46 | ros::shutdown(); 47 | return -1; 48 | } else if (in_topics.size() > 2) { 49 | ROS_ERROR("%s", colorful_char::error("The current version does not support more than two event cameras.").c_str()); 50 | ros::shutdown(); 51 | return -1; 52 | } 53 | int cam_num = in_topics.size(); 54 | 55 | // Set up the subscriber and publisher according to the sensor setup (monocular or stereo). 56 | ros::Subscriber sub1, sub2; 57 | ros::Publisher pub1, pub2; 58 | for (size_t idx = 0; idx < cam_num; ++idx) { 59 | if (idx == 0) { 60 | sub1 = nh.subscribe(in_topics[idx], 100000, boost::bind(Event1Callback, _1, in_topics[idx])); 61 | pub1 = nh.advertise(out_topics[idx], out_freq); 62 | } else if (idx == 1) { 63 | sub2 = nh.subscribe(in_topics[idx], 100000, boost::bind(Event2Callback, _1, in_topics[idx])); 64 | pub2 = nh.advertise(out_topics[idx], out_freq); 65 | } 66 | } 67 | 68 | ros::Rate rate(out_freq); 69 | while (ros::ok()) { 70 | // Swap the events from the buffer to a temporary holder. 71 | std::vector event_holder1, event_holder2; 72 | mtx1.lock(); 73 | event_holder1.swap(event_buffer1); 74 | mtx1.unlock(); 75 | if (cam_num == 2) { 76 | mtx2.lock(); 77 | event_holder2.swap(event_buffer2); 78 | mtx2.unlock(); 79 | } 80 | if ((cam_num == 1 && event_holder1.size() == 0) || (cam_num == 2 && (event_holder1.size() == 0 || event_holder2.size() == 0))) { 81 | rate.sleep(); 82 | ros::spinOnce(); 83 | continue; 84 | } 85 | 86 | // Accumulate events onto the image canvas. Note that, only the last event will be displayed on the canvas with its polarity. 87 | cv_bridge::CvImage event_frame1, event_frame2; 88 | event_frame1.image = cv::Mat(cam1_height, cam1_width, CV_8UC3, cv::Vec3b(255, 255, 255)); 89 | for (auto & event : event_holder1) { 90 | if (event.polarity) event_frame1.image.at(event.y, event.x) = cv::Vec3b(255, 0, 0); // positive - blue 91 | else 92 | event_frame1.image.at(event.y, event.x) = cv::Vec3b(0, 0, 255); // negative - red 93 | } 94 | if (cam_num == 2) { 95 | event_frame2.image = cv::Mat(cam2_height, cam2_width, CV_8UC3, cv::Vec3b(255, 255, 255)); 96 | for (auto & event : event_holder2) { 97 | if (event.polarity) event_frame2.image.at(event.y, event.x) = cv::Vec3b(255, 0, 0); 98 | else 99 | event_frame2.image.at(event.y, event.x) = cv::Vec3b(0, 0, 255); 100 | } 101 | } 102 | 103 | // Publish the accumulated event frame as image. 104 | event_frame1.encoding = "bgr8"; 105 | event_frame1.header.stamp = ros::Time::now(); 106 | pub1.publish(event_frame1.toImageMsg()); 107 | if (cam_num == 2) { 108 | event_frame2.encoding = "bgr8"; 109 | event_frame2.header.stamp = event_frame1.header.stamp; 110 | pub2.publish(event_frame2.toImageMsg()); 111 | } 112 | 113 | rate.sleep(); 114 | ros::spinOnce(); 115 | } 116 | ros::shutdown(); 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /src/timeline_reconstructor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define EVENT_DENSE_THLD 0.00100 // [s] 6 | #define EVENT_JUMP_THLD 0.00001 // [s] 7 | #define EVENT_JUMP_TS 0.00500 // [s] 8 | 9 | // ------------------------------------------------------------------------- // 10 | 11 | // Sensor: Before Trigger --> SYNC --> After Trigger 12 | // IMU NA 200Hz NA 13 | // Camera 5Hz 30Hz 5Hz 14 | // KINECT NA 30Hz 30Hz 15 | // LIDAR Fake 1Hz 10Hz Fake 1Hz 16 | // Event NA ? Operates in another 20ms 17 | 18 | int main(int argc, char ** argv) { 19 | ros::init(argc, argv, "timeline_reconstructor"); 20 | ros::NodeHandle nh; 21 | 22 | std::string bag_in_path, gt_in_path, in_imu_topic, in_event_left_topic, in_event_right_topic, in_camera_left_topic, in_camera_right_topic, 23 | in_kinect_color_topic, in_kinect_depth_topic, in_lidar_topic, out_imu_topic, out_event_left_topic, out_event_right_topic, 24 | out_camera_left_topic, out_camera_right_topic, out_kinect_color_topic, out_kinect_depth_topic, out_lidar_topic, out_gt_topic; 25 | ros::param::get("/timeline_reconstructor/bag_path", bag_in_path); 26 | ros::param::get("/timeline_reconstructor/gt_path", gt_in_path); 27 | ros::param::get("/timeline_reconstructor/in_imu_topic", in_imu_topic); 28 | ros::param::get("/timeline_reconstructor/in_event_left_topic", in_event_left_topic); 29 | ros::param::get("/timeline_reconstructor/in_event_right_topic", in_event_right_topic); 30 | ros::param::get("/timeline_reconstructor/in_camera_left_topic", in_camera_left_topic); 31 | ros::param::get("/timeline_reconstructor/in_camera_right_topic", in_camera_right_topic); 32 | ros::param::get("/timeline_reconstructor/in_kinect_color_topic", in_kinect_color_topic); 33 | ros::param::get("/timeline_reconstructor/in_kinect_depth_topic", in_kinect_depth_topic); 34 | ros::param::get("/timeline_reconstructor/in_lidar_topic", in_lidar_topic); 35 | ros::param::get("/timeline_reconstructor/imu_topic", out_imu_topic); 36 | ros::param::get("/timeline_reconstructor/event_left_topic", out_event_left_topic); 37 | ros::param::get("/timeline_reconstructor/event_right_topic", out_event_right_topic); 38 | ros::param::get("/timeline_reconstructor/camera_left_topic", out_camera_left_topic); 39 | ros::param::get("/timeline_reconstructor/camera_right_topic", out_camera_right_topic); 40 | ros::param::get("/timeline_reconstructor/kinect_color_topic", out_kinect_color_topic); 41 | ros::param::get("/timeline_reconstructor/kinect_depth_topic", out_kinect_depth_topic); 42 | ros::param::get("/timeline_reconstructor/lidar_topic", out_lidar_topic); 43 | ros::param::get("/timeline_reconstructor/ground_truth_topic", out_gt_topic); 44 | std::string bag_out_path = bag_in_path.substr(0, bag_in_path.size() - 4) + ".synced.bag"; 45 | 46 | bool receive_gt = false; 47 | std::deque gt_buffer; 48 | if (gt_in_path != "None") { 49 | std::ifstream gt_file; 50 | gt_file.open(gt_in_path); 51 | if (!gt_file.is_open()) { 52 | ROS_ERROR("%s", colorful_char::error("Found no Ground Truth file under the path: " + gt_in_path).c_str()); 53 | ros::shutdown(); 54 | return -1; 55 | } 56 | receive_gt = true; 57 | u_int32_t gt_seq = 0; 58 | std::string input_line; 59 | // Ground Truth in OptiTrack format. 60 | for (size_t idx = 0; idx < 7; ++idx) std::getline(gt_file, input_line); 61 | while (std::getline(gt_file, input_line)) { 62 | std::vector input_data(9, ""); 63 | std::istringstream read_data(input_line); 64 | for (size_t idx = 0; idx < 9; ++idx) std::getline(read_data, input_data[idx], ','); 65 | gt_buffer.emplace_back(boost::make_shared()); 66 | gt_buffer.back()->header.stamp = ros::Time().fromSec(gt_seq * GT_PERIOD); // Make sure GT_PERIOD = 0.100000000 67 | gt_buffer.back()->header.seq = gt_seq++; 68 | gt_buffer.back()->pose.orientation.x = atof(input_data[2].c_str()); 69 | gt_buffer.back()->pose.orientation.y = atof(input_data[3].c_str()); 70 | gt_buffer.back()->pose.orientation.z = atof(input_data[4].c_str()); 71 | gt_buffer.back()->pose.orientation.w = atof(input_data[5].c_str()); 72 | gt_buffer.back()->pose.position.x = atof(input_data[6].c_str()); 73 | gt_buffer.back()->pose.position.y = atof(input_data[7].c_str()); 74 | gt_buffer.back()->pose.position.z = atof(input_data[8].c_str()); 75 | } 76 | // Ground Truth in TUM format. 77 | // while (std::getline(gt_file, input_line)) { 78 | // std::vector input_data(8, ""); 79 | // std::istringstream read_data(input_line); 80 | // for (size_t idx = 0; idx < 8; ++idx) std::getline(read_data, input_data[idx], ' '); 81 | // gt_buffer.emplace_back(boost::make_shared()); 82 | // gt_buffer.back()->header.stamp = ros::Time().fromSec(gt_seq * GT_PERIOD); // Make sure GT_PERIOD = 0.008333333 83 | // gt_buffer.back()->header.seq = gt_seq++; 84 | // gt_buffer.back()->pose.orientation.x = atof(input_data[4].c_str()); 85 | // gt_buffer.back()->pose.orientation.y = atof(input_data[5].c_str()); 86 | // gt_buffer.back()->pose.orientation.z = atof(input_data[6].c_str()); 87 | // gt_buffer.back()->pose.orientation.w = atof(input_data[7].c_str()); 88 | // gt_buffer.back()->pose.position.x = atof(input_data[1].c_str()); 89 | // gt_buffer.back()->pose.position.y = atof(input_data[2].c_str()); 90 | // gt_buffer.back()->pose.position.z = atof(input_data[3].c_str()); 91 | // } 92 | ROS_INFO("%s", colorful_char::info("Successfully load the Ground Truth file!").c_str()); 93 | } 94 | 95 | rosbag::Bag bag_in, bag_out; 96 | bag_in.open(bag_in_path, rosbag::bagmode::Read); 97 | bag_out.open(bag_out_path, rosbag::bagmode::Write); 98 | rosbag::View view(bag_in); 99 | uint32_t num_msg = view.size(); 100 | 101 | bool receive_imu = false; 102 | bool receive_event_left = false; 103 | bool receive_event_right = false; 104 | bool receive_camera_left = false; 105 | bool receive_camera_right = false; 106 | bool receive_kinect_color = false; 107 | bool receive_kinect_depth = false; 108 | bool receive_lidar = false; 109 | 110 | std::deque imu_ts_buffer; 111 | std::deque event_left_ts_counter; 112 | std::deque event_right_ts_counter; 113 | std::deque> camera_left_ts_buffer; 114 | std::deque> camera_right_ts_buffer; 115 | std::vector kinect_color_ts_buffer; 116 | std::vector kinect_depth_ts_buffer; 117 | std::deque> lidar_ts_buffer; 118 | kinect_color_ts_buffer.reserve(num_msg); 119 | kinect_depth_ts_buffer.reserve(num_msg); 120 | for (const rosbag::MessageInstance & msg : view) { 121 | if (msg.getTopic() == in_imu_topic) { 122 | receive_imu = true; 123 | imu_ts_buffer.emplace_back(msg.instantiate()->header.stamp); 124 | } else if (msg.getTopic() == in_event_left_topic) { 125 | receive_event_left = true; 126 | event_left_ts_counter.emplace_back(true); 127 | } else if (msg.getTopic() == in_event_right_topic) { 128 | receive_event_right = true; 129 | event_right_ts_counter.emplace_back(true); 130 | } else if (msg.getTopic() == in_camera_left_topic) { 131 | receive_camera_left = true; 132 | camera_left_ts_buffer.emplace_back(msg.instantiate()->header.stamp, false); 133 | if (camera_left_ts_buffer.size() != 1) { 134 | size_t last_idx = camera_left_ts_buffer.size() - 1; 135 | if (camera_left_ts_buffer[last_idx].first.toSec() - camera_left_ts_buffer[last_idx - 1].first.toSec() < CAM_PERIOD * 1.5) { 136 | camera_left_ts_buffer[last_idx].second = true; 137 | camera_left_ts_buffer[last_idx - 1].second = true; 138 | } 139 | } 140 | } else if (msg.getTopic() == in_camera_right_topic) { 141 | receive_camera_right = true; 142 | camera_right_ts_buffer.emplace_back(msg.instantiate()->header.stamp, false); 143 | if (camera_right_ts_buffer.size() != 1) { 144 | size_t last_idx = camera_right_ts_buffer.size() - 1; 145 | if (camera_right_ts_buffer[last_idx].first.toSec() - camera_right_ts_buffer[last_idx - 1].first.toSec() < CAM_PERIOD * 1.5) { 146 | camera_right_ts_buffer[last_idx].second = true; 147 | camera_right_ts_buffer[last_idx - 1].second = true; 148 | } 149 | } 150 | } else if (msg.getTopic() == in_kinect_color_topic) { 151 | receive_kinect_color = true; 152 | kinect_color_ts_buffer.emplace_back(msg.instantiate()->header.stamp); 153 | } else if (msg.getTopic() == in_kinect_depth_topic) { 154 | receive_kinect_depth = true; 155 | kinect_depth_ts_buffer.emplace_back(msg.instantiate()->header.stamp); 156 | } else if (msg.getTopic() == in_lidar_topic) { 157 | receive_lidar = true; 158 | lidar_ts_buffer.emplace_back(msg.instantiate()->header.stamp, false); 159 | if (lidar_ts_buffer.size() != 1) { 160 | size_t last_idx = lidar_ts_buffer.size() - 1; 161 | if (lidar_ts_buffer[last_idx].first.toSec() - lidar_ts_buffer[last_idx - 1].first.toSec() < LIDAR_PERIOD * 1.5) { 162 | lidar_ts_buffer[last_idx].second = true; 163 | lidar_ts_buffer[last_idx - 1].second = true; 164 | } 165 | } 166 | } 167 | } 168 | ROS_INFO("%s", colorful_char::info("Successfully load the rosbag!").c_str()); 169 | 170 | double start_ts = imu_ts_buffer.front().toSec(); 171 | size_t imu_msg_idx = 0; 172 | size_t camera_left_msg_idx = 0; 173 | size_t camera_right_msg_idx = 0; 174 | size_t kinect_color_msg_idx = 0; 175 | size_t kinect_color_lost_frames = 1; 176 | double kinect_color_msg_prev_ts = 0; 177 | size_t kinect_depth_msg_idx = 0; 178 | size_t kinect_depth_lost_frames = 1; 179 | double kinect_depth_msg_prev_ts = 0; 180 | double lidar_first_msg_ts = 0; 181 | for (ros::Time & time : imu_ts_buffer) time.fromSec(start_ts + (imu_msg_idx++) * IMU_PERIOD); 182 | for (auto & pair : camera_left_ts_buffer) 183 | if (pair.second) pair.first.fromSec(start_ts + (camera_left_msg_idx++) * CAM_PERIOD); 184 | for (auto & pair : camera_right_ts_buffer) 185 | if (pair.second) pair.first.fromSec(start_ts + (camera_right_msg_idx++) * CAM_PERIOD); 186 | for (size_t idx = 0; idx < kinect_color_ts_buffer.size(); ++idx) { 187 | if (kinect_color_msg_idx != 0) { 188 | double ts_diff = kinect_color_ts_buffer[idx].toSec() - kinect_color_msg_prev_ts; 189 | if (ts_diff > 1.75 * KINECT_PERIOD) { 190 | if (ts_diff > 3.75 * KINECT_PERIOD) { 191 | kinect_color_lost_frames += 3; 192 | ROS_ERROR("%s", colorful_char::error("Kinect Color stream lost three consecutive messages! Unreliable data sequence!").c_str()); 193 | ROS_ERROR("%s", colorful_char::error("Timeline reconstruction shut down! Please re-record another bag instead!").c_str()); 194 | ros::shutdown(); 195 | return -1; 196 | } else if (ts_diff > 2.75 * KINECT_PERIOD) { 197 | kinect_color_lost_frames += 2; 198 | ROS_WARN("%s", colorful_char::warning("Kinect Color stream lost two consecutive messages! Not recommend!").c_str()); 199 | } else if (idx == kinect_color_ts_buffer.size() - 1 200 | || kinect_color_ts_buffer[idx + 1].toSec() - kinect_color_ts_buffer[idx].toSec() > 0.5 * KINECT_PERIOD) { 201 | kinect_color_lost_frames += 1; 202 | ROS_WARN("%s", colorful_char::warning("Kinect Color stream lost one message!").c_str()); 203 | } 204 | } 205 | } 206 | kinect_color_msg_prev_ts = kinect_color_ts_buffer[idx].toSec(); 207 | kinect_color_ts_buffer[idx].fromSec(start_ts + (kinect_color_lost_frames + kinect_color_msg_idx++) * KINECT_PERIOD); 208 | } 209 | for (size_t idx = 0; idx < kinect_depth_ts_buffer.size(); ++idx) { 210 | if (kinect_depth_msg_idx != 0) { 211 | double ts_diff = kinect_depth_ts_buffer[idx].toSec() - kinect_depth_msg_prev_ts; 212 | if (ts_diff > 1.75 * KINECT_PERIOD) { 213 | if (ts_diff > 3.75 * KINECT_PERIOD) { 214 | kinect_depth_lost_frames += 3; 215 | ROS_ERROR("%s", colorful_char::error("Kinect Depth stream lost three consecutive messages! Unreliable data sequence!").c_str()); 216 | ROS_ERROR("%s", colorful_char::error("Timeline reconstruction shut down! Please re-record another bag instead!").c_str()); 217 | ros::shutdown(); 218 | return -1; 219 | } else if (ts_diff > 2.75 * KINECT_PERIOD) { 220 | kinect_depth_lost_frames += 2; 221 | ROS_WARN("%s", colorful_char::warning("Kinect Depth stream lost two consecutive messages! Not recommend!").c_str()); 222 | } else if (idx == kinect_depth_ts_buffer.size() - 1 223 | || kinect_depth_ts_buffer[idx + 1].toSec() - kinect_depth_ts_buffer[idx].toSec() > 0.5 * KINECT_PERIOD) { 224 | kinect_depth_lost_frames += 1; 225 | ROS_WARN("%s", colorful_char::warning("Kinect Depth stream lost one message!").c_str()); 226 | } 227 | } 228 | } 229 | kinect_depth_msg_prev_ts = kinect_depth_ts_buffer[idx].toSec(); 230 | kinect_depth_ts_buffer[idx].fromSec(start_ts + (kinect_depth_lost_frames + kinect_depth_msg_idx++) * KINECT_PERIOD); 231 | } 232 | for (auto & pair : lidar_ts_buffer) { 233 | if (pair.second) { 234 | if (lidar_first_msg_ts == 0) lidar_first_msg_ts = pair.first.toSec(); 235 | pair.first.fromSec(start_ts + pair.first.toSec() - lidar_first_msg_ts); 236 | } 237 | } 238 | for (auto & msg : gt_buffer) msg->header.stamp.fromSec(start_ts + msg->header.stamp.toSec()); 239 | ROS_INFO("%s", colorful_char::info("Successfully reconstruct the ros message timeline!").c_str()); 240 | 241 | std::deque imu_buffer; 242 | std::deque event_left_buffer; 243 | std::deque event_right_buffer; 244 | std::deque camera_left_buffer; 245 | std::deque camera_right_buffer; 246 | std::deque kinect_color_buffer; 247 | std::deque kinect_depth_buffer; 248 | std::deque lidar_buffer; 249 | 250 | bool is_event_left_dense = false; 251 | double prev_event_left_ts = 0; 252 | double prev_event_left_trigger_ts = 0; 253 | double event_left_ts_offset = 0; 254 | bool is_event_right_dense = false; 255 | double prev_event_right_ts = 0; 256 | double prev_event_right_trigger_ts = 0; 257 | double event_right_ts_offset = 0; 258 | size_t kinect_color_ts_idx = 0; 259 | size_t kinect_depth_ts_idx = 0; 260 | for (const rosbag::MessageInstance & msg : view) { 261 | if (msg.getTopic() == in_imu_topic) imu_buffer.emplace_back(msg.instantiate()); 262 | else if (msg.getTopic() == in_event_left_topic) { 263 | event_left_buffer.emplace_back(msg.instantiate()); 264 | for (auto & event : event_left_buffer.back()->events) { 265 | if (is_event_left_dense) { 266 | if (event.ts.toSec() >= prev_event_left_trigger_ts + (EVENT_JUMP_TS - EVENT_JUMP_THLD) 267 | && event.ts.toSec() <= prev_event_left_trigger_ts + (EVENT_JUMP_TS + EVENT_JUMP_THLD)) { 268 | event_left_ts_offset -= EVENT_JUMP_TS; 269 | ROS_WARN("%s", colorful_char::warning("Left Event stream receives two consecutive triggers!").c_str()); 270 | ROS_WARN("%s", colorful_char::warning("Left Event stream delay = " + std::to_string(event_left_ts_offset) + "s").c_str()); 271 | } 272 | if (event.ts.toSec() <= prev_event_left_trigger_ts - (EVENT_JUMP_TS - EVENT_JUMP_THLD) 273 | && event.ts.toSec() >= prev_event_left_trigger_ts - (EVENT_JUMP_TS + EVENT_JUMP_THLD)) { 274 | event_left_ts_offset += EVENT_JUMP_TS; 275 | ROS_WARN("%s", colorful_char::warning("Left Event stream receives no trigger within a short time window!").c_str()); 276 | ROS_WARN("%s", colorful_char::warning("Left Event stream delay = " + std::to_string(event_left_ts_offset) + "s").c_str()); 277 | } 278 | } 279 | prev_event_left_trigger_ts = event.ts.toSec(); 280 | event.ts.fromSec(event.ts.toSec() + event_left_ts_offset); 281 | if (event.ts.toSec() < prev_event_left_ts) event.ts.fromSec(prev_event_left_ts); 282 | is_event_left_dense = (event.ts.toSec() < prev_event_left_ts + EVENT_DENSE_THLD) ? true : false; 283 | prev_event_left_ts = event.ts.toSec(); 284 | event.ts.fromSec(start_ts + event.ts.toSec()); 285 | } 286 | event_left_buffer.back()->header.stamp.fromSec(event_left_buffer.back()->events.front().ts.toSec()); 287 | } else if (msg.getTopic() == in_event_right_topic) { 288 | event_right_buffer.emplace_back(msg.instantiate()); 289 | for (auto & event : event_right_buffer.back()->events) { 290 | if (is_event_right_dense) { 291 | if (event.ts.toSec() >= prev_event_right_trigger_ts + (EVENT_JUMP_TS - EVENT_JUMP_THLD) 292 | && event.ts.toSec() <= prev_event_right_trigger_ts + (EVENT_JUMP_TS + EVENT_JUMP_THLD)) { 293 | event_right_ts_offset -= EVENT_JUMP_TS; 294 | ROS_WARN("%s", colorful_char::warning("Right Event stream receives two consecutive triggers!").c_str()); 295 | ROS_WARN("%s", colorful_char::warning("Right Event stream delay = " + std::to_string(event_right_ts_offset) + "s").c_str()); 296 | } 297 | if (event.ts.toSec() <= prev_event_right_trigger_ts - (EVENT_JUMP_TS - EVENT_JUMP_THLD) 298 | && event.ts.toSec() >= prev_event_right_trigger_ts - (EVENT_JUMP_TS + EVENT_JUMP_THLD)) { 299 | event_right_ts_offset += EVENT_JUMP_TS; 300 | ROS_WARN("%s", colorful_char::warning("Right Event stream receives no trigger within a short time window!").c_str()); 301 | ROS_WARN("%s", colorful_char::warning("Right Event stream delay = " + std::to_string(event_right_ts_offset) + "s").c_str()); 302 | } 303 | } 304 | prev_event_right_trigger_ts = event.ts.toSec(); 305 | event.ts.fromSec(event.ts.toSec() + event_right_ts_offset); 306 | if (event.ts.toSec() < prev_event_right_ts) event.ts.fromSec(prev_event_right_ts); 307 | is_event_right_dense = (event.ts.toSec() < prev_event_right_ts + EVENT_DENSE_THLD) ? true : false; 308 | prev_event_right_ts = event.ts.toSec(); 309 | event.ts.fromSec(start_ts + event.ts.toSec()); 310 | } 311 | event_right_buffer.back()->header.stamp.fromSec(event_right_buffer.back()->events.front().ts.toSec()); 312 | } else if (msg.getTopic() == in_camera_left_topic) { 313 | if (!camera_left_ts_buffer.front().second) camera_left_ts_buffer.pop_front(); 314 | else 315 | camera_left_buffer.emplace_back(msg.instantiate()); 316 | } else if (msg.getTopic() == in_camera_right_topic) { 317 | if (!camera_right_ts_buffer.front().second) camera_right_ts_buffer.pop_front(); 318 | else 319 | camera_right_buffer.emplace_back(msg.instantiate()); 320 | } else if (msg.getTopic() == in_kinect_color_topic) 321 | kinect_color_buffer.emplace_back(msg.instantiate()); 322 | else if (msg.getTopic() == in_kinect_depth_topic) 323 | kinect_depth_buffer.emplace_back(msg.instantiate()); 324 | else if (msg.getTopic() == in_lidar_topic) { 325 | if (!lidar_ts_buffer.front().second) lidar_ts_buffer.pop_front(); 326 | else 327 | lidar_buffer.emplace_back(msg.instantiate()); 328 | } 329 | 330 | while (receive_imu) { 331 | int earliest_topic = 0; 332 | double earliest_ts = DBL_MAX; 333 | if (receive_imu) { 334 | if (imu_buffer.size() == 0) break; 335 | else if (earliest_ts > imu_ts_buffer.front().toSec()) { 336 | earliest_topic = 1; 337 | earliest_ts = imu_ts_buffer.front().toSec(); 338 | } 339 | } 340 | if (receive_event_left) { 341 | if (event_left_buffer.size() == 0) break; 342 | else if (earliest_ts > event_left_buffer.front()->header.stamp.toSec()) { 343 | earliest_topic = 2; 344 | earliest_ts = event_left_buffer.front()->header.stamp.toSec(); 345 | } 346 | } 347 | if (receive_event_right) { 348 | if (event_right_buffer.size() == 0) break; 349 | else if (earliest_ts > event_right_buffer.front()->header.stamp.toSec()) { 350 | earliest_topic = 3; 351 | earliest_ts = event_right_buffer.front()->header.stamp.toSec(); 352 | } 353 | } 354 | if (receive_camera_left) { 355 | if (camera_left_buffer.size() == 0) break; 356 | else if (earliest_ts > camera_left_ts_buffer.front().first.toSec()) { 357 | earliest_topic = 4; 358 | earliest_ts = camera_left_ts_buffer.front().first.toSec(); 359 | } 360 | } 361 | if (receive_camera_right) { 362 | if (camera_right_buffer.size() == 0) break; 363 | else if (earliest_ts > camera_right_ts_buffer.front().first.toSec()) { 364 | earliest_topic = 5; 365 | earliest_ts = camera_right_ts_buffer.front().first.toSec(); 366 | } 367 | } 368 | if (receive_kinect_color) { 369 | if (kinect_color_buffer.size() == 0) break; 370 | else if (earliest_ts > kinect_color_ts_buffer[kinect_color_ts_idx].toSec()) { 371 | earliest_topic = 6; 372 | earliest_ts = kinect_color_ts_buffer[kinect_color_ts_idx].toSec(); 373 | } 374 | } 375 | if (receive_kinect_depth) { 376 | if (kinect_depth_buffer.size() == 0) break; 377 | else if (earliest_ts > kinect_depth_ts_buffer[kinect_depth_ts_idx].toSec()) { 378 | earliest_topic = 7; 379 | earliest_ts = kinect_depth_ts_buffer[kinect_depth_ts_idx].toSec(); 380 | } 381 | } 382 | if (receive_lidar) { 383 | if (lidar_buffer.size() == 0) break; 384 | else if (earliest_ts > lidar_ts_buffer.front().first.toSec()) { 385 | earliest_topic = 8; 386 | earliest_ts = lidar_ts_buffer.front().first.toSec(); 387 | } 388 | } 389 | if (receive_gt) { 390 | if (gt_buffer.size() == 0) break; 391 | else if (earliest_ts > gt_buffer.front()->header.stamp.toSec()) { 392 | earliest_topic = 9; 393 | earliest_ts = gt_buffer.front()->header.stamp.toSec(); 394 | } 395 | } 396 | 397 | switch (earliest_topic) { 398 | case 1 : 399 | imu_buffer.front()->header.stamp.fromSec(imu_ts_buffer.front().toSec()); 400 | bag_out.write(out_imu_topic, imu_ts_buffer.front(), imu_buffer.front()); 401 | imu_buffer.pop_front(); 402 | imu_ts_buffer.pop_front(); 403 | if (imu_ts_buffer.size() == 0) receive_imu = false; 404 | break; 405 | case 2 : 406 | bag_out.write(out_event_left_topic, event_left_buffer.front()->header.stamp, event_left_buffer.front()); 407 | event_left_buffer.pop_front(); 408 | event_left_ts_counter.pop_front(); 409 | if (event_left_ts_counter.size() == 0) receive_event_left = false; 410 | break; 411 | case 3 : 412 | bag_out.write(out_event_right_topic, event_right_buffer.front()->header.stamp, event_right_buffer.front()); 413 | event_right_buffer.pop_front(); 414 | event_right_ts_counter.pop_front(); 415 | if (event_right_ts_counter.size() == 0) receive_event_right = false; 416 | break; 417 | case 4 : 418 | camera_left_buffer.front()->header.stamp.fromSec(camera_left_ts_buffer.front().first.toSec()); 419 | bag_out.write(out_camera_left_topic, camera_left_ts_buffer.front().first, camera_left_buffer.front()); 420 | camera_left_buffer.pop_front(); 421 | camera_left_ts_buffer.pop_front(); 422 | if (camera_left_ts_buffer.size() == 0 || !camera_left_ts_buffer.front().second) receive_camera_left = false; 423 | break; 424 | case 5 : 425 | camera_right_buffer.front()->header.stamp.fromSec(camera_right_ts_buffer.front().first.toSec()); 426 | bag_out.write(out_camera_right_topic, camera_right_ts_buffer.front().first, camera_right_buffer.front()); 427 | camera_right_buffer.pop_front(); 428 | camera_right_ts_buffer.pop_front(); 429 | if (camera_right_ts_buffer.size() == 0 || !camera_right_ts_buffer.front().second) receive_camera_right = false; 430 | break; 431 | case 6 : 432 | kinect_color_buffer.front()->header.stamp.fromSec(kinect_color_ts_buffer[kinect_color_ts_idx].toSec()); 433 | bag_out.write(out_kinect_color_topic, kinect_color_ts_buffer[kinect_color_ts_idx++], kinect_color_buffer.front()); 434 | kinect_color_buffer.pop_front(); 435 | if (kinect_color_ts_idx == kinect_color_ts_buffer.size()) receive_kinect_color = false; 436 | break; 437 | case 7 : 438 | kinect_depth_buffer.front()->header.stamp.fromSec(kinect_depth_ts_buffer[kinect_depth_ts_idx].toSec()); 439 | bag_out.write(out_kinect_depth_topic, kinect_depth_ts_buffer[kinect_depth_ts_idx++], kinect_depth_buffer.front()); 440 | kinect_depth_buffer.pop_front(); 441 | if (kinect_depth_ts_idx == kinect_depth_ts_buffer.size()) receive_kinect_depth = false; 442 | break; 443 | case 8 : 444 | lidar_buffer.front()->header.stamp.fromSec(lidar_ts_buffer.front().first.toSec()); 445 | bag_out.write(out_lidar_topic, lidar_ts_buffer.front().first, lidar_buffer.front()); 446 | lidar_buffer.pop_front(); 447 | lidar_ts_buffer.pop_front(); 448 | if (lidar_ts_buffer.size() == 0 || !lidar_ts_buffer.front().second) receive_lidar = false; 449 | break; 450 | case 9 : 451 | bag_out.write(out_gt_topic, gt_buffer.front()->header.stamp, gt_buffer.front()); 452 | gt_buffer.pop_front(); 453 | if (gt_buffer.size() == 0) receive_gt = false; 454 | break; 455 | } 456 | } 457 | } 458 | bag_out.close(); 459 | ROS_INFO("%s", colorful_char::info("Timeline reconstruction ends successfully. Enjoy the new bag!").c_str()); 460 | 461 | ros::shutdown(); 462 | return 0; 463 | } 464 | --------------------------------------------------------------------------------