├── msg
├── Event.msg
└── EventArray.msg
├── config
├── event_visualizer.yaml
├── event_stereo_visualizer.yaml
├── raw_message_topic.yaml
└── message_topic.yaml
├── launch
├── bag_merger.launch
├── data_validation.launch
├── event_visualization.launch
├── event_stereo_visualization.launch
├── bag2rawfile.launch
├── bag_splitter.launch
└── timeline_reconstruction.launch
├── package.xml
├── script
├── csv2tum.py
└── bag2hdf5.py
├── CMakeLists.txt
├── LICENSE.txt
├── include
└── utility.hpp
├── src
├── event_visualizer.cpp
├── bag_merger.cpp
├── bag2rawfile.cpp
├── bag_splitter.cpp
├── data_validator.cpp
└── timeline_reconstructor.cpp
└── README.md
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/launch/bag_merger.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/launch/data_validation.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/launch/event_visualization.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/launch/event_stereo_visualization.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/launch/bag2rawfile.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/launch/bag_splitter.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
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 |
--------------------------------------------------------------------------------
/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"
--------------------------------------------------------------------------------
/launch/timeline_reconstruction.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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/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_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/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 |
--------------------------------------------------------------------------------