diff --git a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py index efc72425..dc357b04 100644 --- a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py @@ -602,13 +602,13 @@ def _convert_image( for sample_record in sample_records ] synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( - image_timestamp_list, - lidar_timestamp_list, - self._camera_latency, - self._accept_frame_drop, - self._system_scan_period_sec, - self._num_load_frames, - self._msg_display_interval, + image_timestamp_list=image_timestamp_list, + lidar_timestamp_list=lidar_timestamp_list, + lidar_to_camera_latency_sec=self._camera_latency, + system_scan_period=self._system_scan_period_sec, + accept_frame_drop=self._accept_frame_drop, + num_load_frames=self._num_load_frames, + msg_display_interval=self._msg_display_interval, ) # Get image shape diff --git a/perception_dataset/utils/misc.py b/perception_dataset/utils/misc.py index 8560f8fc..3e658080 100644 --- a/perception_dataset/utils/misc.py +++ b/perception_dataset/utils/misc.py @@ -24,25 +24,18 @@ def get_sample_data_filename(sensor_channel: str, frame_index: int, fileformat: def get_lidar_camera_synced_frame_info( image_timestamp_list: List[float], lidar_timestamp_list: List[float], - camera_latency_sec: float = 0.0, - accept_frame_drop: bool = False, + lidar_to_camera_latency_sec: float = 0.0, system_scan_period: float = 0.1, + accept_frame_drop: bool = False, num_load_frames: int = 0, msg_display_interval: int = 100, ): -<<<<<<< HEAD - synced_frame_info_list: List[ - int, int, float - ] = ( - [] - ) # [image_index, lidar_frame_index, dummy_image_timestamp (None if image is not dropped)] -======= """ Get synced frame info list for lidar and camera. LiDAR scan with t_lidar and image with t_image are synced if - t_image - t_lidar >= camera_latency_sec + t_image - t_lidar >= lidar_to_camera_latency_sec and - t_image - t_lidar <= system_scan_period + camera_latency_sec + t_image - t_lidar <= system_scan_period + lidar_to_camera_latency_sec Args: image_timestamp_list: image timestamp list @@ -57,7 +50,6 @@ def get_lidar_camera_synced_frame_info( [[image_index, lidar_frame_index, dummy_image_timestamp (None if image is not dropped)]] """ synced_frame_info_list: List[int, int, float] = [] ->>>>>>> a3f9bb2 (feat: add document for get_lidar_camera_synced_frame_info) current_image_index: int = 0 for lidar_index, lidar_timestamp in enumerate(lidar_timestamp_list): @@ -65,7 +57,7 @@ def get_lidar_camera_synced_frame_info( break image_timestamp = image_timestamp_list[current_image_index] - while image_timestamp - lidar_timestamp < camera_latency_sec: + while image_timestamp - lidar_timestamp < lidar_to_camera_latency_sec: if not accept_frame_drop: raise ValueError( f"LiDAR message may be dropped at image_timestamp={image_timestamp}" @@ -73,7 +65,7 @@ def get_lidar_camera_synced_frame_info( current_image_index += 1 image_timestamp = image_timestamp_list[current_image_index] - if image_timestamp - lidar_timestamp > system_scan_period + camera_latency_sec: + if image_timestamp - lidar_timestamp > system_scan_period + lidar_to_camera_latency_sec: if not accept_frame_drop: raise ValueError( f"Image message may be dropped at lidar_timestamp={lidar_timestamp}" diff --git a/tests/utils/test_misc.py b/tests/utils/test_misc.py index 6f96adcd..febc4483 100644 --- a/tests/utils/test_misc.py +++ b/tests/utils/test_misc.py @@ -30,64 +30,64 @@ def assert_synced_frame_info_list(expected, synced_frame_info_list): def test_get_lidar_camera_synced_frame_info_1(): - image_timestamp_list = [0.04, 0.14, 0.24, 0.34, 0.44] + image_timestamp_list = [0.07, 0.17, 0.27, 0.37, 0.47] lidar_timestamp_list = [0.0, 0.1, 0.2, 0.3, 0.4] expected = [[0, 0, None], [1, 1, None], [2, 2, None], [3, 3, None], [4, 4, None]] synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) def test_get_lidar_camera_synced_frame_info_2(): - image_timestamp_list = [0.04, 0.24, 0.44] + image_timestamp_list = [0.07, 0.27, 0.47] lidar_timestamp_list = [0.0, 0.1, 0.2, 0.3, 0.4] - expected = [[0, 0, None], [None, 1, 0.14], [1, 2, None], [None, 3, 0.34], [2, 4, None]] + expected = [[0, 0, None], [None, 1, 0.17], [1, 2, None], [None, 3, 0.37], [2, 4, None]] synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) def test_get_lidar_camera_synced_frame_info_3(): - image_timestamp_list = [0.07, 0.17, 0.27, 0.37, 0.47] + image_timestamp_list = [0.11, 0.21, 0.31, 0.41, 0.51] lidar_timestamp_list = [0.0, 0.1, 0.2, 0.3, 0.4] expected = [[0, 0, None], [1, 1, None], [2, 2, None], [3, 3, None], [4, 4, None]] synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) def test_get_lidar_camera_synced_frame_info_4(): - image_timestamp_list = [0.11, 0.21, 0.31, 0.41, 0.51] + image_timestamp_list = [0.14, 0.24, 0.34, 0.44, 0.54] lidar_timestamp_list = [0.0, 0.1, 0.2, 0.3, 0.4] expected = [[0, 0, None], [1, 1, None], [2, 2, None], [3, 3, None], [4, 4, None]] synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) @@ -101,9 +101,9 @@ def test_get_lidar_camera_synced_frame_info_5(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) @@ -117,9 +117,9 @@ def test_get_lidar_camera_synced_frame_info_6(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) @@ -133,9 +133,9 @@ def test_get_lidar_camera_synced_frame_info_7(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=len(lidar_timestamp_list), ) assert_synced_frame_info_list(expected, synced_frame_info_list) @@ -149,9 +149,9 @@ def test_get_lidar_camera_synced_frame_info_accept_drop_frame_false(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=False, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=False, num_load_frames=5, ) @@ -164,9 +164,9 @@ def test_get_lidar_camera_synced_frame_info_accept_drop_frame_false(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=False, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=False, num_load_frames=5, ) @@ -180,9 +180,9 @@ def test_get_lidar_camera_synced_frame_info_num_load_frames(): synced_frame_info_list = misc_utils.get_lidar_camera_synced_frame_info( image_timestamp_list=image_timestamp_list, lidar_timestamp_list=lidar_timestamp_list, - accept_frame_drop=True, + lidar_to_camera_latency_sec=0.05, system_scan_period=0.1, - camera_latency_sec=0.02, + accept_frame_drop=True, num_load_frames=num_load_frames, ) assert_synced_frame_info_list(expected, synced_frame_info_list)