From 5a6b27900465a5dbf640e694792b84e9f54cc183 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Wed, 13 Sep 2023 12:11:14 +0200 Subject: [PATCH 1/8] enable synthetic camera images decoding --- perception_dataset/utils/rosbag2.py | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/perception_dataset/utils/rosbag2.py b/perception_dataset/utils/rosbag2.py index 5b0c3191..0fc6bbda 100644 --- a/perception_dataset/utils/rosbag2.py +++ b/perception_dataset/utils/rosbag2.py @@ -162,11 +162,23 @@ def radar_tracks_msg_to_list(radar_tracks_msg: RadarTracks) -> List[Dict[str, An return radar_tracks -def compressed_msg_to_numpy(compressed_image_msg: CompressedImage) -> NDArray: - image_buf = np.ndarray( - shape=(1, len(compressed_image_msg.data)), dtype=np.uint8, buffer=compressed_image_msg.data - ) - image = cv2.imdecode(image_buf, cv2.IMREAD_ANYCOLOR) +def compressed_msg_to_numpy(compressed_image_msg: CompressedImage) -> np.ndarray: + if hasattr(compressed_image_msg, "_encoding"): + try: + np_arr = np.frombuffer(compressed_image_msg.data, np.uint8) + image = np.reshape( + np_arr, (compressed_image_msg.height, compressed_image_msg.width, 3) + ) + except Exception as e: + print(e) + return None + else: + image_buf = np.ndarray( + shape=(1, len(compressed_image_msg.data)), + dtype=np.uint8, + buffer=compressed_image_msg.data, + ) + image = cv2.imdecode(image_buf, cv2.IMREAD_ANYCOLOR) return image From d57e919065f6e96d53108a449cb3c10a55bb6d16 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Wed, 13 Sep 2023 12:19:09 +0200 Subject: [PATCH 2/8] fixed camera view orientation for synthetic data --- perception_dataset/convert.py | 9 +++++++- .../rosbag2_to_non_annotated_t4_converter.py | 22 +++++++++++++++++-- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/perception_dataset/convert.py b/perception_dataset/convert.py index 06171bea..e9292798 100644 --- a/perception_dataset/convert.py +++ b/perception_dataset/convert.py @@ -2,7 +2,7 @@ import yaml -from perception_dataset.rosbag2.converter_params import Rosbag2ConverterParams +from perception_dataset.rosbag2.converter_params import DataType, Rosbag2ConverterParams from perception_dataset.utils.logger import configure_logger logger = configure_logger(modname=__name__) @@ -25,6 +25,11 @@ def main(): action="store_true", help="do NOT compress rosbag/non-annotated-t4", ) + parser.add_argument( + "--synthetic", + action="store_true", + help="convert synthetic data", + ) args = parser.parse_args() with open(args.config, "r") as f: @@ -119,6 +124,8 @@ def main(): **config_dict["conversion"], } converter_params = Rosbag2ConverterParams(**param_args) + if args.synthetic: + converter_params.data_type = DataType.SYNTHETIC converter = Rosbag2ToT4Converter(converter_params) logger.info("[BEGIN] Converting ros2bag output by simulator --> T4 Format Data") 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 c622aa24..1ae8742d 100644 --- a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py @@ -11,6 +11,7 @@ import builtin_interfaces.msg import cv2 import numpy as np +from pyquaternion import Quaternion from radar_msgs.msg import RadarTracks from sensor_msgs.msg import CompressedImage, PointCloud2 @@ -21,7 +22,7 @@ SENSOR_MODALITY_ENUM, T4_FORMAT_DIRECTORY_NAME, ) -from perception_dataset.rosbag2.converter_params import Rosbag2ConverterParams +from perception_dataset.rosbag2.converter_params import DataType, Rosbag2ConverterParams from perception_dataset.rosbag2.rosbag2_reader import Rosbag2Reader from perception_dataset.t4_dataset.classes.abstract_class import AbstractTable from perception_dataset.t4_dataset.classes.attribute import AttributeTable @@ -99,6 +100,7 @@ def __init__(self, params: Rosbag2ConverterParams) -> None: self._camera_latency: float = params.camera_latency_sec self._start_timestamp: float = params.start_timestamp_sec self._end_timestamp: float = 0 + self._data_type: DataType = params.data_type self._ignore_no_ego_transform_at_rosbag_beginning: bool = ( params.ignore_no_ego_transform_at_rosbag_beginning ) @@ -766,7 +768,23 @@ def _generate_calibrated_sensor( camera_distortion=[], ) elif modality == SENSOR_MODALITY_ENUM.CAMERA.value: - cam_info_topic = topic_name.rsplit("/", 2)[0] + "/camera_info" + if self._data_type.value == "synthetic": + # fix of the orientation of camera view + rotation = Quaternion( + rotation["w"], rotation["x"], rotation["y"], rotation["z"] + ) + axes_fix_rotation = Quaternion(0.5, -0.5, 0.5, -0.5) + rotation = rotation * axes_fix_rotation + + rotation = { + "w": rotation.w, + "x": rotation.x, + "y": rotation.y, + "z": rotation.z, + } + + topic_name_splitted = topic_name.split("/") + cam_info_topic = "/".join(topic_name_splitted[:4]) + "/camera_info" info = self._bag_reader.camera_info.get(cam_info_topic) if info is None: continue From 58db7c688b4708eb47a470587bce00bfb027e782 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Wed, 13 Sep 2023 12:21:42 +0200 Subject: [PATCH 3/8] increased _TIMESTAMP_DIFF to be able to generate syntetic data - workaround for Unity's step execution issue --- .../rosbag2/rosbag2_to_non_annotated_t4_converter.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 1ae8742d..75391b27 100644 --- a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py @@ -115,7 +115,9 @@ def __init__(self, params: Rosbag2ConverterParams) -> None: self._calibrated_sensor_target_frame: str = "base_link" # Note: To determine if there is any message dropout, including a delay tolerance of 10Hz. - self._TIMESTAMP_DIFF = 0.15 + # changed from 0.15 to 2.0 - a workaround of issue with Unity's step execution + # #TODO (mkotynia) should be decreased when the issue with Unity is fixed + self._TIMESTAMP_DIFF = 2.0 self._lidar_sensor: Dict[str, str] = params.lidar_sensor self._radar_sensors: List[Dict[str, str]] = params.radar_sensors From 43907dc8d83f722d59a65b0bea8c20a152a76007 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Wed, 13 Sep 2023 13:00:47 +0200 Subject: [PATCH 4/8] implemented generation of 2d bounding boxes annotations in object_ann.json file --- .../rosbag2/rosbag2_to_t4_converter.py | 19 ++++++ .../utils/create_2d_annotations.py | 60 +++++++++++++++++++ 2 files changed, 79 insertions(+) create mode 100644 perception_dataset/utils/create_2d_annotations.py diff --git a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py index 5fb93606..96d71f27 100644 --- a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py @@ -21,6 +21,7 @@ from perception_dataset.t4_dataset.classes.scene import SceneTable from perception_dataset.t4_dataset.classes.sensor import SensorTable from perception_dataset.utils.calculate_num_points import calculate_num_points +from perception_dataset.utils.create_2d_annotations import create_2d_annotations from perception_dataset.utils.logger import configure_logger import perception_dataset.utils.rosbag2 as rosbag2_utils @@ -133,6 +134,8 @@ def convert(self): self._annotation_files_generator.save_tables(self._output_anno_dir) # Calculate and overwrite num_lidar_prs in annotations self._calculate_num_points() + if len(self._camera_sensors) > 0: + self._create_2d_annotations() def _calculate_num_points(self): logger.info("Calculating number of points...") @@ -144,6 +147,22 @@ def _calculate_num_points(self): ) annotation_table.save_json(self._output_anno_dir) + def _create_2d_annotations(self): + logger.info("Creating 2d camera annotations...") + object_ann_table = self._annotation_files_generator._object_ann_table + create_2d_annotations( + self._output_scene_dir, + self._camera_sensors, + self._annotation_files_generator._sample_annotation_table, + self._sample_data_table, + object_ann_table, + self._annotation_files_generator._instance_table, + ) + logger.info( + f"object_ann.json created with {len(object_ann_table._token_to_record)} records" + ) + object_ann_table.save_json(self._output_anno_dir) + def _convert_objects(self, start_timestamp: float): """read object bbox ground truth from rosbag""" start_time_in_time = rosbag2_utils.unix_timestamp_to_stamp(start_timestamp) diff --git a/perception_dataset/utils/create_2d_annotations.py b/perception_dataset/utils/create_2d_annotations.py new file mode 100644 index 00000000..92f22a38 --- /dev/null +++ b/perception_dataset/utils/create_2d_annotations.py @@ -0,0 +1,60 @@ +from typing import List + +import numpy as np +from nuscenes import NuScenes + +from perception_dataset.t4_dataset.classes.instance import InstanceTable +from perception_dataset.t4_dataset.classes.object_ann import ObjectAnnRecord, ObjectAnnTable +from perception_dataset.t4_dataset.classes.sample_annotation import SampleAnnotationTable +from perception_dataset.t4_dataset.classes.sample_data import SampleDataTable + + +def create_2d_annotations( + dataroot: str, + camera_sensor_channels: List[str], + annotation_table: SampleAnnotationTable, + sample_data_table: SampleDataTable, + object_ann_table: ObjectAnnTable, + instance_table: InstanceTable, +): + nusc = NuScenes(version="annotation", dataroot=dataroot, verbose=False) + for sample in nusc.sample: + for camera_sensor in camera_sensor_channels: + camera_channel = camera_sensor["channel"] + camera_token = sample["data"][camera_channel] + _, boxes, camera_intrinsic = nusc.get_sample_data(camera_token) + + # map 3d cuboids to 2d bounding boxes + for box in boxes: + corners = box.corners() + x = corners[0, :] + y = corners[1, :] + z = corners[2, :] + x_y_z = np.array((x, y, z)) + orthographic = np.dot(camera_intrinsic, x_y_z) + perspective_x = orthographic[0] / orthographic[2] + perspective_y = orthographic[1] / orthographic[2] + + min_x = int(np.min(perspective_x)) + max_x = int(np.max(perspective_x)) + min_y = int(np.min(perspective_y)) + max_y = int(np.max(perspective_y)) + + sample_annotation_record = annotation_table._token_to_record[box.token] + instance_token = sample_annotation_record._instance_token + attribute_tokens = sample_annotation_record._attribute_tokens + category_token = instance_table._token_to_record[instance_token]._category_token + imsize = [ + sample_data_table._token_to_record[camera_token].width, + sample_data_table._token_to_record[camera_token].height, + ] + + object_ann_record = ObjectAnnRecord( + sample_data_token=camera_token, + instance_token=instance_token, + category_token=category_token, + attribute_tokens=attribute_tokens, + bbox=[min_x, min_y, max_x, max_y], + mask={"size": imsize}, + ) + object_ann_table._token_to_record[object_ann_record.token] = object_ann_record From 2901114f6ab5e0deb4b01cac06fcefe3f983917d Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Mon, 18 Sep 2023 15:31:24 +0200 Subject: [PATCH 5/8] updated tools overview.md --- .../convert_synthetic_data_camera.yaml | 23 +++++++++++++++++++ docs/tools_overview.md | 23 +++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 config/rosbag2_to_t4/convert_synthetic_data_camera.yaml diff --git a/config/rosbag2_to_t4/convert_synthetic_data_camera.yaml b/config/rosbag2_to_t4/convert_synthetic_data_camera.yaml new file mode 100644 index 00000000..665c9a31 --- /dev/null +++ b/config/rosbag2_to_t4/convert_synthetic_data_camera.yaml @@ -0,0 +1,23 @@ +task: convert_rosbag2_to_t4 +description: + scene: synthetic +conversion: + # path to rosbag dir output by simulator + input_base: .data/odaiba_synthetic_with_images/rosbags + output_base: .data/odaiba_synthetic_with_images/t4dataset + workers_number: 1 + skip_timestamp: 2.0 + num_load_frames: 50 + crop_frames_unit: 1 # crop frames from the end so that the number of frames is divisible by crop_frames_unit. Set to 0 or 1 so as not to crop any frames. + camera_latency_sec: 0.00 + object_topic_name: /ground_truth/objects + object_msg_type: TrackedObjects + world_frame_id: world + lidar_sensor: + topic: /lidar/concatenated/pointcloud + channel: LIDAR_CONCAT + camera_sensors: #Keep the same order as each camera exposure timing + - topic: /sensing/camera/camera4/image_rect_color + channel: CAM_FRONT + - topic: /sensing/camera/camera5/image_rect_color + channel: CAM_BACK diff --git a/docs/tools_overview.md b/docs/tools_overview.md index 4f9d4e84..73022218 100644 --- a/docs/tools_overview.md +++ b/docs/tools_overview.md @@ -152,6 +152,29 @@ output: T4 format data python -m perception_dataset.convert --config config/rosbag2_to_t4/convert_synthetic_data.yaml ``` +### Synthetic bag to T4 format with images and 2d images annotations + +input: rosbag2 +output: T4 format data + +#### Messages + +| Topic Name | Required | Message Type | +| --------------------------------------------- | -------- | -------------------------------------------------- | +| `/ground_truth/objects` | o | `autoware_auto_perception_msgs/msg/TrackedObjects` | +| `/sensing/camera/camera{ID}/camera_info` | o | `visualization_msgs/msg/MarkerArray` | +| `/sensing/lidar/concatenated/pointcloud` | o | `sensor_msgs/msg/PointCloud2` | +| `/tf` | o | `tf2_msgs/msg/TFMessage` | +| `/tf_static` | o | `tf2_msgs/msg/TFMessage` | +| `/sensing/camera/camera{ID}/image_rect_color` | o | `sensor_msgs/msg/Image` | +| `/sensing/camera/camera{ID}/camera_info` | o | `sensor_msgs/msg/CameraInfo` | + +#### script + +```bash +python -m perception_dataset.convert --config config/rosbag2_to_t4/convert_synthetic_data_camera.yaml --synthetic +``` + ### Pseudo-labeled bag to T4 format #### Description From 7734fcded237d6bdf776d53d78ec5258bff61537 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Thu, 28 Sep 2023 12:49:21 +0200 Subject: [PATCH 6/8] added script argument --generate-bbox-from-cuboid to generate 2d bboxes from cuboids --- docs/tools_overview.md | 2 +- perception_dataset/convert.py | 6 ++++++ perception_dataset/rosbag2/converter_params.py | 1 + perception_dataset/rosbag2/rosbag2_to_t4_converter.py | 3 ++- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/docs/tools_overview.md b/docs/tools_overview.md index 73022218..859820c4 100644 --- a/docs/tools_overview.md +++ b/docs/tools_overview.md @@ -172,7 +172,7 @@ output: T4 format data #### script ```bash -python -m perception_dataset.convert --config config/rosbag2_to_t4/convert_synthetic_data_camera.yaml --synthetic +python -m perception_dataset.convert --config config/rosbag2_to_t4/convert_synthetic_data_camera.yaml --synthetic --generate-bbox-from-cuboid ``` ### Pseudo-labeled bag to T4 format diff --git a/perception_dataset/convert.py b/perception_dataset/convert.py index e9292798..d5ffe427 100644 --- a/perception_dataset/convert.py +++ b/perception_dataset/convert.py @@ -30,6 +30,11 @@ def main(): action="store_true", help="convert synthetic data", ) + parser.add_argument( + "--generate-bbox-from-cuboid", + action="store_true", + help="generate 2d images annotations from cuboid annotations", + ) args = parser.parse_args() with open(args.config, "r") as f: @@ -122,6 +127,7 @@ def main(): "scene_description": config_dict["description"]["scene"], "overwrite_mode": args.overwrite, **config_dict["conversion"], + "generate_bbox_from_cuboid": args.generate_bbox_from_cuboid, } converter_params = Rosbag2ConverterParams(**param_args) if args.synthetic: diff --git a/perception_dataset/rosbag2/converter_params.py b/perception_dataset/rosbag2/converter_params.py index 7e7bcc87..fbc2c2c9 100644 --- a/perception_dataset/rosbag2/converter_params.py +++ b/perception_dataset/rosbag2/converter_params.py @@ -42,6 +42,7 @@ class Rosbag2ConverterParams(BaseModel): traffic_light_rois_topic_name: str = "" world_frame_id: str = "map" with_camera: bool = True + generate_bbox_from_cuboid: bool = False # rosbag reader num_load_frames: int # the number of frames to be loaded. if the value isn't positive, read all messages. diff --git a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py index 96d71f27..791261a7 100644 --- a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py @@ -99,6 +99,7 @@ def __init__(self, params: Rosbag2ConverterParams) -> None: self._calibrated_sensor_target_frame: str = "base_link" self._annotation_files_generator = AnnotationFilesGenerator(with_camera=params.with_camera) + self._generate_bbox_from_cuboid = params.generate_bbox_from_cuboid def _init_tables(self): # vehicle @@ -134,7 +135,7 @@ def convert(self): self._annotation_files_generator.save_tables(self._output_anno_dir) # Calculate and overwrite num_lidar_prs in annotations self._calculate_num_points() - if len(self._camera_sensors) > 0: + if len(self._camera_sensors) > 0 and self._generate_bbox_from_cuboid is True: self._create_2d_annotations() def _calculate_num_points(self): From 6ea82a8c4a411a777cb3de07d4296f8568600a9f Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Thu, 28 Sep 2023 12:59:23 +0200 Subject: [PATCH 7/8] Added log when --generate-bbox-from-cuboid is true but no cameras are provided --- perception_dataset/rosbag2/rosbag2_to_t4_converter.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py index 791261a7..8312a555 100644 --- a/perception_dataset/rosbag2/rosbag2_to_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_t4_converter.py @@ -137,6 +137,8 @@ def convert(self): self._calculate_num_points() if len(self._camera_sensors) > 0 and self._generate_bbox_from_cuboid is True: self._create_2d_annotations() + elif len(self._camera_sensors) == 0 and self._generate_bbox_from_cuboid is True: + logger.info("Skipping 2d annotations generation. No camera sensors found.") def _calculate_num_points(self): logger.info("Calculating number of points...") From 7601e4d41513a1177f2a7586b1d7c37b7695b808 Mon Sep 17 00:00:00 2001 From: Magdalena Kotynia Date: Thu, 12 Oct 2023 13:40:24 +0200 Subject: [PATCH 8/8] feat: parametrize timestamp_diff --- perception_dataset/rosbag2/converter_params.py | 1 + .../rosbag2/rosbag2_to_non_annotated_t4_converter.py | 5 ++--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/perception_dataset/rosbag2/converter_params.py b/perception_dataset/rosbag2/converter_params.py index 13db9cd7..77cf1433 100644 --- a/perception_dataset/rosbag2/converter_params.py +++ b/perception_dataset/rosbag2/converter_params.py @@ -52,6 +52,7 @@ class Rosbag2ConverterParams(BaseModel): camera_latency_sec: float = ( 0.0 # camera latency in seconds between the header.stamp and shutter trigger ) + timestamp_diff: float = 0.15 topic_list: list = [] # topic list for input_bag # in synthetic data (from AWSIM) it may be the case that there is no ego transform available at the beginning of rosbag ignore_no_ego_transform_at_rosbag_beginning: bool = False 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 019aadc9..716aedc9 100644 --- a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py +++ b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py @@ -115,9 +115,8 @@ def __init__(self, params: Rosbag2ConverterParams) -> None: self._calibrated_sensor_target_frame: str = "base_link" # Note: To determine if there is any message dropout, including a delay tolerance of 10Hz. - # changed from 0.15 to 2.0 - a workaround of issue with Unity's step execution - # #TODO (mkotynia) should be decreased when the issue with Unity is fixed - self._TIMESTAMP_DIFF = 2.0 + + self._TIMESTAMP_DIFF = params.timestamp_diff self._lidar_sensor: Dict[str, str] = params.lidar_sensor self._radar_sensors: List[Dict[str, str]] = params.radar_sensors