From ff616ff04b96b2eb20732d5920a0c347d4b1839e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Mon, 22 Jan 2024 17:51:12 +0900 Subject: [PATCH 1/8] feat: use project-style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- projects/AutowareCenterPoint/README.md | 0 .../centerpoint/__init__.py | 3 + .../centerpoint/pillar_encoder_autoware.py | 165 ++++ .../centerpoint_onnx_converter.py | 273 ++++++ .../configs/centerpoint_custom.py | 847 +++++++++++++++++ .../configs/centerpoint_custom_test.py | 607 ++++++++++++ .../AutowareCenterPoint/datasets/__init__.py | 3 + .../datasets/tier4_dataset.py | 114 +++ .../evaluation/__init__.py | 17 + .../functional/nuscenes_utils/__init__.py | 16 + .../functional/nuscenes_utils/eval.py | 172 ++++ .../functional/nuscenes_utils/utils.py | 302 ++++++ .../evaluation/metrics/__init__.py | 3 + .../metrics/nuscenes_custom_metric.py | 870 ++++++++++++++++++ tools/create_data.py | 45 + .../dataset_converters/create_gt_database.py | 23 + .../tier4dataset_converter.py | 644 +++++++++++++ .../dataset_converters/update_infos_to_v2.py | 139 +++ 18 files changed, 4243 insertions(+) create mode 100644 projects/AutowareCenterPoint/README.md create mode 100644 projects/AutowareCenterPoint/centerpoint/__init__.py create mode 100644 projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py create mode 100644 projects/AutowareCenterPoint/centerpoint_onnx_converter.py create mode 100755 projects/AutowareCenterPoint/configs/centerpoint_custom.py create mode 100644 projects/AutowareCenterPoint/configs/centerpoint_custom_test.py create mode 100644 projects/AutowareCenterPoint/datasets/__init__.py create mode 100644 projects/AutowareCenterPoint/datasets/tier4_dataset.py create mode 100644 projects/AutowareCenterPoint/evaluation/__init__.py create mode 100644 projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py create mode 100644 projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py create mode 100644 projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py create mode 100644 projects/AutowareCenterPoint/evaluation/metrics/__init__.py create mode 100644 projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py create mode 100644 tools/dataset_converters/tier4dataset_converter.py diff --git a/projects/AutowareCenterPoint/README.md b/projects/AutowareCenterPoint/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/projects/AutowareCenterPoint/centerpoint/__init__.py b/projects/AutowareCenterPoint/centerpoint/__init__.py new file mode 100644 index 0000000000..77f26085f3 --- /dev/null +++ b/projects/AutowareCenterPoint/centerpoint/__init__.py @@ -0,0 +1,3 @@ +from .pillar_encoder_autoware import PillarFeatureNetAutoware + +__all__ = [ 'PillarFeatureNetAutoware'] \ No newline at end of file diff --git a/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py new file mode 100644 index 0000000000..882b0ac124 --- /dev/null +++ b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py @@ -0,0 +1,165 @@ +from typing import Optional, Tuple + +import torch +from mmcv.cnn import build_norm_layer +from mmcv.ops import DynamicScatter +from torch import Tensor, nn + +from mmdet3d.registry import MODELS +from mmdet3d.models.voxel_encoders.utils import PFNLayer, get_paddings_indicator +@MODELS.register_module() +class PillarFeatureNetAutoware(nn.Module): + """Pillar Feature Net. + + The network prepares the pillar features and performs forward pass + through PFNLayers. + + Args: + in_channels (int, optional): Number of input features, + either x, y, z or x, y, z, r. Defaults to 4. + feat_channels (tuple, optional): Number of features in each of the + N PFNLayers. Defaults to (64, ). + with_distance (bool, optional): Whether to include Euclidean distance + to points. Defaults to False. + with_cluster_center (bool, optional): [description]. Defaults to True. + with_voxel_center (bool, optional): [description]. Defaults to True. + voxel_size (tuple[float], optional): Size of voxels, only utilize x + and y size. Defaults to (0.2, 0.2, 4). + point_cloud_range (tuple[float], optional): Point cloud range, only + utilizes x and y min. Defaults to (0, -40, -3, 70.4, 40, 1). + norm_cfg ([type], optional): [description]. + Defaults to dict(type='BN1d', eps=1e-3, momentum=0.01). + mode (str, optional): The mode to gather point features. Options are + 'max' or 'avg'. Defaults to 'max'. + legacy (bool, optional): Whether to use the new behavior or + the original behavior. Defaults to True. + """ + + def __init__(self, + in_channels: Optional[int] = 4, + feat_channels: Optional[tuple] = (64,), + with_distance: Optional[bool] = False, + with_cluster_center: Optional[bool] = True, + with_voxel_center: Optional[bool] = True, + voxel_size: Optional[Tuple[float]] = (0.2, 0.2, 4), + point_cloud_range: Optional[Tuple[float]] = (0, -40, -3, 70.4, + 40, 1), + norm_cfg: Optional[dict] = dict( + type='BN1d', eps=1e-3, momentum=0.01), + mode: Optional[str] = 'max', + legacy: Optional[bool] = True, + use_voxel_center_z: Optional[bool] = True, ): + super(PillarFeatureNetAutoware, self).__init__() + assert len(feat_channels) > 0 + self.legacy = legacy + self.use_voxel_center_z = use_voxel_center_z + if with_cluster_center: + in_channels += 3 + if with_voxel_center: + in_channels += 2 + if self.use_voxel_center_z: + in_channels += 1 + if with_distance: + in_channels += 1 + self._with_distance = with_distance + self._with_cluster_center = with_cluster_center + self._with_voxel_center = with_voxel_center + # Create PillarFeatureNet layers + self.in_channels = in_channels + feat_channels = [in_channels] + list(feat_channels) + pfn_layers = [] + for i in range(len(feat_channels) - 1): + in_filters = feat_channels[i] + out_filters = feat_channels[i + 1] + if i < len(feat_channels) - 2: + last_layer = False + else: + last_layer = True + pfn_layers.append( + PFNLayer( + in_filters, + out_filters, + norm_cfg=norm_cfg, + last_layer=last_layer, + mode=mode)) + self.pfn_layers = nn.ModuleList(pfn_layers) + + # Need pillar (voxel) size and x/y offset in order to calculate offset + self.vx = voxel_size[0] + self.vy = voxel_size[1] + self.vz = voxel_size[2] + self.x_offset = self.vx / 2 + point_cloud_range[0] + self.y_offset = self.vy / 2 + point_cloud_range[1] + self.z_offset = self.vz / 2 + point_cloud_range[2] + self.point_cloud_range = point_cloud_range + + def forward(self, features: Tensor, num_points: Tensor, coors: Tensor, + *args, **kwargs) -> Tensor: + """Forward function. + + Args: + features (torch.Tensor): Point features or raw points in shape + (N, M, C). + num_points (torch.Tensor): Number of points in each pillar. + coors (torch.Tensor): Coordinates of each voxel. + + Returns: + torch.Tensor: Features of pillars. + """ + features_ls = [features] + # Find distance of x, y, and z from cluster center + if self._with_cluster_center: + points_mean = features[:, :, :3].sum( + dim=1, keepdim=True) / num_points.type_as(features).view( + -1, 1, 1) + f_cluster = features[:, :, :3] - points_mean + features_ls.append(f_cluster) + + # Find distance of x, y, and z from pillar center + dtype = features.dtype + if self._with_voxel_center: + center_feature_size = 3 if self.use_voxel_center_z else 2 + if not self.legacy: + f_center = torch.zeros_like(features[:, :, :center_feature_size]) + f_center[:, :, 0] = features[:, :, 0] - ( + coors[:, 3].to(dtype).unsqueeze(1) * self.vx + + self.x_offset) + f_center[:, :, 1] = features[:, :, 1] - ( + coors[:, 2].to(dtype).unsqueeze(1) * self.vy + + self.y_offset) + if self.use_voxel_center_z: + f_center[:, :, 2] = features[:, :, 2] - ( + coors[:, 1].to(dtype).unsqueeze(1) * self.vz + + self.z_offset) + else: + f_center = features[:, :, :center_feature_size] + f_center[:, :, 0] = f_center[:, :, 0] - ( + coors[:, 3].type_as(features).unsqueeze(1) * self.vx + + self.x_offset) + f_center[:, :, 1] = f_center[:, :, 1] - ( + coors[:, 2].type_as(features).unsqueeze(1) * self.vy + + self.y_offset) + if self.use_voxel_center_z: + f_center[:, :, 2] = f_center[:, :, 2] - ( + coors[:, 1].type_as(features).unsqueeze(1) * self.vz + + self.z_offset) + features_ls.append(f_center) + + if self._with_distance: + points_dist = torch.norm(features[:, :, :3], 2, 2, keepdim=True) + features_ls.append(points_dist) + + # Combine together feature decorations + features = torch.cat(features_ls, dim=-1) + # The feature decorations were calculated without regard to whether + # pillar was empty. Need to ensure that + # empty pillars remain set to zeros. + voxel_count = features.shape[1] + mask = get_paddings_indicator(num_points, voxel_count, axis=0) + mask = torch.unsqueeze(mask, -1).type_as(features) + features *= mask + + for pfn in self.pfn_layers: + features = pfn(features, num_points) + + return features.squeeze(1) \ No newline at end of file diff --git a/projects/AutowareCenterPoint/centerpoint_onnx_converter.py b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py new file mode 100644 index 0000000000..7a6a293959 --- /dev/null +++ b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py @@ -0,0 +1,273 @@ +import argparse +import os +import torch +from typing import Any, Dict, Optional, Tuple, List +from mmengine import Config +from mmengine.registry import MODELS, Registry +from mmengine.runner import Runner +from mmdet3d.apis import init_model +from mmdet3d.models.dense_heads.centerpoint_head import SeparateHead, CenterHead +from mmdet3d.models.voxel_encoders.utils import get_paddings_indicator +from projects.AutowareCenterPoint.centerpoint.pillar_encoder_autoware import PillarFeatureNetAutoware + + +def parse_args(): + parser = argparse.ArgumentParser(description='Create autoware compitable onnx file from torch checkpoint ') + parser.add_argument('--cfg', help='train config file path') + parser.add_argument('--ckpt', help='checkpoint weeight') + parser.add_argument('--work-dir', help='the dir to save onnx files') + + args = parser.parse_args() + return args + + +class CenterPointToONNX(object): + def __init__( + self, + config: Config, + checkpoint_path: Optional[str] = None, + output_path: Optional[str] = None, + ): + assert isinstance(config, Config), f"expected `mmcv.Config`, but got {type(config)}" + _, ext = os.path.splitext(checkpoint_path) + assert ext == ".pth", f"expected .pth model, but got {ext}" + + self.config = config + self.checkpoint_path = checkpoint_path + + os.makedirs(output_path, exist_ok=True) + self.output_path = output_path + + def save_onnx(self) -> None: + # Overwrite models with Autoware's TensorRT compatible versions + self.config.model.pts_voxel_encoder.type = "PillarFeatureNetONNX" + self.config.model.pts_bbox_head.type = "CenterHeadONNX" + self.config.model.pts_bbox_head.separate_head.type = "SeparateHeadONNX" + + model = init_model(self.config, self.checkpoint_path, device="cuda:0") + dataloader = Runner.build_dataloader(self.config.test_dataloader) + batch_dict = next(iter(dataloader)) + + voxel_dict = model.data_preprocessor.voxelize(batch_dict["inputs"]["points"], batch_dict) + input_features = model.pts_voxel_encoder.get_input_features(voxel_dict["voxels"], voxel_dict["num_points"], + voxel_dict["coors"]).to("cuda:0") + + # CenterPoint's PointPillar voxel encoder ONNX conversion + pth_onnx_pve = os.path.join(self.output_path, "pts_voxel_encoder_centerpoint_custom.onnx") + torch.onnx.export( + model.pts_voxel_encoder, + (input_features,), + f=pth_onnx_pve, + input_names=("input_features",), + output_names=("pillar_features",), + dynamic_axes={ + "input_features": {0: "num_voxels", 1: "num_max_points"}, + "pillar_features": {0: "num_voxels"}, + }, + verbose=False, + opset_version=11, + ) + print(f"Saved pts_voxel_encoder onnx model: {pth_onnx_pve}") + + voxel_features = model.pts_voxel_encoder(input_features) + voxel_features = voxel_features.squeeze() + + batch_size = voxel_dict["coors"][-1, 0] + 1 + x = model.pts_middle_encoder(voxel_features, voxel_dict["coors"], batch_size) + + # CenterPoint backbone's to neck ONNX conversion + pts_backbone_neck_head = CenterPointHeadONNX( + model.pts_backbone, + model.pts_neck, + model.pts_bbox_head, + ) + + pth_onnx_backbone_neck_head = os.path.join(self.output_path, "pts_backbone_neck_head_centerpoint_custom.onnx") + torch.onnx.export( + pts_backbone_neck_head, + (x,), + f=pth_onnx_backbone_neck_head, + input_names=("spatial_features",), + output_names=tuple(model.pts_bbox_head.output_names), + dynamic_axes={ + name: {0: "batch_size", 2: "H", 3: "W"} + for name in ["spatial_features"] + model.pts_bbox_head.output_names + }, + verbose=False, + opset_version=11, + ) + print(f"Saved pts_backbone_neck_head onnx model: {pth_onnx_backbone_neck_head}") + + +@MODELS.register_module() +class PillarFeatureNetONNX(PillarFeatureNetAutoware): + def __init__(self, **kwargs): + super().__init__(**kwargs) + + def get_input_features( + self, features: torch.Tensor, num_points: torch.Tensor, coors: torch.Tensor + ) -> torch.Tensor: + """Forward function. + Args: + features (torch.Tensor): Point features or raw points in shape + (N, M, C). + num_points (torch.Tensor): Number of points in each pillar. + coors (torch.Tensor): Coordinates of each voxel. + Returns: + torch.Tensor: Features of pillars. + """ + + features_ls = [features] + # Find distance of x, y, and z from cluster center + if self._with_cluster_center: + points_mean = features[:, :, :3].sum( + dim=1, keepdim=True) / num_points.type_as(features).view( + -1, 1, 1) + f_cluster = features[:, :, :3] - points_mean + features_ls.append(f_cluster) + + # Find distance of x, y, and z from pillar center + dtype = features.dtype + if self._with_voxel_center: + center_feature_size = 3 if self.use_voxel_center_z else 2 + if not self.legacy: + f_center = torch.zeros_like(features[:, :, :center_feature_size]) + f_center[:, :, 0] = features[:, :, 0] - ( + coors[:, 3].to(dtype).unsqueeze(1) * self.vx + + self.x_offset) + f_center[:, :, 1] = features[:, :, 1] - ( + coors[:, 2].to(dtype).unsqueeze(1) * self.vy + + self.y_offset) + if self.use_voxel_center_z: + f_center[:, :, 2] = features[:, :, 2] - ( + coors[:, 1].to(dtype).unsqueeze(1) * self.vz + + self.z_offset) + else: + f_center = features[:, :, :center_feature_size] + f_center[:, :, 0] = f_center[:, :, 0] - ( + coors[:, 3].type_as(features).unsqueeze(1) * self.vx + + self.x_offset) + f_center[:, :, 1] = f_center[:, :, 1] - ( + coors[:, 2].type_as(features).unsqueeze(1) * self.vy + + self.y_offset) + if self.use_voxel_center_z: + f_center[:, :, 2] = f_center[:, :, 2] - ( + coors[:, 1].type_as(features).unsqueeze(1) * self.vz + + self.z_offset) + features_ls.append(f_center) + + if self._with_distance: + points_dist = torch.norm(features[:, :, :3], 2, 2, keepdim=True) + features_ls.append(points_dist) + + features = torch.cat(features_ls, dim=-1) + + voxel_count = features.shape[1] + mask = get_paddings_indicator(num_points, voxel_count, axis=0) + mask = torch.unsqueeze(mask, -1).type_as(features) + features *= mask + + return features + + def forward( + self, + features: torch.Tensor, + ) -> torch.Tensor: + """Forward function. + Args: + features (torch.Tensor): Point features in shape (N, M, C). + num_points (torch.Tensor): Number of points in each pillar. + coors (torch.Tensor): + Returns: + torch.Tensor: Features of pillars. + """ + + for pfn in self.pfn_layers: + features = pfn(features) + + return features + + +@MODELS.register_module() +class SeparateHeadONNX(SeparateHead): + def __init__(self, **kwargs): + super().__init__(**kwargs) + + # Order output's of the heads + rot_heads = {k: None for k in self.heads.keys() if "rot" in k} + self.heads: Dict[str, None] = { + "heatmap": None, + "reg": None, + "height": None, + "dim": None, + **rot_heads, + "vel": None, + } + + +@MODELS.register_module() +class CenterHeadONNX(CenterHead): + def __init__(self, **kwargs): + super().__init__(**kwargs) + + self.task_heads: List[SeparateHeadONNX] + self.output_names: List[str] = list(self.task_heads[0].heads.keys()) + + def forward(self, x: List[torch.Tensor]) -> Tuple[torch.Tensor]: + """ + Args: + x (List[torch.Tensor]): multi-level features + Returns: + pred (Tuple[torch.Tensor]): Output results for tasks. + """ + assert len(x) == 1, "The input of CenterHeadONNX must be a single-level feature" + + x = self.shared_conv(x[0]) + head_tensors: Dict[str, torch.Tensor] = self.task_heads[0](x) + + ret_list: List[torch.Tensor] = list() + for head_name in self.output_names: + ret_list.append(head_tensors[head_name]) + + return tuple(ret_list) + + +class CenterPointHeadONNX(torch.nn.Module): + + def __init__(self, backbone: torch.nn.Module, neck: torch.nn.Module, bbox_head: torch.nn.Module): + super(CenterPointHeadONNX, self).__init__() + self.backbone: torch.nn.Module = backbone + self.neck: torch.nn.Module = neck + self.bbox_head: torch.nn.Module = bbox_head + + def forward(self, x: torch.Tensor) -> Tuple[List[Dict[str, torch.Tensor]]]: + """ + Args: + x (torch.Tensor): (B, C, H, W) + Returns: + tuple[list[dict[str, any]]]: + (num_classes x [num_detect x {'reg', 'height', 'dim', 'rot', 'vel', 'heatmap'}]) + """ + x = self.backbone(x) + if self.neck is not None: + x = self.neck(x) + x = self.bbox_head(x) + return x + + +CUSTOM_MODEL_REGISTRY = Registry('model', parent=MODELS) +CUSTOM_MODEL_REGISTRY.register_module(module=PillarFeatureNetONNX, force=True) +CUSTOM_MODEL_REGISTRY.register_module(module=CenterHeadONNX, force=True) +CUSTOM_MODEL_REGISTRY.register_module(module=SeparateHeadONNX, force=True) + + +def main(): + args = parse_args() + + cfg = Config.fromfile(args.cfg) + det = CenterPointToONNX(config=cfg, checkpoint_path=args.ckpt, output_path=args.work_dir) + det.save_onnx() + + +if __name__ == '__main__': + main() diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom.py b/projects/AutowareCenterPoint/configs/centerpoint_custom.py new file mode 100755 index 0000000000..9f466287a8 --- /dev/null +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom.py @@ -0,0 +1,847 @@ +custom_imports = dict( + imports=['projects.AutowareCenterPoint.centerpoint'], allow_failed_imports=False) + +auto_scale_lr = dict(base_batch_size=32, enable=False) +backend_args = None +class_names = [ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', +] +data_prefix = dict(img='', pts='samples/LIDAR_TOP', sweeps='sweeps/LIDAR_TOP') +data_root = 'data/nuscenes/' +dataset_type = 'NuScenesDataset' +db_sampler = dict( + backend_args=None, + classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + data_root='data/nuscenes/', + info_path='data/nuscenes/nuscenes_dbinfos_train.pkl', + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, truck=5)), + rate=1.0, + sample_groups=dict(bicycle=6, bus=4, car=2, pedestrian=2, truck=3)) +default_hooks = dict( + checkpoint=dict(interval=1, save_optimizer=True, type='CheckpointHook'), + logger=dict(interval=50, type='LoggerHook'), + param_scheduler=dict(type='ParamSchedulerHook'), + sampler_seed=dict(type='DistSamplerSeedHook'), + timer=dict(type='IterTimerHook'), + visualization=dict(type='Det3DVisualizationHook')) +default_scope = 'mmdet3d' +env_cfg = dict( + cudnn_benchmark=False, + dist_cfg=dict(backend='nccl'), + mp_cfg=dict(mp_start_method='fork', opencv_num_threads=0)) +eval_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + sweeps_num=10, + test_mode=True, + type='LoadPointsFromMultiSweeps'), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), +] +input_modality = dict(use_camera=False, use_lidar=True) +launcher = 'none' +load_from = None +log_level = 'INFO' +log_processor = dict(by_epoch=True, type='LogProcessor', window_size=50) +lr = 0.0001 +metainfo = dict(classes=[ + 'car', + 'truck', + 'trailer', + 'bus', + 'construction_vehicle', + 'bicycle', + 'motorcycle', + 'pedestrian', + 'traffic_cone', + 'barrier', +]) +model = dict( + data_preprocessor=dict( + type='Det3DDataPreprocessor', + voxel=True, + voxel_layer=dict( + max_num_points=20, + max_voxels=( + 30000, + 40000, + ), + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + voxel_size=[ + 0.2, + 0.2, + 8, + ])), + pts_backbone=dict( + conv_cfg=dict(bias=False, type='Conv2d'), + in_channels=32, + layer_nums=[ + 3, + 5, + 5, + ], + layer_strides=[ + 1, + 2, + 2, + ], + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN'), + out_channels=[ + 64, + 128, + 256, + ], + type='SECOND'), + pts_bbox_head=dict( + bbox_coder=dict( + code_size=9, + max_num=500, + out_size_factor=1, + pc_range=[ + -51.2, + -51.2, + ], + post_center_range=[ + -61.2, + -61.2, + -10.0, + 61.2, + 61.2, + 10.0, + ], + score_threshold=0.1, + type='CenterPointBBoxCoder', + voxel_size=[ + 0.2, + 0.2, + ]), + common_heads=dict( + dim=( + 3, + 2, + ), + height=( + 1, + 2, + ), + reg=( + 2, + 2, + ), + rot=( + 2, + 2, + ), + vel=( + 2, + 2, + )), + in_channels=384, + loss_bbox=dict( + loss_weight=0.25, reduction='mean', type='mmdet.L1Loss'), + loss_cls=dict(reduction='mean', type='mmdet.GaussianFocalLoss'), + norm_bbox=True, + separate_head=dict( + final_kernel=3, init_bias=-2.19, type='SeparateHead'), + share_conv_channel=64, + tasks=[ + dict( + class_names=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + num_class=5), + ], + type='CenterHead'), + pts_middle_encoder=dict( + in_channels=32, output_shape=( + 512, + 512, + ), type='PointPillarsScatter'), + pts_neck=dict( + in_channels=[ + 64, + 128, + 256, + ], + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN'), + out_channels=[ + 128, + 128, + 128, + ], + type='SECONDFPN', + upsample_cfg=dict(bias=False, type='deconv'), + upsample_strides=[ + 1, + 2, + 4, + ], + use_conv_for_no_stride=True), + pts_voxel_encoder=dict( + feat_channels=[ + 32, + 32, + ], + in_channels=4, + legacy=False, + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN1d'), + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + type='PillarFeatureNetAutoware', + voxel_size=( + 0.2, + 0.2, + 8, + ), + use_voxel_center_z=False, + with_distance=False), + test_cfg=dict( + pts=dict( + max_per_img=500, + max_pool_nms=False, + min_radius=[ + 4, + 12, + 10, + 1, + 0.85, + 0.175, + ], + nms_thr=0.2, + nms_type='circle', + out_size_factor=1, + pc_range=[ + -51.2, + -51.2, + ], + post_center_limit_range=[ + -61.2, + -61.2, + -10.0, + 61.2, + 61.2, + 10.0, + ], + post_max_size=83, + pre_max_size=1000, + score_threshold=0.1, + voxel_size=[ + 0.2, + 0.2, + ])), + train_cfg=dict( + pts=dict( + code_weights=[ + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 0.2, + 0.2, + ], + dense_reg=1, + gaussian_overlap=0.1, + grid_size=[ + 512, + 512, + 1, + ], + max_objs=500, + min_radius=2, + out_size_factor=1, + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + voxel_size=[ + 0.2, + 0.2, + 8, + ])), + type='CenterPoint') +optim_wrapper = dict( + clip_grad=dict(max_norm=35, norm_type=2), + optimizer=dict(lr=0.0001, type='AdamW', weight_decay=0.01), + type='OptimWrapper') +out_size_factor = 1 +param_scheduler = [ + dict( + T_max=8, + begin=0, + by_epoch=True, + convert_to_iter_based=True, + end=8, + eta_min=0.001, + type='CosineAnnealingLR'), + dict( + T_max=12, + begin=8, + by_epoch=True, + convert_to_iter_based=True, + end=20, + eta_min=1e-08, + type='CosineAnnealingLR'), + dict( + T_max=8, + begin=0, + by_epoch=True, + convert_to_iter_based=True, + end=8, + eta_min=0.8947368421052632, + type='CosineAnnealingMomentum'), + dict( + T_max=12, + begin=8, + by_epoch=True, + convert_to_iter_based=True, + end=20, + eta_min=1, + type='CosineAnnealingMomentum'), +] +point_cloud_range = [ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, +] +point_load_dim = 5 +point_use_dim = [ + 0, + 1, + 2, + 4, +] +resume = False +test_cfg = dict() +test_dataloader = dict( + batch_size=1, + dataset=dict( + ann_file='nuscenes_infos_val.pkl', + backend_args=None, + box_type_3d='LiDAR', + data_prefix=dict( + img='', pts='samples/LIDAR_TOP', sweeps='sweeps/LIDAR_TOP'), + data_root='data/nuscenes/', + metainfo=dict(classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ]), + modality=dict(use_camera=False, use_lidar=True), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=9, + type='LoadPointsFromMultiSweeps', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + dict( + flip=False, + img_scale=( + 1333, + 800, + ), + pts_scale_ratio=1, + transforms=[ + dict( + rot_range=[ + 0, + 0, + ], + scale_ratio_range=[ + 1.0, + 1.0, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict(type='RandomFlip3D'), + ], + type='MultiScaleFlipAug3D'), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), + ], + test_mode=True, + type='NuScenesDataset'), + drop_last=False, + num_workers=1, + persistent_workers=True, + sampler=dict(shuffle=False, type='DefaultSampler')) +test_evaluator = dict( + ann_file='data/nuscenes/nuscenes_infos_val.pkl', + backend_args=None, + data_root='data/nuscenes/', + metric='bbox', + type='NuScenesMetric') +test_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=9, + type='LoadPointsFromMultiSweeps', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + dict( + flip=False, + img_scale=( + 1333, + 800, + ), + pts_scale_ratio=1, + transforms=[ + dict( + rot_range=[ + 0, + 0, + ], + scale_ratio_range=[ + 1.0, + 1.0, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict(type='RandomFlip3D'), + ], + type='MultiScaleFlipAug3D'), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), +] +train_cfg = dict(by_epoch=True, max_epochs=20, val_interval=5) +train_dataloader = dict( + batch_size=2, + dataset=dict( + dataset=dict( + ann_file='nuscenes_infos_train.pkl', + backend_args=None, + box_type_3d='LiDAR', + data_prefix=dict( + img='', pts='samples/LIDAR_TOP', sweeps='sweeps/LIDAR_TOP'), + data_root='data/nuscenes/', + metainfo=dict(classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ]), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=9, + type='LoadPointsFromMultiSweeps', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + dict( + type='LoadAnnotations3D', + with_bbox_3d=True, + with_label_3d=True), + dict( + db_sampler=dict( + backend_args=None, + classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + data_root='data/nuscenes/', + info_path='data/nuscenes/nuscenes_dbinfos_train.pkl', + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, + truck=5)), + rate=1.0, + sample_groups=dict( + bicycle=6, bus=4, car=2, pedestrian=2, truck=3)), + type='ObjectSample'), + dict( + rot_range=[ + -0.3925, + 0.3925, + ], + scale_ratio_range=[ + 0.95, + 1.05, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict( + flip_ratio_bev_horizontal=0.5, + flip_ratio_bev_vertical=0.5, + sync_2d=False, + type='RandomFlip3D'), + dict( + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + type='PointsRangeFilter'), + dict( + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + type='ObjectRangeFilter'), + dict( + classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + type='ObjectNameFilter'), + dict(type='PointShuffle'), + dict( + keys=[ + 'points', + 'gt_bboxes_3d', + 'gt_labels_3d', + ], + type='Pack3DDetInputs'), + ], + test_mode=False, + type='NuScenesDataset', + use_valid_flag=True), + type='CBGSDataset'), + num_workers=4, + persistent_workers=True, + sampler=dict(shuffle=True, type='DefaultSampler')) +train_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=9, + type='LoadPointsFromMultiSweeps', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + dict(type='LoadAnnotations3D', with_bbox_3d=True, with_label_3d=True), + dict( + db_sampler=dict( + backend_args=None, + classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + data_root='data/nuscenes/', + info_path='data/nuscenes/nuscenes_dbinfos_train.pkl', + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, truck=5)), + rate=1.0, + sample_groups=dict(bicycle=6, bus=4, car=2, pedestrian=2, + truck=3)), + type='ObjectSample'), + dict( + rot_range=[ + -0.3925, + 0.3925, + ], + scale_ratio_range=[ + 0.95, + 1.05, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict( + flip_ratio_bev_horizontal=0.5, + flip_ratio_bev_vertical=0.5, + sync_2d=False, + type='RandomFlip3D'), + dict( + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + type='PointsRangeFilter'), + dict( + point_cloud_range=[ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, + ], + type='ObjectRangeFilter'), + dict( + classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ], + type='ObjectNameFilter'), + dict(type='PointShuffle'), + dict( + keys=[ + 'points', + 'gt_bboxes_3d', + 'gt_labels_3d', + ], + type='Pack3DDetInputs'), +] +val_cfg = dict() +val_dataloader = dict( + batch_size=1, + dataset=dict( + ann_file='nuscenes_infos_val.pkl', + backend_args=None, + box_type_3d='LiDAR', + data_prefix=dict( + img='', pts='samples/LIDAR_TOP', sweeps='sweeps/LIDAR_TOP'), + data_root='data/nuscenes/', + metainfo=dict(classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ]), + modality=dict(use_camera=False, use_lidar=True), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=5, + type='LoadPointsFromFile', + use_dim=5), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=9, + type='LoadPointsFromMultiSweeps', + use_dim=[ + 0, + 1, + 2, + 4, + ]), + dict( + flip=False, + img_scale=( + 1333, + 800, + ), + pts_scale_ratio=1, + transforms=[ + dict( + rot_range=[ + 0, + 0, + ], + scale_ratio_range=[ + 1.0, + 1.0, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict(type='RandomFlip3D'), + ], + type='MultiScaleFlipAug3D'), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), + ], + test_mode=True, + type='NuScenesDataset'), + drop_last=False, + num_workers=1, + persistent_workers=True, + sampler=dict(shuffle=False, type='DefaultSampler')) +val_evaluator = dict( + ann_file='data/nuscenes/nuscenes_infos_val.pkl', + backend_args=None, + data_root='data/nuscenes/', + metric='bbox', + type='NuScenesMetric') +vis_backends = [ + dict(type='LocalVisBackend'), +] +visualizer = dict( + name='visualizer', + type='Det3DLocalVisualizer', + vis_backends=[ + dict(type='LocalVisBackend'), + ]) +voxel_size = [ + 0.2, + 0.2, + 8, +] +work_dir = './work_dirs/centerpoint_pillar02_second_secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d-CUSTOM' \ No newline at end of file diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py new file mode 100644 index 0000000000..21e08c9cb4 --- /dev/null +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py @@ -0,0 +1,607 @@ +custom_imports = dict( + imports=['projects.AutowareCenterPoint.centerpoint', + 'projects.AutowareCenterPoint.datasets', + 'projects.AutowareCenterPoint.evaluation'], allow_failed_imports=False) + +auto_scale_lr = dict(base_batch_size=32, enable=False) +backend_args = None + +use_dim =[0,1,2,4,] +use_dim_num = 5 +load_dim_num = 5 +point_cloud_range = [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0,] + +class_names = [ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', +] + +voxel_size=[0.2, 0.2, 8.0,] + +data_prefix = dict(img='', pts='', sweeps='') +data_root = "data/sample_dataset/" +dataset_type = 'Tier4Dataset' + +train_ann_file = dataset_type + '_infos_train.pkl' +val_ann_file = dataset_type + '_infos_val.pkl' +test_ann_file = dataset_type + '_infos_train.pkl' + +db_info_path = data_root + '/' + dataset_type + '_dbinfos_train.pkl' + +db_sampler = dict( + backend_args=None, + classes=class_names, + data_root=data_root, + info_path=db_info_path, + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, truck=5)), + rate=1.0, + sample_groups=dict(bicycle=6, bus=4, car=2, pedestrian=2, truck=3)) +default_hooks = dict( + checkpoint=dict(interval=1, save_optimizer=True, type='CheckpointHook'), + logger=dict(interval=50, type='LoggerHook'), + param_scheduler=dict(type='ParamSchedulerHook'), + sampler_seed=dict(type='DistSamplerSeedHook'), + timer=dict(type='IterTimerHook'), + visualization=dict(type='Det3DVisualizationHook')) +default_scope = 'mmdet3d' +env_cfg = dict( + cudnn_benchmark=False, + dist_cfg=dict(backend='nccl'), + mp_cfg=dict(mp_start_method='fork', opencv_num_threads=0)) +eval_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + sweeps_num=0, + test_mode=True, + type='LoadPointsFromMultiSweeps'), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), +] +input_modality = dict(use_camera=False, use_lidar=True) +launcher = 'none' +load_from = None +log_level = 'INFO' +log_processor = dict(by_epoch=True, type='LogProcessor', window_size=50) +lr = 0.0001 +metainfo = dict(classes=class_names) +model = dict( + data_preprocessor=dict( + type='Det3DDataPreprocessor', + voxel=True, + voxel_layer=dict( + max_num_points=20, + max_voxels=( + 30000, + 40000, + ), + point_cloud_range=point_cloud_range, + voxel_size=voxel_size)), + pts_backbone=dict( + conv_cfg=dict(bias=False, type='Conv2d'), + in_channels=32, + layer_nums=[ + 3, + 5, + 5, + ], + layer_strides=[ + 1, + 2, + 2, + ], + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN'), + out_channels=[ + 64, + 128, + 256, + ], + type='SECOND'), + pts_bbox_head=dict( + bbox_coder=dict( + code_size=9, + max_num=500, + out_size_factor=1, + pc_range=[ + -51.2, + -51.2, + ], + post_center_range=[ + -61.2, + -61.2, + -10.0, + 61.2, + 61.2, + 10.0, + ], + score_threshold=0.1, + type='CenterPointBBoxCoder', + voxel_size=[ + 0.2, + 0.2, + ]), + common_heads=dict( + dim=( + 3, + 2, + ), + height=( + 1, + 2, + ), + reg=( + 2, + 2, + ), + rot=( + 2, + 2, + ), + vel=( + 2, + 2, + )), + in_channels=384, + loss_bbox=dict( + loss_weight=0.25, reduction='mean', type='mmdet.L1Loss'), + loss_cls=dict(reduction='mean', type='mmdet.GaussianFocalLoss'), + norm_bbox=True, + separate_head=dict( + final_kernel=3, init_bias=-2.19, type='SeparateHead'), + share_conv_channel=64, + tasks=[ + dict( + class_names=class_names, + num_class=5), + ], + type='CenterHead'), + pts_middle_encoder=dict( + in_channels=32, output_shape=( + 512, + 512, + ), type='PointPillarsScatter'), + pts_neck=dict( + in_channels=[ + 64, + 128, + 256, + ], + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN'), + out_channels=[ + 128, + 128, + 128, + ], + type='SECONDFPN', + upsample_cfg=dict(bias=False, type='deconv'), + upsample_strides=[ + 1, + 2, + 4, + ], + use_conv_for_no_stride=True), + pts_voxel_encoder=dict( + feat_channels=[ + 32, + 32, + ], + in_channels=4, + legacy=False, + norm_cfg=dict(eps=0.001, momentum=0.01, type='BN1d'), + point_cloud_range=point_cloud_range, + type='PillarFeatureNetAutoware', + voxel_size=voxel_size, + with_distance=False, + use_voxel_center_z=False), + test_cfg=dict( + pts=dict( + max_per_img=500, + max_pool_nms=False, + min_radius=[ + 4, + 12, + 10, + 1, + 0.85, + 0.175, + ], + nms_thr=0.2, + nms_type='circle', + out_size_factor=1, + pc_range=[ + -51.2, + -51.2, + ], + post_center_limit_range=[ + -61.2, + -61.2, + -10.0, + 61.2, + 61.2, + 10.0, + ], + post_max_size=83, + pre_max_size=1000, + score_threshold=0.1, + voxel_size=[ + 0.2, + 0.2, + ])), + train_cfg=dict( + pts=dict( + code_weights=[ + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 0.2, + 0.2, + ], + dense_reg=1, + gaussian_overlap=0.1, + grid_size=[ + 512, + 512, + 1, + ], + max_objs=500, + min_radius=2, + out_size_factor=1, + point_cloud_range=point_cloud_range, + voxel_size=voxel_size)), + type='CenterPoint') +optim_wrapper = dict( + clip_grad=dict(max_norm=35, norm_type=2), + optimizer=dict(lr=0.0001, type='AdamW', weight_decay=0.01), + type='OptimWrapper') +out_size_factor = 1 +param_scheduler = [ + dict( + T_max=8, + begin=0, + by_epoch=True, + convert_to_iter_based=True, + end=8, + eta_min=0.001, + type='CosineAnnealingLR'), + dict( + T_max=12, + begin=8, + by_epoch=True, + convert_to_iter_based=True, + end=20, + eta_min=1e-08, + type='CosineAnnealingLR'), + dict( + T_max=8, + begin=0, + by_epoch=True, + convert_to_iter_based=True, + end=8, + eta_min=0.8947368421052632, + type='CosineAnnealingMomentum'), + dict( + T_max=12, + begin=8, + by_epoch=True, + convert_to_iter_based=True, + end=20, + eta_min=1, + type='CosineAnnealingMomentum'), +] + +resume = False +test_cfg = dict() +test_dataloader = dict( + batch_size=1, + dataset=dict( + ann_file=test_ann_file, + backend_args=None, + box_type_3d='LiDAR', + data_prefix=data_prefix, + data_root=data_root, + metainfo=dict(classes=[ + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ]), + modality=dict(use_camera=False, use_lidar=True), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=0, + type='LoadPointsFromMultiSweeps', + use_dim=use_dim), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), + ], + test_mode=True, + type='Tier4Dataset'), + drop_last=False, + num_workers=1, + persistent_workers=True, + sampler=dict(shuffle=False, type='DefaultSampler')) +test_evaluator = dict( + ann_file= data_root + test_ann_file, + backend_args=None, + data_root=data_root, + metric='bbox', + type='NuScenesCustomMetric') +test_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=0, + type='LoadPointsFromMultiSweeps', + use_dim=use_dim), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), +] +train_cfg = dict(by_epoch=True, max_epochs=20, val_interval=5) +train_dataloader = dict( + batch_size=2, + dataset=dict( + dataset=dict( + ann_file=train_ann_file, + backend_args=None, + box_type_3d='LiDAR', + data_prefix=data_prefix, + data_root=data_root, + metainfo=dict(classes=class_names), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=0, + type='LoadPointsFromMultiSweeps', + use_dim=use_dim), + dict( + type='LoadAnnotations3D', + with_bbox_3d=True, + with_label_3d=True), + dict( + db_sampler=dict( + backend_args=None, + classes=class_names, + data_root=data_root, + info_path=data_root + train_ann_file, + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, + truck=5)), + rate=1.0, + sample_groups=dict( + bicycle=6, bus=4, car=2, pedestrian=2, truck=3)), + type='ObjectSample'), + dict( + rot_range=[ + -0.3925, + 0.3925, + ], + scale_ratio_range=[ + 0.95, + 1.05, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict( + flip_ratio_bev_horizontal=0.5, + flip_ratio_bev_vertical=0.5, + sync_2d=False, + type='RandomFlip3D'), + dict( + point_cloud_range=point_cloud_range, + type='PointsRangeFilter'), + dict( + point_cloud_range=point_cloud_range, + type='ObjectRangeFilter'), + dict( + classes=class_names, + type='ObjectNameFilter'), + dict(type='PointShuffle'), + dict( + keys=[ + 'points', + 'gt_bboxes_3d', + 'gt_labels_3d', + ], + type='Pack3DDetInputs'), + ], + test_mode=False, + type=dataset_type, + use_valid_flag=True), + type='CBGSDataset'), + num_workers=4, + persistent_workers=True, + sampler=dict(shuffle=True, type='DefaultSampler')) +train_pipeline = [ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=0, + type='LoadPointsFromMultiSweeps', + use_dim=use_dim), + dict(type='LoadAnnotations3D', with_bbox_3d=True, with_label_3d=True), + dict( + db_sampler=dict( + backend_args=None, + classes=class_names, + data_root=data_root, + info_path=data_root+ train_ann_file, + points_loader=dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim), + prepare=dict( + filter_by_difficulty=[ + -1, + ], + filter_by_min_points=dict( + bicycle=5, bus=5, car=5, pedestrian=5, truck=5)), + rate=1.0, + sample_groups=dict(bicycle=6, bus=4, car=2, pedestrian=2, + truck=3)), + type='ObjectSample'), + dict( + rot_range=[ + -0.3925, + 0.3925, + ], + scale_ratio_range=[ + 0.95, + 1.05, + ], + translation_std=[ + 0, + 0, + 0, + ], + type='GlobalRotScaleTrans'), + dict( + flip_ratio_bev_horizontal=0.5, + flip_ratio_bev_vertical=0.5, + sync_2d=False, + type='RandomFlip3D'), + dict( + point_cloud_range=point_cloud_range + , + type='PointsRangeFilter'), + dict( + point_cloud_range=point_cloud_range, + type='ObjectRangeFilter'), + dict( + classes=class_names, + type='ObjectNameFilter'), + dict(type='PointShuffle'), + dict( + keys=[ + 'points', + 'gt_bboxes_3d', + 'gt_labels_3d', + ], + type='Pack3DDetInputs'), +] +val_cfg = dict() +val_dataloader = dict( + batch_size=1, + dataset=dict( + ann_file=val_ann_file, + backend_args=None, + box_type_3d='LiDAR', + data_prefix=data_prefix, + data_root=data_root, + metainfo=dict(classes=class_names), + modality=dict(use_camera=False, use_lidar=True), + pipeline=[ + dict( + backend_args=None, + coord_type='LIDAR', + load_dim=load_dim_num, + type='LoadPointsFromFile', + use_dim=use_dim_num), + dict( + backend_args=None, + pad_empty_sweeps=True, + remove_close=True, + sweeps_num=0, + type='LoadPointsFromMultiSweeps', + use_dim=use_dim), + dict(keys=[ + 'points', + ], type='Pack3DDetInputs'), + ], + test_mode=True, + type='NuScenesDataset'), + drop_last=False, + num_workers=1, + persistent_workers=True, + sampler=dict(shuffle=False, type='DefaultSampler')) +val_evaluator = dict( + ann_file=data_root + val_ann_file, + backend_args=None, + data_root=data_root, + metric='bbox', + type='NuScenesCustomMetric') +vis_backends = [ + dict(type='LocalVisBackend'), +] +visualizer = dict( + name='visualizer', + type='Det3DLocalVisualizer', + vis_backends=[ + dict(type='LocalVisBackend'), + ]) + +work_dir = './work_dirs/centerpoint_custom_test' \ No newline at end of file diff --git a/projects/AutowareCenterPoint/datasets/__init__.py b/projects/AutowareCenterPoint/datasets/__init__.py new file mode 100644 index 0000000000..fba124e3db --- /dev/null +++ b/projects/AutowareCenterPoint/datasets/__init__.py @@ -0,0 +1,3 @@ +from .tier4_dataset import Tier4Dataset + +__all__ = [ 'Tier4Dataset'] \ No newline at end of file diff --git a/projects/AutowareCenterPoint/datasets/tier4_dataset.py b/projects/AutowareCenterPoint/datasets/tier4_dataset.py new file mode 100644 index 0000000000..51a43d550f --- /dev/null +++ b/projects/AutowareCenterPoint/datasets/tier4_dataset.py @@ -0,0 +1,114 @@ +from os import path as osp +import os +from typing import Callable, List, Union + +import numpy as np + +from mmdet3d.registry import DATASETS +from mmdet3d.structures import LiDARInstance3DBoxes +from mmdet3d.structures.bbox_3d.cam_box3d import CameraInstance3DBoxes +# from .det3d_dataset import Det3DDataset +from mmdet3d.datasets.nuscenes_dataset import NuScenesDataset + + +@DATASETS.register_module() +class Tier4Dataset(NuScenesDataset): + METAINFO = { + 'classes': ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), + 'version': 'v1.0-trainval', + 'palette': [ + (255, 158, 0), # Orange + (255, 99, 71), # Tomato + (255, 140, 0), # Darkorange + (255, 127, 80), # Coral + (233, 150, 70), # Darksalmon + ] + } + + def __init__(self, + box_type_3d: str = 'LiDAR', + load_type: str = 'frame_based', + with_velocity: bool = True, + use_valid_flag: bool = False, + **kwargs,) -> None: + + self.use_valid_flag = use_valid_flag + self.with_velocity = with_velocity + + # TODO: Redesign multi-view data process in the future + assert load_type in ('frame_based', 'mv_image_based', + 'fov_image_based') + self.load_type = load_type + + assert box_type_3d.lower() in ('lidar', 'camera') + super().__init__(**kwargs) + + def parse_data_info(self, info: dict) -> dict: + """Process the raw data info. + + Convert all relative path of needed modality data file to + the absolute path. And process the `instances` field to + `ann_info` in training stage. + + Args: + info (dict): Raw info dict. + + Returns: + dict: Has `ann_info` in training stage. And + all path has been converted to absolute path. + """ + if self.load_type == 'mv_image_based': + info = super().parse_data_info(info) + else: + if self.modality['use_lidar']: + info['lidar_points']['lidar_path'] = \ + osp.join( + self.data_prefix.get('pts', ''), + info['lidar_points']['lidar_path']) + + info['num_pts_feats'] = info['lidar_points']['num_pts_feats'] + info['lidar_path'] = info['lidar_points']['lidar_path'] + if 'lidar_sweeps' in info: + for sweep in info['lidar_sweeps']: + file_suffix_splitted = sweep['lidar_points']['lidar_path'].split(os.sep) + file_suffix = os.sep.join(file_suffix_splitted[-4:]) + if 'samples' in sweep['lidar_points']['lidar_path']: + sweep['lidar_points']['lidar_path'] = osp.join( + self.data_prefix['pts'], file_suffix) + else: + sweep['lidar_points']['lidar_path'] = info['lidar_points']['lidar_path'] + + if self.modality['use_camera']: + for cam_id, img_info in info['images'].items(): + if 'img_path' in img_info: + if cam_id in self.data_prefix: + cam_prefix = self.data_prefix[cam_id] + else: + cam_prefix = self.data_prefix.get('img', '') + img_info['img_path'] = osp.join(cam_prefix, + img_info['img_path']) + if self.default_cam_key is not None: + info['img_path'] = info['images'][ + self.default_cam_key]['img_path'] + if 'lidar2cam' in info['images'][self.default_cam_key]: + info['lidar2cam'] = np.array( + info['images'][self.default_cam_key]['lidar2cam']) + if 'cam2img' in info['images'][self.default_cam_key]: + info['cam2img'] = np.array( + info['images'][self.default_cam_key]['cam2img']) + if 'lidar2img' in info['images'][self.default_cam_key]: + info['lidar2img'] = np.array( + info['images'][self.default_cam_key]['lidar2img']) + else: + info['lidar2img'] = info['cam2img'] @ info['lidar2cam'] + + if not self.test_mode: + # used in training + info['ann_info'] = self.parse_ann_info(info) + if self.test_mode and self.load_eval_anns: + info['eval_ann_info'] = self.parse_ann_info(info) + + return info + + + diff --git a/projects/AutowareCenterPoint/evaluation/__init__.py b/projects/AutowareCenterPoint/evaluation/__init__.py new file mode 100644 index 0000000000..75a2057d51 --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/__init__.py @@ -0,0 +1,17 @@ +from .functional.nuscenes_utils.eval import DetectionConfig, nuScenesDetectionEval +from .functional.nuscenes_utils.utils import ( + class_mapping_kitti2nuscenes, + format_nuscenes_metrics, + format_nuscenes_metrics_table, + transform_det_annos_to_nusc_annos, +) + +from .metrics.nuscenes_custom_metric import NuScenesCustomMetric + +__all__ = ['NuScenesCustomMetric,' + 'DetectionConfig' + 'nuScenesDetectionEval' + 'class_mapping_kitti2nuscenes' + 'format_nuscenes_metrics_table' + 'format_nuscenes_metrics' + 'transform_det_annos_to_nusc_annos'] diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py new file mode 100644 index 0000000000..85608f4c01 --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py @@ -0,0 +1,16 @@ +from .eval import DetectionConfig, nuScenesDetectionEval +from .utils import ( + class_mapping_kitti2nuscenes, + format_nuscenes_metrics, + format_nuscenes_metrics_table, + transform_det_annos_to_nusc_annos, +) + +__all__ = [ + "DetectionConfig", + "nuScenesDetectionEval", + "class_mapping_kitti2nuscenes", + "format_nuscenes_metrics_table", + "format_nuscenes_metrics", + "transform_det_annos_to_nusc_annos", +] diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py new file mode 100644 index 0000000000..755b582910 --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py @@ -0,0 +1,172 @@ +import json +import os +from typing import Dict, List, Optional, Tuple + +import numpy as np +from nuscenes.eval.common.data_classes import EvalBox, EvalBoxes +from nuscenes.eval.common.loaders import load_prediction +from nuscenes.eval.common.utils import center_distance +from nuscenes.eval.detection.data_classes import DetectionBox +from nuscenes.eval.detection.evaluate import DetectionEval as _DetectionEval + +def toEvalBoxes(nusc_boxes: Dict[str, List[Dict]], box_cls: EvalBox = DetectionBox) -> EvalBoxes: + """ + + nusc_boxes = { + "sample_token_1": [ + { + "sample_token": str, + "translation": List[float], (x, y, z) + "size": List[float], (width, length, height) + "rotation": List[float], (w, x, y, z) + "velocity": List[float], (vx, vy) + "detection_name": str, + "detection_score": float, + "attribute_name": str, + }, + ... + ], + ... + } + + Args: + nusc_boxes (Dict[List[Dict]]): [description] + box_cls (EvalBox, optional): [description]. Defaults to DetectionBox. + + Returns: + EvalBoxes: [description] + """ + return EvalBoxes.deserialize(nusc_boxes, box_cls) + + +class DetectionConfig: + """Data class that specifies the detection evaluation settings.""" + + def __init__( + self, + class_names: List[str], + class_range: Dict[str, int], + dist_fcn: str, + dist_ths: List[float], + dist_th_tp: float, + min_recall: float, + min_precision: float, + max_boxes_per_sample: float, + mean_ap_weight: int, + ): + + # assert set(class_range.keys()) == set(DETECTION_NAMES), "Class count mismatch." + assert dist_th_tp in dist_ths, "dist_th_tp must be in set of dist_ths." + + self.class_range = class_range + self.dist_fcn = dist_fcn + self.dist_ths = dist_ths + self.dist_th_tp = dist_th_tp + self.min_recall = min_recall + self.min_precision = min_precision + self.max_boxes_per_sample = max_boxes_per_sample + self.mean_ap_weight = mean_ap_weight + + self.class_names = class_names + + def __eq__(self, other): + eq = True + for key in self.serialize().keys(): + eq = eq and np.array_equal(getattr(self, key), getattr(other, key)) + return eq + + def serialize(self) -> dict: + """Serialize instance into json-friendly format.""" + return { + "class_names": self.class_names, + "class_range": self.class_range, + "dist_fcn": self.dist_fcn, + "dist_ths": self.dist_ths, + "dist_th_tp": self.dist_th_tp, + "min_recall": self.min_recall, + "min_precision": self.min_precision, + "max_boxes_per_sample": self.max_boxes_per_sample, + "mean_ap_weight": self.mean_ap_weight, + } + + @classmethod + def deserialize(cls, content: dict): + """Initialize from serialized dictionary.""" + return cls( + content["class_names"], + content["class_range"], + content["dist_fcn"], + content["dist_ths"], + content["dist_th_tp"], + content["min_recall"], + content["min_precision"], + content["max_boxes_per_sample"], + content["mean_ap_weight"], + ) + + @property + def dist_fcn_callable(self): + """Return the distance function corresponding to the dist_fcn string.""" + if self.dist_fcn == "center_distance": + return center_distance + else: + raise Exception("Error: Unknown distance function %s!" % self.dist_fcn) + + +class nuScenesDetectionEval(_DetectionEval): + """ + This is the official nuScenes detection evaluation code. + Results are written to the provided output_dir. + nuScenes uses the following detection metrics: + - Mean Average Precision (mAP): Uses center-distance as matching criterion; averaged over distance thresholds. + - True Positive (TP) metrics: Average of translation, velocity, scale, orientation and attribute errors. + - nuScenes Detection Score (NDS): The weighted sum of the above. + Here is an overview of the functions in this method: + - init: Loads GT annotations and predictions stored in JSON format and filters the boxes. + - run: Performs evaluation and dumps the metric data to disk. + - render: Renders various plots and dumps to disk. + We assume that: + - Every sample_token is given in the results, although there may be not predictions for that sample. + Please see https://www.nuscenes.org/object-detection for more details. + """ + + def __init__( + self, + config: DetectionConfig, + result_boxes: Dict, + gt_boxes: Dict, + meta: Dict, + eval_set: str, + output_dir: Optional[str] = None, + verbose: bool = True, + ): + """ + Initialize a DetectionEval object. + :param config: A DetectionConfig object. + :param result_boxes: result bounding boxes. + :param gt_boxes: ground-truth bounding boxes. + :param eval_set: The dataset split to evaluate on, e.g. train, val or test. + :param output_dir: Folder to save plots and results to. + :param verbose: Whether to print to stdout. + """ + self.cfg = config + self.meta = meta + self.eval_set = eval_set + self.output_dir = output_dir + self.verbose = verbose + + # Make dirs. + self.plot_dir = os.path.join(self.output_dir, "plots") + if not os.path.isdir(self.output_dir): + os.makedirs(self.output_dir) + if not os.path.isdir(self.plot_dir): + os.makedirs(self.plot_dir) + + self.pred_boxes: EvalBoxes = toEvalBoxes(result_boxes) + self.gt_boxes: EvalBoxes = toEvalBoxes(gt_boxes) + + assert set(self.pred_boxes.sample_tokens) == set( + self.gt_boxes.sample_tokens + ), "Samples in split doesn't match samples in predictions." + + self.sample_tokens = self.gt_boxes.sample_tokens diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py new file mode 100644 index 0000000000..0e186dc52a --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py @@ -0,0 +1,302 @@ +""" +modified from https://github.com/open-mmlab/OpenPCDet/blob/v0.5.2/pcdet/datasets/nuscenes/nuscenes_utils.py +""" +from collections import defaultdict +import operator +from typing import Dict, List + +from mmengine import print_log +import numpy as np +from nuscenes.utils.data_classes import Box +import pandas as pd +from pyquaternion import Quaternion +from terminaltables import AsciiTable + +class_mapping_kitti2nuscenes = { + "Car": "car", + "Cyclist": "bicycle", + "Pedestrian": "pedestrian", +} + +cls_attr_dist = { + "barrier": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 0, + "vehicle.parked": 0, + "vehicle.stopped": 0, + }, + "bicycle": { + "cycle.with_rider": 2791, + "cycle.without_rider": 8946, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 0, + "vehicle.parked": 0, + "vehicle.stopped": 0, + }, + "bus": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 9092, + "vehicle.parked": 3294, + "vehicle.stopped": 3881, + }, + "car": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 114304, + "vehicle.parked": 330133, + "vehicle.stopped": 46898, + }, + "construction_vehicle": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 882, + "vehicle.parked": 11549, + "vehicle.stopped": 2102, + }, + "ignore": { + "cycle.with_rider": 307, + "cycle.without_rider": 73, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 165, + "vehicle.parked": 400, + "vehicle.stopped": 102, + }, + "motorcycle": { + "cycle.with_rider": 4233, + "cycle.without_rider": 8326, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 0, + "vehicle.parked": 0, + "vehicle.stopped": 0, + }, + "pedestrian": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 157444, + "pedestrian.sitting_lying_down": 13939, + "pedestrian.standing": 46530, + "vehicle.moving": 0, + "vehicle.parked": 0, + "vehicle.stopped": 0, + }, + "traffic_cone": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 0, + "vehicle.parked": 0, + "vehicle.stopped": 0, + }, + "trailer": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 3421, + "vehicle.parked": 19224, + "vehicle.stopped": 1895, + }, + "truck": { + "cycle.with_rider": 0, + "cycle.without_rider": 0, + "pedestrian.moving": 0, + "pedestrian.sitting_lying_down": 0, + "pedestrian.standing": 0, + "vehicle.moving": 21339, + "vehicle.parked": 55626, + "vehicle.stopped": 11097, + }, +} + + +def boxes_lidar_to_nuscenes(det_info): + boxes3d = det_info["boxes_lidar"] + scores = det_info["score"] + labels = det_info["pred_labels"] + + box_list = [] + for k in range(boxes3d.shape[0]): + quat = Quaternion(axis=[0, 0, 1], radians=boxes3d[k, 6]) + velocity = (*boxes3d[k, 7:9], 0.0) if boxes3d.shape[1] == 9 else (0.0, 0.0, 0.0) + box = Box( + boxes3d[k, :3], + boxes3d[k, 3:6], # wlh + quat, + label=labels[k], + score=scores[k], + velocity=velocity, + ) + box_list.append(box) + return box_list + + +def transform_det_annos_to_nusc_annos(det_annos, velocity_threshold=0.2): + """convert detections to nuscenes-format for evaluation + modified from: https://github.com/open-mmlab/OpenPCDet/blob/master/pcdet/datasets/nuscenes/nuscenes_utils.py#L423 + + Args: + det_annos ([type]): [description] + velocity_threshold (float, optional): [description]. Defaults to 0.2. + + det_annos = [ + { + boxes_lidar: (np.ndarray, float), + pred_labels: (np.ndarray, int), + score: (np.ndarray, float), + name: (np.ndarray, str), + metadata: { + token: (str) + } + }, + ... + ] + + Returns: + nusc_annos (dict[list]): [description] + """ + + nusc_annos = {} + + for det in det_annos: + annos = [] + box_list = boxes_lidar_to_nuscenes(det) + + for k, box in enumerate(box_list): + name = det["name"][k] + if np.sqrt(box.velocity[0] ** 2 + box.velocity[1] ** 2) > velocity_threshold: + if name in ["car", "construction_vehicle", "bus", "truck", "trailer"]: + attr = "vehicle.moving" + elif name in ["bicycle", "motorcycle"]: + attr = "cycle.with_rider" + else: + attr = None + else: + if name in ["pedestrian"]: + attr = "pedestrian.standing" + elif name in ["bus"]: + attr = "vehicle.stopped" + else: + attr = None + attr = ( + attr + if attr is not None + else max(cls_attr_dist[name].items(), key=operator.itemgetter(1))[0] + ) + nusc_anno = { + "sample_token": det["metadata"]["token"], + "translation": box.center.tolist(), + "size": box.wlh.tolist(), + "rotation": box.orientation.elements.tolist(), + "velocity": box.velocity[:2].tolist(), + "detection_name": name, + "detection_score": box.score, + "attribute_name": attr, + } + annos.append(nusc_anno) + + nusc_annos.update({det["metadata"]["token"]: annos}) + + return nusc_annos + + +def format_nuscenes_metrics(metrics: Dict, class_names: List[str], version="default"): + result = f"----------------nuScenes {version} results-----------------\n" + + result_dict: Dict[str, Dict[str, float]] = defaultdict(dict) + for name in class_names: + result_dict[name].update( + { + f"mAP": sum([v for v in metrics["label_aps"][name].values()]) + * 100 + / len(metrics["label_aps"][name]) + } + ) + result_dict[name].update( + {f"AP@{k}m": v * 100 for k, v in metrics["label_aps"][name].items()} + ) + result_dict[name].update( + {f"error@{k}": v for k, v in metrics["label_tp_errors"][name].items()} + ) + result_dict[name].pop("error@attr_err") + + df = pd.DataFrame.from_dict(result_dict, orient="index") + df.index.name = "class_name" + result += df.to_markdown(mode="str", floatfmt=".2f") + details = {} + for key, val in metrics["tp_errors"].items(): + details[key] = val + + for key, val in metrics["mean_dist_aps"].items(): + details[f"mAP_{key}"] = val + + details.update( + { + "mAP": metrics["mean_ap"], + "NDS": metrics["nd_score"], + } + ) + + return result, details + + +def format_nuscenes_metrics_table(metrics, class_names, logger=None): + name = class_names[0] + thresholds = list(map(str, metrics["label_aps"][name].keys())) + # not use error@attr + errors = [x.split("_")[0] for x in list(metrics["label_tp_errors"][name].keys())][:4] + + APs_data = [ + ["class", "mAP"] + [f"AP@{e}" for e in thresholds] + [f"error@{e}" for e in errors] + ] + + for name in class_names: + ap_list = list(metrics["label_aps"][name].values()) + error_list = list(metrics["label_tp_errors"][name].values())[:4] + + mAP = round(metrics["mean_dist_aps"][name] * 100, 3) + AP = ( + [name, mAP] + + [round(ap * 100, 3) for ap in ap_list] + + [round(e, 3) for e in error_list] + ) + APs_data.append(AP) + + APs_table = AsciiTable(APs_data) + # APs_table.inner_footing_row_border = True + print_log("\n" + APs_table.table, logger=logger) + + details = {} + for key, val in metrics["tp_errors"].items(): + details[key] = val + + details.update( + { + "mAP": metrics["mean_ap"], + "NDS": metrics["nd_score"], + } + ) + + return details diff --git a/projects/AutowareCenterPoint/evaluation/metrics/__init__.py b/projects/AutowareCenterPoint/evaluation/metrics/__init__.py new file mode 100644 index 0000000000..ab243f9572 --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/metrics/__init__.py @@ -0,0 +1,3 @@ +from .nuscenes_custom_metric import NuScenesCustomMetric + +__all__ = ['NuScenesCustomMetric'] diff --git a/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py new file mode 100644 index 0000000000..104194df45 --- /dev/null +++ b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py @@ -0,0 +1,870 @@ +# Copyright (c) OpenMMLab. All rights reserved. +import tempfile +from os import path as osp +from typing import Dict, List, Optional, Sequence, Tuple, Union + +import mmengine +import numpy as np +import pyquaternion +import torch +from mmengine import Config, load +from mmengine.evaluator import BaseMetric +from mmengine.logging import MMLogger +from nuscenes.eval.detection.config import config_factory +from nuscenes.eval.detection.data_classes import DetectionConfig +from nuscenes.utils.data_classes import Box as NuScenesBox + +from mmdet3d.models.layers import box3d_multiclass_nms +from mmdet3d.registry import METRICS +from mmdet3d.structures import (CameraInstance3DBoxes, LiDARInstance3DBoxes, + bbox3d2result, xywhr2xyxyr) + +from projects.AutowareCenterPoint.evaluation.functional import nuscenes_utils + +import mmcv +import mmengine + +@METRICS.register_module() +class NuScenesCustomMetric(BaseMetric): + """Nuscenes evaluation metric for custom dataset. + + Args: + data_root (str): Path of dataset root. + ann_file (str): Path of annotation file. + metric (str or List[str]): Metrics to be evaluated. Defaults to 'bbox'. + modality (dict): Modality to specify the sensor data used as input. + Defaults to dict(use_camera=False, use_lidar=True). + prefix (str, optional): The prefix that will be added in the metric + names to disambiguate homonymous metrics of different evaluators. + If prefix is not provided in the argument, self.default_prefix will + be used instead. Defaults to None. + format_only (bool): Format the output results without perform + evaluation. It is useful when you want to format the result to a + specific format and submit it to the test server. + Defaults to False. + jsonfile_prefix (str, optional): The prefix of json files including the + file path and the prefix of filename, e.g., "a/b/prefix". + If not specified, a temp file will be created. Defaults to None. + eval_version (str): Configuration version of evaluation. + Defaults to 'detection_cvpr_2019'. + collect_device (str): Device name used for collecting results from + different ranks during distributed training. Must be 'cpu' or + 'gpu'. Defaults to 'cpu'. + backend_args (dict, optional): Arguments to instantiate the + corresponding backend. Defaults to None. + """ + CLASSES = ( + "car", + "truck", + "bus", + "bicycle", + "pedestrian", + ) + + NameMapping = { + 'movable_object.barrier': 'barrier', + 'vehicle.bicycle': 'bicycle', + 'vehicle.bus.bendy': 'bus', + 'vehicle.bus.rigid': 'bus', + 'vehicle.car': 'car', + 'vehicle.construction': 'construction_vehicle', + 'vehicle.motorcycle': 'motorcycle', + 'human.pedestrian.adult': 'pedestrian', + 'human.pedestrian.child': 'pedestrian', + 'human.pedestrian.construction_worker': 'pedestrian', + 'human.pedestrian.police_officer': 'pedestrian', + 'movable_object.trafficcone': 'traffic_cone', + 'vehicle.trailer': 'trailer', + 'vehicle.truck': 'truck' + } + DefaultAttribute = { + 'car': 'vehicle.parked', + 'pedestrian': 'pedestrian.moving', + 'trailer': 'vehicle.parked', + 'truck': 'vehicle.parked', + 'bus': 'vehicle.moving', + 'motorcycle': 'cycle.without_rider', + 'construction_vehicle': 'vehicle.parked', + 'bicycle': 'cycle.without_rider', + 'barrier': '', + 'traffic_cone': '', + } + # https://github.com/nutonomy/nuscenes-devkit/blob/57889ff20678577025326cfc24e57424a829be0a/python-sdk/nuscenes/eval/detection/evaluate.py#L222 # noqa + ErrNameMapping = { + 'trans_err': 'mATE', + 'scale_err': 'mASE', + 'orient_err': 'mAOE', + 'vel_err': 'mAVE', + 'attr_err': 'mAAE' + } + + def __init__(self, + data_root: str, + ann_file: str, + metric: Union[str, List[str]] = 'bbox', + modality: dict = dict(use_camera=False, use_lidar=True), + prefix: Optional[str] = None, + format_only: bool = False, + jsonfile_prefix: Optional[str] = None, + eval_version: str = 'detection_cvpr_2019', + collect_device: str = 'cpu', + backend_args: Optional[dict] = None) -> None: + self.default_prefix = 'NuScenes metric' + super(NuScenesCustomMetric, self).__init__( + collect_device=collect_device, prefix=prefix) + if modality is None: + modality = dict( + use_camera=False, + use_lidar=True, + ) + self.ann_file = ann_file + self.data_root = data_root + self.modality = modality + self.format_only = format_only + if self.format_only: + assert jsonfile_prefix is not None, 'jsonfile_prefix must be not ' + 'None when format_only is True, otherwise the result files will ' + 'be saved to a temp directory which will be cleanup at the end.' + + self.jsonfile_prefix = jsonfile_prefix + self.backend_args = backend_args + + self.metrics = metric if isinstance(metric, list) else [metric] + + self.eval_version = eval_version + self.eval_detection_configs = config_factory(self.eval_version) + + def process(self, data_batch: dict, data_samples: Sequence[dict]) -> None: + """Process one batch of data samples and predictions. + + The processed results should be stored in ``self.results``, which will + be used to compute the metrics when all batches have been processed. + + Args: + data_batch (dict): A batch of data from the dataloader. + data_samples (Sequence[dict]): A batch of outputs from the model. + """ + for data_sample in data_samples: + result = dict() + pred_3d = data_sample['pred_instances_3d'] + pred_2d = data_sample['pred_instances'] + for attr_name in pred_3d: + pred_3d[attr_name] = pred_3d[attr_name].to('cpu') + result['pred_instances_3d'] = pred_3d + for attr_name in pred_2d: + pred_2d[attr_name] = pred_2d[attr_name].to('cpu') + result['pred_instances'] = pred_2d + sample_idx = data_sample['sample_idx'] + result['sample_idx'] = sample_idx + self.results.append(result) + + def compute_metrics(self, results: List[dict]) -> Dict[str, float]: + """Compute the metrics from processed results. + + Args: + results (List[dict]): The processed results of each batch. + + Returns: + Dict[str, float]: The computed metrics. The keys are the names of + the metrics, and the values are corresponding results. + """ + logger: MMLogger = MMLogger.get_current_instance() + + classes = self.dataset_meta['classes'] + self.version = self.dataset_meta['version'] + + # load annotations + self.data_infos = load( + self.ann_file, backend_args=self.backend_args)['data_list'] + result_dict, tmp_dir = self.format_results(results, classes, + self.jsonfile_prefix) + metric_dict = {} + if self.format_only: + logger.info( + f'results are saved in {osp.basename(self.jsonfile_prefix)}') + return metric_dict + + for metric in self.metrics: + ap_dict = self.nus_evaluate( + result_dict, classes=classes, metric=metric, logger=logger) + for result in ap_dict: + metric_dict[result] = ap_dict[result] + + if tmp_dir is not None: + tmp_dir.cleanup() + return metric_dict + + def nus_evaluate(self, + result_dict: dict, + metric: str = 'bbox', + classes: Optional[List[str]] = None, + logger: Optional[MMLogger] = None) -> Dict[str, float]: + """Evaluation in Nuscenes protocol. + + Args: + result_dict (dict): Formatted results of the dataset. + metric (str): Metrics to be evaluated. Defaults to 'bbox'. + classes (List[str], optional): A list of class name. + Defaults to None. + logger (MMLogger, optional): Logger used for printing related + information during evaluation. Defaults to None. + + Returns: + Dict[str, float]: Results of each evaluation metric. + """ + metric_dict = dict() + for name in result_dict: + print(f'Evaluating bboxes of {name}') + ret_dict = self._evaluate_single( + result_dict[name], classes=classes, result_name=name) + metric_dict.update(ret_dict) + return metric_dict + + def _evaluate_single( + self, + result_path: str, + classes: Optional[List[str]] = None, + result_name: str = 'pred_instances_3d') -> Dict[str, float]: + """Evaluation for a single model in nuScenes protocol. + + Args: + result_path (str): Path of the result file. + classes (List[str], optional): A list of class name. + Defaults to None. + result_name (str): Result name in the metric prefix. + Defaults to 'pred_instances_3d'. + + Returns: + Dict[str, float]: Dictionary of evaluation details. + """ + from nuscenes import NuScenes + from nuscenes.eval.detection.evaluate import NuScenesEval + + output_dir = osp.join(*osp.split(result_path)[:-1]) + + result_dict = mmengine.load(result_path)["results"] + + + tmp_dir = tempfile.TemporaryDirectory() + assert tmp_dir is not None + + gt_dict = mmengine.load(self._format_gt_to_nusc(tmp_dir.name))["results"] + tmp_dir.cleanup() + + nusc_eval = nuscenes_utils.nuScenesDetectionEval( + config=self.eval_detection_configs, + result_boxes=result_dict, + gt_boxes=gt_dict, + meta=self.modality, + eval_set="val", + output_dir=output_dir, + verbose=False, + ) + + metrics, _ = nusc_eval.evaluate() + metrics_summary = metrics.serialize() + + metrics_str, ap_dict = nuscenes_utils.format_nuscenes_metrics( + metrics_summary, sorted(set(self.CLASSES), key=self.CLASSES.index) + ) + + detail = dict(result = metrics_str) + return detail + + def format_results( + self, + results: List[dict], + classes: Optional[List[str]] = None, + jsonfile_prefix: Optional[str] = None + ) -> Tuple[dict, Union[tempfile.TemporaryDirectory, None]]: + """Format the mmdet3d results to standard NuScenes json file. + + Args: + results (List[dict]): Testing results of the dataset. + classes (List[str], optional): A list of class name. + Defaults to None. + jsonfile_prefix (str, optional): The prefix of json files. It + includes the file path and the prefix of filename, e.g., + "a/b/prefix". If not specified, a temp file will be created. + Defaults to None. + + Returns: + tuple: Returns (result_dict, tmp_dir), where ``result_dict`` is a + dict containing the json filepaths, ``tmp_dir`` is the temporal + directory created for saving json files when ``jsonfile_prefix`` is + not specified. + """ + assert isinstance(results, list), 'results must be a list' + + if jsonfile_prefix is None: + tmp_dir = tempfile.TemporaryDirectory() + jsonfile_prefix = osp.join(tmp_dir.name, 'results') + else: + tmp_dir = None + result_dict = dict() + sample_idx_list = [result['sample_idx'] for result in results] + + + for name in results[0]: + if 'pred' in name and '3d' in name and name[0] != '_': + print(f'\nFormating bboxes of {name}') + results_ = [out[name] for out in results] + tmp_file_ = osp.join(jsonfile_prefix, name) + box_type_3d = type(results_[0]['bboxes_3d']) + + if box_type_3d == LiDARInstance3DBoxes: + result_dict[name] = self._format_lidar_bbox( + results_, sample_idx_list, classes, tmp_file_) + elif box_type_3d == CameraInstance3DBoxes: + result_dict[name] = self._format_camera_bbox( + results_, sample_idx_list, classes, tmp_file_) + + return result_dict, tmp_dir + + def get_attr_name(self, attr_idx: int, label_name: str) -> str: + """Get attribute from predicted index. + + This is a workaround to predict attribute when the predicted velocity + is not reliable. We map the predicted attribute index to the one in the + attribute set. If it is consistent with the category, we will keep it. + Otherwise, we will use the default attribute. + + Args: + attr_idx (int): Attribute index. + label_name (str): Predicted category name. + + Returns: + str: Predicted attribute name. + """ + # TODO: Simplify the variable name + AttrMapping_rev2 = [ + 'cycle.with_rider', 'cycle.without_rider', 'pedestrian.moving', + 'pedestrian.standing', 'pedestrian.sitting_lying_down', + 'vehicle.moving', 'vehicle.parked', 'vehicle.stopped', 'None' + ] + if label_name == 'car' or label_name == 'bus' \ + or label_name == 'truck' or label_name == 'trailer' \ + or label_name == 'construction_vehicle': + if AttrMapping_rev2[attr_idx] == 'vehicle.moving' or \ + AttrMapping_rev2[attr_idx] == 'vehicle.parked' or \ + AttrMapping_rev2[attr_idx] == 'vehicle.stopped': + return AttrMapping_rev2[attr_idx] + else: + return self.DefaultAttribute[label_name] + elif label_name == 'pedestrian': + if AttrMapping_rev2[attr_idx] == 'pedestrian.moving' or \ + AttrMapping_rev2[attr_idx] == 'pedestrian.standing' or \ + AttrMapping_rev2[attr_idx] == \ + 'pedestrian.sitting_lying_down': + return AttrMapping_rev2[attr_idx] + else: + return self.DefaultAttribute[label_name] + elif label_name == 'bicycle' or label_name == 'motorcycle': + if AttrMapping_rev2[attr_idx] == 'cycle.with_rider' or \ + AttrMapping_rev2[attr_idx] == 'cycle.without_rider': + return AttrMapping_rev2[attr_idx] + else: + return self.DefaultAttribute[label_name] + else: + return self.DefaultAttribute[label_name] + + def _format_camera_bbox(self, + results: List[dict], + sample_idx_list: List[int], + classes: Optional[List[str]] = None, + jsonfile_prefix: Optional[str] = None) -> str: + """Convert the results to the standard format. + + Args: + results (List[dict]): Testing results of the dataset. + sample_idx_list (List[int]): List of result sample idx. + classes (List[str], optional): A list of class name. + Defaults to None. + jsonfile_prefix (str, optional): The prefix of the output jsonfile. + You can specify the output directory/filename by modifying the + jsonfile_prefix. Defaults to None. + + Returns: + str: Path of the output json file. + """ + nusc_annos = {} + + print('Start to convert detection format...') + + # Camera types in Nuscenes datasets + camera_types = [ + 'CAM_FRONT', + 'CAM_FRONT_RIGHT', + 'CAM_FRONT_LEFT', + 'CAM_BACK', + 'CAM_BACK_LEFT', + 'CAM_BACK_RIGHT', + ] + + CAM_NUM = 6 + + for i, det in enumerate(mmengine.track_iter_progress(results)): + + sample_idx = sample_idx_list[i] + + frame_sample_idx = sample_idx // CAM_NUM + camera_type_id = sample_idx % CAM_NUM + + if camera_type_id == 0: + boxes_per_frame = [] + attrs_per_frame = [] + + # need to merge results from images of the same sample + annos = [] + boxes, attrs = output_to_nusc_box(det) + sample_token = self.data_infos[frame_sample_idx]['token'] + camera_type = camera_types[camera_type_id] + boxes, attrs = cam_nusc_box_to_global( + self.data_infos[frame_sample_idx], boxes, attrs, classes, + self.eval_detection_configs, camera_type) + boxes_per_frame.extend(boxes) + attrs_per_frame.extend(attrs) + # Remove redundant predictions caused by overlap of images + if (sample_idx + 1) % CAM_NUM != 0: + continue + boxes = global_nusc_box_to_cam(self.data_infos[frame_sample_idx], + boxes_per_frame, classes, + self.eval_detection_configs) + cam_boxes3d, scores, labels = nusc_box_to_cam_box3d(boxes) + # box nms 3d over 6 images in a frame + # TODO: move this global setting into config + nms_cfg = dict( + use_rotate_nms=True, + nms_across_levels=False, + nms_pre=4096, + nms_thr=0.05, + score_thr=0.01, + min_bbox_size=0, + max_per_frame=500) + nms_cfg = Config(nms_cfg) + cam_boxes3d_for_nms = xywhr2xyxyr(cam_boxes3d.bev) + boxes3d = cam_boxes3d.tensor + # generate attr scores from attr labels + attrs = labels.new_tensor([attr for attr in attrs_per_frame]) + boxes3d, scores, labels, attrs = box3d_multiclass_nms( + boxes3d, + cam_boxes3d_for_nms, + scores, + nms_cfg.score_thr, + nms_cfg.max_per_frame, + nms_cfg, + mlvl_attr_scores=attrs) + cam_boxes3d = CameraInstance3DBoxes(boxes3d, box_dim=9) + det = bbox3d2result(cam_boxes3d, scores, labels, attrs) + boxes, attrs = output_to_nusc_box(det) + boxes, attrs = cam_nusc_box_to_global( + self.data_infos[frame_sample_idx], boxes, attrs, classes, + self.eval_detection_configs) + + for i, box in enumerate(boxes): + name = classes[box.label] + attr = self.get_attr_name(attrs[i], name) + nusc_anno = dict( + sample_token=sample_token, + translation=box.center.tolist(), + size=box.wlh.tolist(), + rotation=box.orientation.elements.tolist(), + velocity=box.velocity[:2].tolist(), + detection_name=name, + detection_score=box.score, + attribute_name=attr) + annos.append(nusc_anno) + # other views results of the same frame should be concatenated + if sample_token in nusc_annos: + nusc_annos[sample_token].extend(annos) + else: + nusc_annos[sample_token] = annos + + nusc_submissions = { + 'meta': self.modality, + 'results': nusc_annos, + } + + mmengine.mkdir_or_exist(jsonfile_prefix) + res_path = osp.join(jsonfile_prefix, 'results_nusc.json') + print(f'Results writes to {res_path}') + mmengine.dump(nusc_submissions, res_path) + return res_path + + def _format_lidar_bbox(self, + results: List[dict], + sample_idx_list: List[int], + classes: Optional[List[str]] = None, + jsonfile_prefix: Optional[str] = None) -> str: + """Convert the results to the standard format. + + Args: + results (List[dict]): Testing results of the dataset. + sample_idx_list (List[int]): List of result sample idx. + classes (List[str], optional): A list of class name. + Defaults to None. + jsonfile_prefix (str, optional): The prefix of the output jsonfile. + You can specify the output directory/filename by modifying the + jsonfile_prefix. Defaults to None. + + Returns: + str: Path of the output json file. + """ + nusc_annos = {} + + print('Start to convert detection format...') + for i, det in enumerate(mmengine.track_iter_progress(results)): + annos = [] + boxes, attrs = output_to_nusc_box(det) + sample_idx = sample_idx_list[i] + sample_token = self.data_infos[sample_idx]['token'] + boxes = lidar_nusc_box_to_global(self.data_infos[sample_idx], + boxes, classes, + self.eval_detection_configs) + for i, box in enumerate(boxes): + name = classes[box.label] + nusc_anno = dict( + sample_token=sample_token, + translation=box.center.tolist(), + size=box.wlh.tolist(), + rotation=box.orientation.elements.tolist(), + velocity=box.velocity[:2].tolist(), + detection_name=name, + detection_score=box.score, + attribute_name=self.DefaultAttribute[name]) + annos.append(nusc_anno) + nusc_annos[sample_token] = annos + nusc_submissions = { + 'meta': self.modality, + 'results': nusc_annos, + } + + mmengine.mkdir_or_exist(jsonfile_prefix) + res_path = osp.join(jsonfile_prefix, 'results_nusc.json') + print(f'Results writes to {res_path}') + mmengine.dump(nusc_submissions, res_path) + return res_path + + + def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = None): + """Convert ground-truth annotations to nuscenes Box format. + Args: + output_dir (str): the path to output directory + pipeline (list[dict], optional): pipeline for formatting GTs + + Returns: + str: the path to the formatted ground-truth file + """ + + if pipeline is not None: + pipeline = self._get_pipeline(pipeline) + + nusc_annos = {} + for sample_id in range(len(self.data_infos)): + + sample_token = self.data_infos[sample_id]['token'] + gt_raw = self.data_infos[sample_id]['instances'] + + gt_labels_3d = torch.tensor([]).int() + gt_scores_3d = torch.tensor([]) + + bboxes_3d = np.array([]) + + for i in range(len(gt_raw)): + labels = gt_raw[i]['bbox_label_3d'] + label_as_torch = torch.tensor([labels]).int() + gt_labels_3d = torch.cat((gt_labels_3d, label_as_torch), 0) + + score_as_torch = torch.tensor([1.0]) + gt_scores_3d = torch.cat((gt_scores_3d, score_as_torch), 0) + + bbox3d = gt_raw[i]['bbox_3d'] + bbox3d_np = np.append(np.array(bbox3d), np.array(gt_raw[i]['velocity'])) + + if i == 0: + bboxes_3d = np.array([bbox3d_np]) + else: + bboxes_3d = np.vstack([bboxes_3d, np.array(bbox3d_np)]) + + gt_bboxes_3d = LiDARInstance3DBoxes( + bboxes_3d, + box_dim=bboxes_3d.shape[-1], + origin=(0.5, 0.5, 0.5), + ) + + instance_bboxes = dict( + labels_3d=gt_labels_3d, + bboxes_3d= gt_bboxes_3d, + ) + + if pipeline is not None: + instance_bboxes = pipeline(instance_bboxes) + + instance_result = dict( + labels_3d=instance_bboxes["labels_3d"], + scores_3d=gt_scores_3d, + bboxes_3d= instance_bboxes["bboxes_3d"], + sample_idx = i, + ) + + + annos = [] + boxes, attrs = output_to_nusc_box(instance_result) + boxes = lidar_nusc_box_to_global(self.data_infos[sample_id], + boxes, self.dataset_meta['classes'], + self.eval_detection_configs) + for i, box in enumerate(boxes): + name = self.dataset_meta['classes'][box.label] + nusc_anno = dict( + sample_token=sample_token, + translation=box.center.tolist(), + size=box.wlh.tolist(), + rotation=box.orientation.elements.tolist(), + velocity=box.velocity[:2].tolist(), + detection_name=name, + detection_score=box.score, + attribute_name=self.DefaultAttribute[name]) + + annos.append(nusc_anno) + nusc_annos[sample_token] = annos + + + nusc_submissions = { + "meta": self.modality, + "results": nusc_annos, + } + + mmengine.mkdir_or_exist(output_dir) + res_path = osp.join(output_dir, 'results_nusc_gt.json') + print(f'Results writes to {res_path}') + mmengine.dump(nusc_submissions, res_path) + return res_path + +def output_to_nusc_box( + detection: dict) -> Tuple[List[NuScenesBox], Union[np.ndarray, None]]: + """Convert the output to the box class in the nuScenes. + + Args: + detection (dict): Detection results. + + - bboxes_3d (:obj:`BaseInstance3DBoxes`): Detection bbox. + - scores_3d (torch.Tensor): Detection scores. + - labels_3d (torch.Tensor): Predicted box labels. + + Returns: + Tuple[List[:obj:`NuScenesBox`], np.ndarray or None]: List of standard + NuScenesBoxes and attribute labels. + """ + bbox3d = detection['bboxes_3d'] + scores = detection['scores_3d'].numpy() + labels = detection['labels_3d'].numpy() + attrs = None + if 'attr_labels' in detection: + attrs = detection['attr_labels'].numpy() + + box_gravity_center = bbox3d.gravity_center.numpy() + box_dims = bbox3d.dims.numpy() + box_yaw = bbox3d.yaw.numpy() + + box_list = [] + if isinstance(bbox3d, LiDARInstance3DBoxes): + # our LiDAR coordinate system -> nuScenes box coordinate system + nus_box_dims = box_dims[:, [1, 0, 2]] + for i in range(len(bbox3d)): + quat = pyquaternion.Quaternion(axis=[0, 0, 1], radians=box_yaw[i]) + velocity = (*bbox3d.tensor[i, 7:9], 0.0) + # velo_val = np.linalg.norm(box3d[i, 7:9]) + # velo_ori = box3d[i, 6] + # velocity = ( + # velo_val * np.cos(velo_ori), velo_val * np.sin(velo_ori), 0.0) + box = NuScenesBox( + box_gravity_center[i], + nus_box_dims[i], + quat, + label=labels[i], + score=scores[i], + velocity=velocity) + box_list.append(box) + elif isinstance(bbox3d, CameraInstance3DBoxes): + # our Camera coordinate system -> nuScenes box coordinate system + # convert the dim/rot to nuscbox convention + nus_box_dims = box_dims[:, [2, 0, 1]] + nus_box_yaw = -box_yaw + for i in range(len(bbox3d)): + q1 = pyquaternion.Quaternion( + axis=[0, 0, 1], radians=nus_box_yaw[i]) + q2 = pyquaternion.Quaternion(axis=[1, 0, 0], radians=np.pi / 2) + quat = q2 * q1 + velocity = (bbox3d.tensor[i, 7], 0.0, bbox3d.tensor[i, 8]) + box = NuScenesBox( + box_gravity_center[i], + nus_box_dims[i], + quat, + label=labels[i], + score=scores[i], + velocity=velocity) + box_list.append(box) + else: + raise NotImplementedError( + f'Do not support convert {type(bbox3d)} bboxes ' + 'to standard NuScenesBoxes.') + + return box_list, attrs + + +def lidar_nusc_box_to_global( + info: dict, boxes: List[NuScenesBox], classes: List[str], + eval_configs: DetectionConfig) -> List[NuScenesBox]: + """Convert the box from ego to global coordinate. + + Args: + info (dict): Info for a specific sample data, including the calibration + information. + boxes (List[:obj:`NuScenesBox`]): List of predicted NuScenesBoxes. + classes (List[str]): Mapped classes in the evaluation. + eval_configs (:obj:`DetectionConfig`): Evaluation configuration object. + + Returns: + List[:obj:`DetectionConfig`]: List of standard NuScenesBoxes in the + global coordinate. + """ + box_list = [] + for box in boxes: + # Move box to ego vehicle coord system + lidar2ego = np.array(info['lidar_points']['lidar2ego']) + box.rotate( + pyquaternion.Quaternion(matrix=lidar2ego, rtol=1e-05, atol=1e-07)) + box.translate(lidar2ego[:3, 3]) + # filter det in ego. + cls_range_map = eval_configs.class_range + radius = np.linalg.norm(box.center[:2], 2) + det_range = cls_range_map[classes[box.label]] + if radius > det_range: + continue + # Move box to global coord system + ego2global = np.array(info['ego2global']) + box.rotate( + pyquaternion.Quaternion(matrix=ego2global, rtol=1e-05, atol=1e-07)) + box.translate(ego2global[:3, 3]) + box_list.append(box) + return box_list + + +def cam_nusc_box_to_global( + info: dict, + boxes: List[NuScenesBox], + attrs: np.ndarray, + classes: List[str], + eval_configs: DetectionConfig, + camera_type: str = 'CAM_FRONT', +) -> Tuple[List[NuScenesBox], List[int]]: + """Convert the box from camera to global coordinate. + + Args: + info (dict): Info for a specific sample data, including the calibration + information. + boxes (List[:obj:`NuScenesBox`]): List of predicted NuScenesBoxes. + attrs (np.ndarray): Predicted attributes. + classes (List[str]): Mapped classes in the evaluation. + eval_configs (:obj:`DetectionConfig`): Evaluation configuration object. + camera_type (str): Type of camera. Defaults to 'CAM_FRONT'. + + Returns: + Tuple[List[:obj:`NuScenesBox`], List[int]]: List of standard + NuScenesBoxes in the global coordinate and attribute label. + """ + box_list = [] + attr_list = [] + for (box, attr) in zip(boxes, attrs): + # Move box to ego vehicle coord system + cam2ego = np.array(info['images'][camera_type]['cam2ego']) + box.rotate( + pyquaternion.Quaternion(matrix=cam2ego, rtol=1e-05, atol=1e-07)) + box.translate(cam2ego[:3, 3]) + # filter det in ego. + cls_range_map = eval_configs.class_range + radius = np.linalg.norm(box.center[:2], 2) + det_range = cls_range_map[classes[box.label]] + if radius > det_range: + continue + # Move box to global coord system + ego2global = np.array(info['ego2global']) + box.rotate( + pyquaternion.Quaternion(matrix=ego2global, rtol=1e-05, atol=1e-07)) + box.translate(ego2global[:3, 3]) + box_list.append(box) + attr_list.append(attr) + return box_list, attr_list + + +def global_nusc_box_to_cam(info: dict, boxes: List[NuScenesBox], + classes: List[str], + eval_configs: DetectionConfig) -> List[NuScenesBox]: + """Convert the box from global to camera coordinate. + + Args: + info (dict): Info for a specific sample data, including the calibration + information. + boxes (List[:obj:`NuScenesBox`]): List of predicted NuScenesBoxes. + classes (List[str]): Mapped classes in the evaluation. + eval_configs (:obj:`DetectionConfig`): Evaluation configuration object. + + Returns: + List[:obj:`NuScenesBox`]: List of standard NuScenesBoxes in camera + coordinate. + """ + box_list = [] + for box in boxes: + # Move box to ego vehicle coord system + ego2global = np.array(info['ego2global']) + box.translate(-ego2global[:3, 3]) + box.rotate( + pyquaternion.Quaternion(matrix=ego2global, rtol=1e-05, + atol=1e-07).inverse) + # filter det in ego. + cls_range_map = eval_configs.class_range + radius = np.linalg.norm(box.center[:2], 2) + det_range = cls_range_map[classes[box.label]] + if radius > det_range: + continue + # Move box to camera coord system + cam2ego = np.array(info['images']['CAM_FRONT']['cam2ego']) + box.translate(-cam2ego[:3, 3]) + box.rotate( + pyquaternion.Quaternion(matrix=cam2ego, rtol=1e-05, + atol=1e-07).inverse) + box_list.append(box) + return box_list + + +def nusc_box_to_cam_box3d( + boxes: List[NuScenesBox] +) -> Tuple[CameraInstance3DBoxes, torch.Tensor, torch.Tensor]: + """Convert boxes from :obj:`NuScenesBox` to :obj:`CameraInstance3DBoxes`. + + Args: + boxes (:obj:`List[NuScenesBox]`): List of predicted NuScenesBoxes. + + Returns: + Tuple[:obj:`CameraInstance3DBoxes`, torch.Tensor, torch.Tensor]: + Converted 3D bounding boxes, scores and labels. + """ + locs = torch.Tensor([b.center for b in boxes]).view(-1, 3) + dims = torch.Tensor([b.wlh for b in boxes]).view(-1, 3) + rots = torch.Tensor([b.orientation.yaw_pitch_roll[0] + for b in boxes]).view(-1, 1) + velocity = torch.Tensor([b.velocity[0::2] for b in boxes]).view(-1, 2) + + # convert nusbox to cambox convention + dims[:, [0, 1, 2]] = dims[:, [1, 2, 0]] + rots = -rots + + boxes_3d = torch.cat([locs, dims, rots, velocity], dim=1).cuda() + cam_boxes3d = CameraInstance3DBoxes( + boxes_3d, box_dim=9, origin=(0.5, 0.5, 0.5)) + scores = torch.Tensor([b.score for b in boxes]).cuda() + labels = torch.LongTensor([b.label for b in boxes]).cuda() + nms_scores = scores.new_zeros(scores.shape[0], 10 + 1) + indices = labels.new_tensor(list(range(scores.shape[0]))) + nms_scores[indices, labels] = scores + return cam_boxes3d, nms_scores, labels diff --git a/tools/create_data.py b/tools/create_data.py index 34356c2a8f..59efda3fe6 100644 --- a/tools/create_data.py +++ b/tools/create_data.py @@ -6,6 +6,8 @@ from tools.dataset_converters import kitti_converter as kitti from tools.dataset_converters import lyft_converter as lyft_converter from tools.dataset_converters import nuscenes_converter as nuscenes_converter +from tools.dataset_converters import tier4dataset_converter as tier4_converter + from tools.dataset_converters import semantickitti_converter from tools.dataset_converters.create_gt_database import ( GTDatabaseCreater, create_groundtruth_database) @@ -86,6 +88,35 @@ def nuscenes_data_prep(root_path, create_groundtruth_database(dataset_name, root_path, info_prefix, f'{info_prefix}_infos_train.pkl') +def tier4_data_prep(root_path, + info_prefix, + version, + dataset_name, + out_dir, + max_sweeps=1): + """Prepare data related to Tier4 dataset. + + Related data consists of '.pkl' files recording basic infos, + 2D annotations and groundtruth database. + + Args: + root_path (str): Path of dataset root. + info_prefix (str): The prefix of info filenames. + version (str): Dataset version. + dataset_name (str): The dataset class name. + out_dir (str): Output directory of the groundtruth database info. + max_sweeps (int, optional): Number of input consecutive frames. + Default: 10 + """ + tier4_converter.create_tier4_infos( + root_path, info_prefix, version=version, max_sweeps=max_sweeps) + info_train_path = osp.join(out_dir, f'{info_prefix}_infos_train.pkl') + info_val_path = osp.join(out_dir, f'{info_prefix}_infos_val.pkl') + update_pkl_infos('tier4', out_dir=out_dir, pkl_path=info_train_path) + update_pkl_infos('tier4', out_dir=out_dir, pkl_path=info_val_path) + create_groundtruth_database(dataset_name, root_path, info_prefix, + f'{info_prefix}_infos_train.pkl') + def lyft_data_prep(root_path, info_prefix, version, max_sweeps=10): """Prepare data related to Lyft dataset. @@ -268,6 +299,12 @@ def semantickitti_data_prep(info_prefix, out_dir): default='./data/kitti', required=False, help='name of info pkl') +parser.add_argument( + '--annotation-hz', + type=int, + default='10', + required=False, + help='annotation hz of the dataset') parser.add_argument('--extra-tag', type=str, default='kitti') parser.add_argument( '--workers', type=int, default=4, help='number of threads to be used') @@ -334,6 +371,14 @@ def semantickitti_data_prep(info_prefix, out_dir): dataset_name='NuScenesDataset', out_dir=args.out_dir, max_sweeps=args.max_sweeps) + elif args.dataset == 'Tier4Dataset': + tier4_data_prep( + root_path=args.root_path, + info_prefix=args.extra_tag, + version=args.version, + dataset_name='Tier4Dataset', + out_dir=args.out_dir, + max_sweeps=args.max_sweeps) elif args.dataset == 'lyft': train_version = f'{args.version}-train' lyft_data_prep( diff --git a/tools/dataset_converters/create_gt_database.py b/tools/dataset_converters/create_gt_database.py index ae452eb543..c1eb19cd7b 100644 --- a/tools/dataset_converters/create_gt_database.py +++ b/tools/dataset_converters/create_gt_database.py @@ -192,6 +192,29 @@ def create_groundtruth_database(dataset_class_name, with_label_3d=True) ]) + elif dataset_class_name == 'Tier4Dataset': + dataset_cfg.update( + use_valid_flag=True, + data_prefix=dict( + pts='', img='', sweeps=''), + pipeline=[ + dict( + type='LoadPointsFromFile', + coord_type='LIDAR', + load_dim=5, + use_dim=5), + dict( + type='LoadPointsFromMultiSweeps', + sweeps_num=1, + use_dim=[0, 1, 2, 4], + pad_empty_sweeps=True, + remove_close=True), + dict( + type='LoadAnnotations3D', + with_bbox_3d=True, + with_label_3d=True) + ]) + elif dataset_class_name == 'WaymoDataset': backend_args = None dataset_cfg.update( diff --git a/tools/dataset_converters/tier4dataset_converter.py b/tools/dataset_converters/tier4dataset_converter.py new file mode 100644 index 0000000000..88595cbb1d --- /dev/null +++ b/tools/dataset_converters/tier4dataset_converter.py @@ -0,0 +1,644 @@ +# Copyright (c) OpenMMLab. All rights reserved. +import os +import yaml +from collections import OrderedDict +from os import path as osp +from typing import List, Tuple, Union + +import mmcv +import mmengine +import numpy as np +from nuscenes.nuscenes import NuScenes +from nuscenes.utils.geometry_utils import view_points +from pyquaternion import Quaternion +from shapely.geometry import MultiPoint, box + +from mmdet3d.datasets.convert_utils import NuScenesNameMapping +from mmdet3d.structures import points_cam2img + +nus_categories = ('car', 'truck', 'bus', 'bicycle', 'pedestrian') + +nus_attributes = ('cycle.with_rider', 'cycle.without_rider', + 'pedestrian.moving', 'pedestrian.standing', + 'pedestrian.sitting_lying_down', 'vehicle.moving', + 'vehicle.parked', 'vehicle.stopped', 'None') + + +def create_tier4_infos(root_path, + info_prefix, + version='sample_dataset', + max_sweeps=0): + """Create info file of tier4 dataset. + + Given the raw data, generate its related info file in pkl format. + + Args: + root_path (str): Path of the data root. + info_prefix (str): Prefix of the info file to be generated. + version (str, optional): Version of the data. + max_sweeps (int, optional): Max number of sweeps. + Default: 1. + """ + + def process_scene(nusc, train_scenes, val_scenes, is_test=False): + if not is_test: + train_nusc_infos, val_nusc_infos = _fill_trainval_infos( + nusc, train_scenes, val_scenes, False, max_sweeps=max_sweeps) + return train_nusc_infos, val_nusc_infos + else: + train_nusc_infos, _ = _fill_trainval_infos( + nusc, train_scenes, val_scenes, True, max_sweeps=max_sweeps) + return train_nusc_infos + + dataset_config = root_path + version + ".yaml" + with open(dataset_config, "r") as f: + dataset_config_dict = yaml.safe_load(f) + print(dataset_config_dict) + + all_train_nusc_infos = [] + all_val_nusc_infos = [] + all_test_nusc_infos = [] + + if dataset_config_dict["train"] is not None: + for scene in dataset_config_dict["train"]: + scene_path = root_path + "/" + scene + nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + available_scenes = get_available_scenes(nusc) + available_scene_tokens = [s['token'] for s in available_scenes] + train_nusc_infos, _ = process_scene(nusc, available_scene_tokens, []) + all_train_nusc_infos += train_nusc_infos + + if dataset_config_dict["val"] is not None: + for scene in dataset_config_dict["val"]: + scene_path = root_path + "/" + scene + nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + available_scenes = get_available_scenes(nusc) + available_scene_tokens = [s['token'] for s in available_scenes] + _, val_nusc_infos = process_scene(nusc,[], available_scene_tokens) + all_val_nusc_infos += val_nusc_infos + + if dataset_config_dict["test"] is not None: + for scene in dataset_config_dict["test"]: + scene_path = root_path + "/" + scene + nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + available_scenes = get_available_scenes(nusc) + available_scene_tokens = [s['token'] for s in available_scenes] + train_nusc_infos = process_scene(nusc,available_scene_tokens, [], True) + all_test_nusc_infos += train_nusc_infos + + metadata = dict(version=version) + + print('train sample: {}'.format(len(all_train_nusc_infos))) + data = dict(infos=all_train_nusc_infos, metadata=metadata) + info_path = osp.join(root_path, '{}_infos_train.pkl'.format(info_prefix)) + mmengine.dump(data, info_path) + + print('val sample: {}'.format(len(all_val_nusc_infos))) + data['infos'] = all_val_nusc_infos + info_val_path = osp.join(root_path, '{}_infos_val.pkl'.format(info_prefix)) + mmengine.dump(data, info_val_path) + + print('test sample: {}'.format(len(all_test_nusc_infos))) + data = dict(infos=all_test_nusc_infos, metadata=metadata) + info_path = osp.join(root_path, '{}_infos_test.pkl'.format(info_prefix)) + mmengine.dump(data, info_path) + + +def get_available_scenes(nusc): + """Get available scenes from the input nuscenes class. + + Given the raw data, get the information of available scenes for + further info generation. + + Args: + nusc (class): Dataset class in the nuScenes dataset. + + Returns: + available_scenes (list[dict]): List of basic information for the + available scenes. + """ + available_scenes = [] + print('total scene num: {}'.format(len(nusc.scene))) + for scene in nusc.scene: + scene_token = scene['token'] + scene_rec = nusc.get('scene', scene_token) + sample_rec = nusc.get('sample', scene_rec['first_sample_token']) + sd_rec = nusc.get('sample_data', sample_rec['data']['LIDAR_CONCAT']) + has_more_frames = True + scene_not_exist = False + while has_more_frames: + lidar_path, boxes, _ = nusc.get_sample_data(sd_rec['token']) + lidar_path = str(lidar_path) + if os.getcwd() in lidar_path: + # path from lyftdataset is absolute path + lidar_path = lidar_path.split(f'{os.getcwd()}/')[-1] + # relative path + if not mmengine.is_filepath(lidar_path): + scene_not_exist = True + break + else: + break + if scene_not_exist: + continue + available_scenes.append(scene) + print('exist scene num: {}'.format(len(available_scenes))) + return available_scenes + + +def _fill_trainval_infos(nusc, + train_scenes, + val_scenes, + test=False, + max_sweeps=10): + """Generate the train/val infos from the raw data. + + Args: + nusc (:obj:`NuScenes`): Dataset class in the nuScenes dataset. + train_scenes (list[str]): Basic information of training scenes. + val_scenes (list[str]): Basic information of validation scenes. + test (bool, optional): Whether use the test mode. In test mode, no + annotations can be accessed. Default: False. + max_sweeps (int, optional): Max number of sweeps. Default: 10. + + Returns: + tuple[list[dict]]: Information of training set and validation set + that will be saved to the info file. + """ + train_nusc_infos = [] + val_nusc_infos = [] + + for idx, sample in enumerate(mmengine.track_iter_progress(nusc.sample)): + + lidar_token = sample['data']['LIDAR_CONCAT'] + sd_rec = nusc.get('sample_data', sample['data']['LIDAR_CONCAT']) + cs_record = nusc.get('calibrated_sensor', + sd_rec['calibrated_sensor_token']) + + pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token']) + lidar_path, boxes, _ = nusc.get_sample_data(lidar_token) + + mmengine.check_file_exist(lidar_path) + + info = { + 'lidar_path': lidar_path, + 'num_features': 5, + 'token': sample['token'], + 'sweeps': [], + 'cams': dict(), + 'lidar2ego_translation': cs_record['translation'], + 'lidar2ego_rotation': cs_record['rotation'], + 'ego2global_translation': pose_record['translation'], + 'ego2global_rotation': pose_record['rotation'], + 'timestamp': sample['timestamp'], + } + + l2e_r = info['lidar2ego_rotation'] + l2e_t = info['lidar2ego_translation'] + e2g_r = info['ego2global_rotation'] + e2g_t = info['ego2global_translation'] + l2e_r_mat = Quaternion(l2e_r).rotation_matrix + e2g_r_mat = Quaternion(e2g_r).rotation_matrix + + # obtain 6 image's information per frame + camera_types = [ + 'CAM_FRONT', + 'CAM_FRONT_RIGHT', + 'CAM_FRONT_LEFT', + 'CAM_BACK', + 'CAM_BACK_LEFT', + 'CAM_BACK_RIGHT', + ] + + for cam in camera_types: + + if not cam in sample['data']: + continue + + cam_token = sample['data'][cam] + cam_path, _, cam_intrinsic = nusc.get_sample_data(cam_token) + cam_info = obtain_sensor2top(nusc, cam_token, l2e_t, l2e_r_mat, + e2g_t, e2g_r_mat, cam) + cam_info.update(cam_intrinsic=cam_intrinsic) + info['cams'].update({cam: cam_info}) + + # obtain sweeps for a single key-frame + sd_rec = nusc.get('sample_data', sample['data']['LIDAR_CONCAT']) + sweeps = [] + while len(sweeps) < max_sweeps: + if not sd_rec['prev'] == '': + sweep = obtain_sensor2top(nusc, sd_rec['prev'], l2e_t, + l2e_r_mat, e2g_t, e2g_r_mat, 'lidar') + sweeps.append(sweep) + sd_rec = nusc.get('sample_data', sd_rec['prev']) + else: + break + info['sweeps'] = sweeps + # obtain annotation + if not test: + annotations = [ + nusc.get('sample_annotation', token) + for token in sample['anns'] + ] + locs = np.array([b.center for b in boxes]).reshape(-1, 3) + dims = np.array([b.wlh for b in boxes]).reshape(-1, 3) + rots = np.array([b.orientation.yaw_pitch_roll[0] + for b in boxes]).reshape(-1, 1) + velocity = np.array( + [nusc.box_velocity(token)[:2] for token in sample['anns']]) + valid_flag = np.array( + [(anno['num_lidar_pts'] + anno['num_radar_pts']) > 0 + for anno in annotations], + dtype=bool).reshape(-1) + # convert velo from global to lidar + for i in range(len(boxes)): + velo = np.array([*velocity[i], 0.0]) + velo = velo @ np.linalg.inv(e2g_r_mat).T @ np.linalg.inv( + l2e_r_mat).T + velocity[i] = velo[:2] + + names = [b.name for b in boxes] + for i in range(len(names)): + if names[i] in NuScenesNameMapping: + names[i] = NuScenesNameMapping[names[i]] + names = np.array(names) + # we need to convert box size to + # the format of our lidar coordinate system + # which is x_size, y_size, z_size (corresponding to l, w, h) + gt_boxes = np.concatenate([locs, dims[:, [1, 0, 2]], rots], axis=1) + assert len(gt_boxes) == len( + annotations), f'{len(gt_boxes)}, {len(annotations)}' + info['gt_boxes'] = gt_boxes + info['gt_names'] = names + info['gt_velocity'] = velocity.reshape(-1, 2) + info['num_lidar_pts'] = np.array( + [a['num_lidar_pts'] for a in annotations]) + info['num_radar_pts'] = np.array( + [a['num_radar_pts'] for a in annotations]) + info['valid_flag'] = valid_flag + + if 'lidarseg' in nusc.table_names: + info['pts_semantic_mask_path'] = osp.join( + nusc.dataroot, + nusc.get('lidarseg', lidar_token)['filename']) + + if sample['scene_token'] in train_scenes: + train_nusc_infos.append(info) + else: + val_nusc_infos.append(info) + + return train_nusc_infos, val_nusc_infos + + +def obtain_sensor2top(nusc, + sensor_token, + l2e_t, + l2e_r_mat, + e2g_t, + e2g_r_mat, + sensor_type='lidar'): + """Obtain the info with RT matric from general sensor to Top LiDAR. + + Args: + nusc (class): Dataset class in the nuScenes dataset. + sensor_token (str): Sample data token corresponding to the + specific sensor type. + l2e_t (np.ndarray): Translation from lidar to ego in shape (1, 3). + l2e_r_mat (np.ndarray): Rotation matrix from lidar to ego + in shape (3, 3). + e2g_t (np.ndarray): Translation from ego to global in shape (1, 3). + e2g_r_mat (np.ndarray): Rotation matrix from ego to global + in shape (3, 3). + sensor_type (str, optional): Sensor to calibrate. Default: 'lidar'. + + Returns: + sweep (dict): Sweep information after transformation. + """ + sd_rec = nusc.get('sample_data', sensor_token) + cs_record = nusc.get('calibrated_sensor', + sd_rec['calibrated_sensor_token']) + pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token']) + data_path = str(nusc.get_sample_data_path(sd_rec['token'])) + if os.getcwd() in data_path: # path from lyftdataset is absolute path + data_path = data_path.split(f'{os.getcwd()}/')[-1] # relative path + sweep = { + 'data_path': data_path, + 'type': sensor_type, + 'sample_data_token': sd_rec['token'], + 'sensor2ego_translation': cs_record['translation'], + 'sensor2ego_rotation': cs_record['rotation'], + 'ego2global_translation': pose_record['translation'], + 'ego2global_rotation': pose_record['rotation'], + 'timestamp': sd_rec['timestamp'] + } + l2e_r_s = sweep['sensor2ego_rotation'] + l2e_t_s = sweep['sensor2ego_translation'] + e2g_r_s = sweep['ego2global_rotation'] + e2g_t_s = sweep['ego2global_translation'] + + # obtain the RT from sensor to Top LiDAR + # sweep->ego->global->ego'->lidar + l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix + e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix + R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ ( + np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) + T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ ( + np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) + T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T + ) + l2e_t @ np.linalg.inv(l2e_r_mat).T + sweep['sensor2lidar_rotation'] = R.T # points @ R.T + T + sweep['sensor2lidar_translation'] = T + return sweep + + +def export_2d_annotation(root_path, info_path, version, mono3d=True): + """Export 2d annotation from the info file and raw data. + + Args: + root_path (str): Root path of the raw data. + info_path (str): Path of the info file. + version (str): Dataset version. + mono3d (bool, optional): Whether to export mono3d annotation. + Default: True. + """ + # get bbox annotations for camera + camera_types = [ + 'CAM_FRONT', + 'CAM_FRONT_RIGHT', + 'CAM_FRONT_LEFT', + 'CAM_BACK', + 'CAM_BACK_LEFT', + 'CAM_BACK_RIGHT', + ] + nusc_infos = mmengine.load(info_path)['infos'] + nusc = NuScenes(version=version, dataroot=root_path, verbose=True) + # info_2d_list = [] + cat2Ids = [ + dict(id=nus_categories.index(cat_name), name=cat_name) + for cat_name in nus_categories + ] + coco_ann_id = 0 + coco_2d_dict = dict(annotations=[], images=[], categories=cat2Ids) + for info in mmengine.track_iter_progress(nusc_infos): + for cam in camera_types: + cam_info = info['cams'][cam] + coco_infos = get_2d_boxes( + nusc, + cam_info['sample_data_token'], + visibilities=['', '1', '2', '3', '4'], + mono3d=mono3d) + (height, width, _) = mmcv.imread(cam_info['data_path']).shape + coco_2d_dict['images'].append( + dict( + file_name=cam_info['data_path'].split('data/nuscenes/') + [-1], + id=cam_info['sample_data_token'], + token=info['token'], + cam2ego_rotation=cam_info['sensor2ego_rotation'], + cam2ego_translation=cam_info['sensor2ego_translation'], + ego2global_rotation=info['ego2global_rotation'], + ego2global_translation=info['ego2global_translation'], + cam_intrinsic=cam_info['cam_intrinsic'], + width=width, + height=height)) + for coco_info in coco_infos: + if coco_info is None: + continue + # add an empty key for coco format + coco_info['segmentation'] = [] + coco_info['id'] = coco_ann_id + coco_2d_dict['annotations'].append(coco_info) + coco_ann_id += 1 + if mono3d: + json_prefix = f'{info_path[:-4]}_mono3d' + else: + json_prefix = f'{info_path[:-4]}' + mmengine.dump(coco_2d_dict, f'{json_prefix}.coco.json') + + +def get_2d_boxes(nusc, + sample_data_token: str, + visibilities: List[str], + mono3d=True): + """Get the 2D annotation records for a given `sample_data_token`. + + Args: + sample_data_token (str): Sample data token belonging to a camera + keyframe. + visibilities (list[str]): Visibility filter. + mono3d (bool): Whether to get boxes with mono3d annotation. + + Return: + list[dict]: List of 2D annotation record that belongs to the input + `sample_data_token`. + """ + + # Get the sample data and the sample corresponding to that sample data. + sd_rec = nusc.get('sample_data', sample_data_token) + + assert sd_rec[ + 'sensor_modality'] == 'camera', 'Error: get_2d_boxes only works' \ + ' for camera sample_data!' + if not sd_rec['is_key_frame']: + raise ValueError( + 'The 2D re-projections are available only for keyframes.') + + s_rec = nusc.get('sample', sd_rec['sample_token']) + + # Get the calibrated sensor and ego pose + # record to get the transformation matrices. + cs_rec = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) + pose_rec = nusc.get('ego_pose', sd_rec['ego_pose_token']) + camera_intrinsic = np.array(cs_rec['camera_intrinsic']) + + # Get all the annotation with the specified visibilties. + ann_recs = [ + nusc.get('sample_annotation', token) for token in s_rec['anns'] + ] + ann_recs = [ + ann_rec for ann_rec in ann_recs + if (ann_rec['visibility_token'] in visibilities) + ] + + repro_recs = [] + + for ann_rec in ann_recs: + # Augment sample_annotation with token information. + ann_rec['sample_annotation_token'] = ann_rec['token'] + ann_rec['sample_data_token'] = sample_data_token + + # Get the box in global coordinates. + box = nusc.get_box(ann_rec['token']) + + # Move them to the ego-pose frame. + box.translate(-np.array(pose_rec['translation'])) + box.rotate(Quaternion(pose_rec['rotation']).inverse) + + # Move them to the calibrated sensor frame. + box.translate(-np.array(cs_rec['translation'])) + box.rotate(Quaternion(cs_rec['rotation']).inverse) + + # Filter out the corners that are not in front of the calibrated + # sensor. + corners_3d = box.corners() + in_front = np.argwhere(corners_3d[2, :] > 0).flatten() + corners_3d = corners_3d[:, in_front] + + # Project 3d box to 2d. + corner_coords = view_points(corners_3d, camera_intrinsic, + True).T[:, :2].tolist() + + # Keep only corners that fall within the image. + final_coords = post_process_coords(corner_coords) + + # Skip if the convex hull of the re-projected corners + # does not intersect the image canvas. + if final_coords is None: + continue + else: + min_x, min_y, max_x, max_y = final_coords + + # Generate dictionary record to be included in the .json file. + repro_rec = generate_record(ann_rec, min_x, min_y, max_x, max_y, + sample_data_token, sd_rec['filename']) + + # If mono3d=True, add 3D annotations in camera coordinates + if mono3d and (repro_rec is not None): + loc = box.center.tolist() + + dim = box.wlh + dim[[0, 1, 2]] = dim[[1, 2, 0]] # convert wlh to our lhw + dim = dim.tolist() + + rot = box.orientation.yaw_pitch_roll[0] + rot = [-rot] # convert the rot to our cam coordinate + + global_velo2d = nusc.box_velocity(box.token)[:2] + global_velo3d = np.array([*global_velo2d, 0.0]) + e2g_r_mat = Quaternion(pose_rec['rotation']).rotation_matrix + c2e_r_mat = Quaternion(cs_rec['rotation']).rotation_matrix + cam_velo3d = global_velo3d @ np.linalg.inv( + e2g_r_mat).T @ np.linalg.inv(c2e_r_mat).T + velo = cam_velo3d[0::2].tolist() + + repro_rec['bbox_cam3d'] = loc + dim + rot + repro_rec['velo_cam3d'] = velo + + center3d = np.array(loc).reshape([1, 3]) + center2d = points_cam2img( + center3d, camera_intrinsic, with_depth=True) + repro_rec['center2d'] = center2d.squeeze().tolist() + # normalized center2D + depth + # if samples with depth < 0 will be removed + if repro_rec['center2d'][2] <= 0: + continue + + ann_token = nusc.get('sample_annotation', + box.token)['attribute_tokens'] + if len(ann_token) == 0: + attr_name = 'None' + else: + attr_name = nusc.get('attribute', ann_token[0])['name'] + attr_id = nus_attributes.index(attr_name) + repro_rec['attribute_name'] = attr_name + repro_rec['attribute_id'] = attr_id + + repro_recs.append(repro_rec) + + return repro_recs + + +def post_process_coords( + corner_coords: List, imsize: Tuple[int, int] = (1600, 900) +) -> Union[Tuple[float, float, float, float], None]: + """Get the intersection of the convex hull of the reprojected bbox corners + and the image canvas, return None if no intersection. + + Args: + corner_coords (list[int]): Corner coordinates of reprojected + bounding box. + imsize (tuple[int]): Size of the image canvas. + + Return: + tuple [float]: Intersection of the convex hull of the 2D box + corners and the image canvas. + """ + polygon_from_2d_box = MultiPoint(corner_coords).convex_hull + img_canvas = box(0, 0, imsize[0], imsize[1]) + + if polygon_from_2d_box.intersects(img_canvas): + img_intersection = polygon_from_2d_box.intersection(img_canvas) + intersection_coords = np.array( + [coord for coord in img_intersection.exterior.coords]) + + min_x = min(intersection_coords[:, 0]) + min_y = min(intersection_coords[:, 1]) + max_x = max(intersection_coords[:, 0]) + max_y = max(intersection_coords[:, 1]) + + return min_x, min_y, max_x, max_y + else: + return None + + +def generate_record(ann_rec: dict, x1: float, y1: float, x2: float, y2: float, + sample_data_token: str, filename: str) -> OrderedDict: + """Generate one 2D annotation record given various information on top of + the 2D bounding box coordinates. + + Args: + ann_rec (dict): Original 3d annotation record. + x1 (float): Minimum value of the x coordinate. + y1 (float): Minimum value of the y coordinate. + x2 (float): Maximum value of the x coordinate. + y2 (float): Maximum value of the y coordinate. + sample_data_token (str): Sample data token. + filename (str):The corresponding image file where the annotation + is present. + + Returns: + dict: A sample 2D annotation record. + - file_name (str): file name + - image_id (str): sample data token + - area (float): 2d box area + - category_name (str): category name + - category_id (int): category id + - bbox (list[float]): left x, top y, dx, dy of 2d box + - iscrowd (int): whether the area is crowd + """ + repro_rec = OrderedDict() + repro_rec['sample_data_token'] = sample_data_token + coco_rec = dict() + + relevant_keys = [ + 'attribute_tokens', + 'category_name', + 'instance_token', + 'next', + 'num_lidar_pts', + 'num_radar_pts', + 'prev', + 'sample_annotation_token', + 'sample_data_token', + 'visibility_token', + ] + + for key, value in ann_rec.items(): + if key in relevant_keys: + repro_rec[key] = value + + repro_rec['bbox_corners'] = [x1, y1, x2, y2] + repro_rec['filename'] = filename + + coco_rec['file_name'] = filename + coco_rec['image_id'] = sample_data_token + coco_rec['area'] = (y2 - y1) * (x2 - x1) + + if repro_rec['category_name'] not in NuScenesNameMapping: + return None + cat_name = NuScenesNameMapping[repro_rec['category_name']] + coco_rec['category_name'] = cat_name + coco_rec['category_id'] = nus_categories.index(cat_name) + coco_rec['bbox'] = [x1, y1, x2 - x1, y2 - y1] + coco_rec['iscrowd'] = 0 + + return coco_rec diff --git a/tools/dataset_converters/update_infos_to_v2.py b/tools/dataset_converters/update_infos_to_v2.py index a2ddbd0688..f7d86276bb 100644 --- a/tools/dataset_converters/update_infos_to_v2.py +++ b/tools/dataset_converters/update_infos_to_v2.py @@ -13,6 +13,7 @@ import time from os import path as osp from pathlib import Path +import os import mmengine import numpy as np @@ -387,6 +388,142 @@ def update_nuscenes_infos(pkl_path, out_dir): mmengine.dump(converted_data_info, out_path, 'pkl') +def update_tier4_dataset_infos(pkl_path, out_dir): + camera_types = [ + 'CAM_FRONT', + 'CAM_FRONT_RIGHT', + 'CAM_FRONT_LEFT', + 'CAM_BACK', + 'CAM_BACK_LEFT', + 'CAM_BACK_RIGHT', + ] + print(f'{pkl_path} will be modified.') + if out_dir in pkl_path: + print(f'Warning, you may overwriting ' + f'the original data {pkl_path}.') + print(f'Reading from input file: {pkl_path}.') + data_list = mmengine.load(pkl_path) + + METAINFO = { + 'classes': + ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), + } + + print('Start updating:') + converted_list = [] + for i, ori_info_dict in enumerate( + mmengine.track_iter_progress(data_list['infos'])): + temp_data_info = get_empty_standard_data_info( + camera_types=camera_types) + temp_data_info['sample_idx'] = i + temp_data_info['token'] = ori_info_dict['token'] + temp_data_info['ego2global'] = convert_quaternion_to_matrix( + ori_info_dict['ego2global_rotation'], + ori_info_dict['ego2global_translation']) + temp_data_info['lidar_points']['num_pts_feats'] = ori_info_dict.get( + 'num_features', 5) + file_suffix_splitted = ori_info_dict['lidar_path'].split(os.sep) + file_suffix = os.sep.join(file_suffix_splitted[-4:]) + temp_data_info['lidar_points']['lidar_path'] = file_suffix + temp_data_info['lidar_points'][ + 'lidar2ego'] = convert_quaternion_to_matrix( + ori_info_dict['lidar2ego_rotation'], + ori_info_dict['lidar2ego_translation']) + # bc-breaking: Timestamp has divided 1e6 in pkl infos. + temp_data_info['timestamp'] = ori_info_dict['timestamp'] / 1e6 + for ori_sweep in ori_info_dict['sweeps']: + temp_lidar_sweep = get_single_lidar_sweep() + temp_lidar_sweep['lidar_points'][ + 'lidar2ego'] = convert_quaternion_to_matrix( + ori_sweep['sensor2ego_rotation'], + ori_sweep['sensor2ego_translation']) + temp_lidar_sweep['ego2global'] = convert_quaternion_to_matrix( + ori_sweep['ego2global_rotation'], + ori_sweep['ego2global_translation']) + lidar2sensor = np.eye(4) + rot = ori_sweep['sensor2lidar_rotation'] + trans = ori_sweep['sensor2lidar_translation'] + lidar2sensor[:3, :3] = rot.T + lidar2sensor[:3, 3:4] = -1 * np.matmul(rot.T, trans.reshape(3, 1)) + temp_lidar_sweep['lidar_points'][ + 'lidar2sensor'] = lidar2sensor.astype(np.float32).tolist() + temp_lidar_sweep['timestamp'] = ori_sweep['timestamp'] / 1e6 + temp_lidar_sweep['lidar_points']['lidar_path'] = ori_sweep[ + 'data_path'] + temp_lidar_sweep['sample_data_token'] = ori_sweep[ + 'sample_data_token'] + temp_data_info['lidar_sweeps'].append(temp_lidar_sweep) + temp_data_info['images'] = {} + for cam in ori_info_dict['cams']: + empty_img_info = get_empty_img_info() + empty_img_info['img_path'] = Path( + ori_info_dict['cams'][cam]['data_path']).name + empty_img_info['cam2img'] = ori_info_dict['cams'][cam][ + 'cam_intrinsic'].tolist() + empty_img_info['sample_data_token'] = ori_info_dict['cams'][cam][ + 'sample_data_token'] + # bc-breaking: Timestamp has divided 1e6 in pkl infos. + empty_img_info[ + 'timestamp'] = ori_info_dict['cams'][cam]['timestamp'] / 1e6 + empty_img_info['cam2ego'] = convert_quaternion_to_matrix( + ori_info_dict['cams'][cam]['sensor2ego_rotation'], + ori_info_dict['cams'][cam]['sensor2ego_translation']) + lidar2sensor = np.eye(4) + rot = ori_info_dict['cams'][cam]['sensor2lidar_rotation'] + trans = ori_info_dict['cams'][cam]['sensor2lidar_translation'] + lidar2sensor[:3, :3] = rot.T + lidar2sensor[:3, 3:4] = -1 * np.matmul(rot.T, trans.reshape(3, 1)) + empty_img_info['lidar2cam'] = lidar2sensor.astype( + np.float32).tolist() + temp_data_info['images'][cam] = empty_img_info + ignore_class_name = set() + if 'gt_boxes' in ori_info_dict: + num_instances = ori_info_dict['gt_boxes'].shape[0] + for i in range(num_instances): + empty_instance = get_empty_instance() + empty_instance['bbox_3d'] = ori_info_dict['gt_boxes'][ + i, :].tolist() + if ori_info_dict['gt_names'][i] in METAINFO['classes']: + empty_instance['bbox_label'] = METAINFO['classes'].index( + ori_info_dict['gt_names'][i]) + else: + ignore_class_name.add(ori_info_dict['gt_names'][i]) + empty_instance['bbox_label'] = -1 + empty_instance['bbox_label_3d'] = copy.deepcopy( + empty_instance['bbox_label']) + empty_instance['velocity'] = ori_info_dict['gt_velocity'][ + i, :].tolist() + empty_instance['num_lidar_pts'] = ori_info_dict[ + 'num_lidar_pts'][i] + empty_instance['num_radar_pts'] = ori_info_dict[ + 'num_radar_pts'][i] + empty_instance['bbox_3d_isvalid'] = ori_info_dict[ + 'valid_flag'][i] + empty_instance = clear_instance_unused_keys(empty_instance) + temp_data_info['instances'].append(empty_instance) + + if 'pts_semantic_mask_path' in ori_info_dict: + temp_data_info['pts_semantic_mask_path'] = Path( + ori_info_dict['pts_semantic_mask_path']).name + temp_data_info, _ = clear_data_info_unused_keys(temp_data_info) + converted_list.append(temp_data_info) + pkl_name = Path(pkl_path).name + out_path = osp.join(out_dir, pkl_name) + print(f'Writing to output file: {out_path}.') + # print(f'ignore classes: {ignore_class_name}') + + metainfo = dict() + metainfo['categories'] = {k: i for i, k in enumerate(METAINFO['classes'])} + # if ignore_class_name: + # for ignore_class in ignore_class_name: + # metainfo['categories'][ignore_class] = -1 + metainfo['dataset'] = 'tier4' + metainfo['version'] = data_list['metadata']['version'] + metainfo['info_version'] = '1.0' + converted_data_info = dict(metainfo=metainfo, data_list=converted_list) + + mmengine.dump(converted_data_info, out_path, 'pkl') + def update_kitti_infos(pkl_path, out_dir): print(f'{pkl_path} will be modified.') @@ -1148,6 +1285,8 @@ def update_pkl_infos(dataset, out_dir, pkl_path): update_nuscenes_infos(pkl_path=pkl_path, out_dir=out_dir) elif dataset.lower() == 's3dis': update_s3dis_infos(pkl_path=pkl_path, out_dir=out_dir) + elif dataset.lower() == 'tier4': + update_tier4_dataset_infos(pkl_path=pkl_path, out_dir=out_dir) else: raise NotImplementedError(f'Do not support convert {dataset} to v2.') From 840ae1da0df00073cfa3134286a56d780db97b18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Mon, 22 Jan 2024 18:07:15 +0900 Subject: [PATCH 2/8] feat: rm create gt dataset MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- tools/create_data.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/tools/create_data.py b/tools/create_data.py index 59efda3fe6..969d993882 100644 --- a/tools/create_data.py +++ b/tools/create_data.py @@ -114,8 +114,6 @@ def tier4_data_prep(root_path, info_val_path = osp.join(out_dir, f'{info_prefix}_infos_val.pkl') update_pkl_infos('tier4', out_dir=out_dir, pkl_path=info_train_path) update_pkl_infos('tier4', out_dir=out_dir, pkl_path=info_val_path) - create_groundtruth_database(dataset_name, root_path, info_prefix, - f'{info_prefix}_infos_train.pkl') def lyft_data_prep(root_path, info_prefix, version, max_sweeps=10): From 37fc84d8dfff68e7b87225e4e61eba82bb72fe84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Mon, 19 Feb 2024 17:33:17 +0300 Subject: [PATCH 3/8] Update projects/AutowareCenterPoint/datasets/tier4_dataset.py Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- projects/AutowareCenterPoint/datasets/tier4_dataset.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/AutowareCenterPoint/datasets/tier4_dataset.py b/projects/AutowareCenterPoint/datasets/tier4_dataset.py index 51a43d550f..b7dad19d50 100644 --- a/projects/AutowareCenterPoint/datasets/tier4_dataset.py +++ b/projects/AutowareCenterPoint/datasets/tier4_dataset.py @@ -12,7 +12,7 @@ @DATASETS.register_module() -class Tier4Dataset(NuScenesDataset): +class T4Dataset(NuScenesDataset): METAINFO = { 'classes': ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), 'version': 'v1.0-trainval', From 0238da855a73c9324fd873166d3515c7aa9c178d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Tue, 27 Feb 2024 14:01:55 +0300 Subject: [PATCH 4/8] feat: change Tier4 dataset name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../AutowareCenterPoint/configs/centerpoint_custom_test.py | 4 ++-- projects/AutowareCenterPoint/datasets/__init__.py | 4 ++-- tools/create_data.py | 4 ++-- tools/dataset_converters/create_gt_database.py | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py index 21e08c9cb4..55e3d72fcd 100644 --- a/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py @@ -23,7 +23,7 @@ data_prefix = dict(img='', pts='', sweeps='') data_root = "data/sample_dataset/" -dataset_type = 'Tier4Dataset' +dataset_type = 'T4Dataset' train_ann_file = dataset_type + '_infos_train.pkl' val_ann_file = dataset_type + '_infos_val.pkl' @@ -351,7 +351,7 @@ ], type='Pack3DDetInputs'), ], test_mode=True, - type='Tier4Dataset'), + type='T4Dataset'), drop_last=False, num_workers=1, persistent_workers=True, diff --git a/projects/AutowareCenterPoint/datasets/__init__.py b/projects/AutowareCenterPoint/datasets/__init__.py index fba124e3db..0f1b7de3dc 100644 --- a/projects/AutowareCenterPoint/datasets/__init__.py +++ b/projects/AutowareCenterPoint/datasets/__init__.py @@ -1,3 +1,3 @@ -from .tier4_dataset import Tier4Dataset +from .tier4_dataset import T4Dataset -__all__ = [ 'Tier4Dataset'] \ No newline at end of file +__all__ = [ 'T4Dataset'] \ No newline at end of file diff --git a/tools/create_data.py b/tools/create_data.py index 969d993882..4d423eb703 100644 --- a/tools/create_data.py +++ b/tools/create_data.py @@ -369,12 +369,12 @@ def semantickitti_data_prep(info_prefix, out_dir): dataset_name='NuScenesDataset', out_dir=args.out_dir, max_sweeps=args.max_sweeps) - elif args.dataset == 'Tier4Dataset': + elif args.dataset == 'T4Dataset': tier4_data_prep( root_path=args.root_path, info_prefix=args.extra_tag, version=args.version, - dataset_name='Tier4Dataset', + dataset_name='T4Dataset', out_dir=args.out_dir, max_sweeps=args.max_sweeps) elif args.dataset == 'lyft': diff --git a/tools/dataset_converters/create_gt_database.py b/tools/dataset_converters/create_gt_database.py index c1eb19cd7b..5670e4ac6f 100644 --- a/tools/dataset_converters/create_gt_database.py +++ b/tools/dataset_converters/create_gt_database.py @@ -192,7 +192,7 @@ def create_groundtruth_database(dataset_class_name, with_label_3d=True) ]) - elif dataset_class_name == 'Tier4Dataset': + elif dataset_class_name == 'T4Dataset': dataset_cfg.update( use_valid_flag=True, data_prefix=dict( From 934d97f57349ffc2328bfe220bf26148f009c53e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Tue, 27 Feb 2024 14:10:25 +0300 Subject: [PATCH 5/8] fix: refactor repo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../centerpoint/__init__.py | 2 +- .../centerpoint/pillar_encoder_autoware.py | 64 +-- .../centerpoint_onnx_converter.py | 155 +++++--- .../configs/centerpoint_custom.py | 5 +- .../configs/centerpoint_custom_test.py | 66 +-- .../AutowareCenterPoint/datasets/__init__.py | 2 +- .../datasets/tier4_dataset.py | 36 +- .../evaluation/__init__.py | 27 +- .../functional/nuscenes_utils/__init__.py | 21 +- .../functional/nuscenes_utils/eval.py | 64 +-- .../functional/nuscenes_utils/utils.py | 376 +++++++++--------- .../metrics/nuscenes_custom_metric.py | 69 ++-- tools/create_data.py | 14 +- .../dataset_converters/create_gt_database.py | 3 +- .../tier4dataset_converter.py | 49 ++- .../dataset_converters/update_infos_to_v2.py | 6 +- 16 files changed, 505 insertions(+), 454 deletions(-) diff --git a/projects/AutowareCenterPoint/centerpoint/__init__.py b/projects/AutowareCenterPoint/centerpoint/__init__.py index 77f26085f3..ee200d0c9b 100644 --- a/projects/AutowareCenterPoint/centerpoint/__init__.py +++ b/projects/AutowareCenterPoint/centerpoint/__init__.py @@ -1,3 +1,3 @@ from .pillar_encoder_autoware import PillarFeatureNetAutoware -__all__ = [ 'PillarFeatureNetAutoware'] \ No newline at end of file +__all__ = ['PillarFeatureNetAutoware'] diff --git a/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py index 882b0ac124..e697585579 100644 --- a/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py +++ b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py @@ -5,8 +5,11 @@ from mmcv.ops import DynamicScatter from torch import Tensor, nn +from mmdet3d.models.voxel_encoders.utils import (PFNLayer, + get_paddings_indicator) from mmdet3d.registry import MODELS -from mmdet3d.models.voxel_encoders.utils import PFNLayer, get_paddings_indicator + + @MODELS.register_module() class PillarFeatureNetAutoware(nn.Module): """Pillar Feature Net. @@ -35,20 +38,20 @@ class PillarFeatureNetAutoware(nn.Module): the original behavior. Defaults to True. """ - def __init__(self, - in_channels: Optional[int] = 4, - feat_channels: Optional[tuple] = (64,), - with_distance: Optional[bool] = False, - with_cluster_center: Optional[bool] = True, - with_voxel_center: Optional[bool] = True, - voxel_size: Optional[Tuple[float]] = (0.2, 0.2, 4), - point_cloud_range: Optional[Tuple[float]] = (0, -40, -3, 70.4, - 40, 1), - norm_cfg: Optional[dict] = dict( - type='BN1d', eps=1e-3, momentum=0.01), - mode: Optional[str] = 'max', - legacy: Optional[bool] = True, - use_voxel_center_z: Optional[bool] = True, ): + def __init__( + self, + in_channels: Optional[int] = 4, + feat_channels: Optional[tuple] = (64, ), + with_distance: Optional[bool] = False, + with_cluster_center: Optional[bool] = True, + with_voxel_center: Optional[bool] = True, + voxel_size: Optional[Tuple[float]] = (0.2, 0.2, 4), + point_cloud_range: Optional[Tuple[float]] = (0, -40, -3, 70.4, 40, 1), + norm_cfg: Optional[dict] = dict(type='BN1d', eps=1e-3, momentum=0.01), + mode: Optional[str] = 'max', + legacy: Optional[bool] = True, + use_voxel_center_z: Optional[bool] = True, + ): super(PillarFeatureNetAutoware, self).__init__() assert len(feat_channels) > 0 self.legacy = legacy @@ -111,7 +114,7 @@ def forward(self, features: Tensor, num_points: Tensor, coors: Tensor, if self._with_cluster_center: points_mean = features[:, :, :3].sum( dim=1, keepdim=True) / num_points.type_as(features).view( - -1, 1, 1) + -1, 1, 1) f_cluster = features[:, :, :3] - points_mean features_ls.append(f_cluster) @@ -120,29 +123,30 @@ def forward(self, features: Tensor, num_points: Tensor, coors: Tensor, if self._with_voxel_center: center_feature_size = 3 if self.use_voxel_center_z else 2 if not self.legacy: - f_center = torch.zeros_like(features[:, :, :center_feature_size]) + f_center = torch.zeros_like( + features[:, :, :center_feature_size]) f_center[:, :, 0] = features[:, :, 0] - ( - coors[:, 3].to(dtype).unsqueeze(1) * self.vx + - self.x_offset) + coors[:, 3].to(dtype).unsqueeze(1) * self.vx + + self.x_offset) f_center[:, :, 1] = features[:, :, 1] - ( - coors[:, 2].to(dtype).unsqueeze(1) * self.vy + - self.y_offset) + coors[:, 2].to(dtype).unsqueeze(1) * self.vy + + self.y_offset) if self.use_voxel_center_z: f_center[:, :, 2] = features[:, :, 2] - ( - coors[:, 1].to(dtype).unsqueeze(1) * self.vz + - self.z_offset) + coors[:, 1].to(dtype).unsqueeze(1) * self.vz + + self.z_offset) else: f_center = features[:, :, :center_feature_size] f_center[:, :, 0] = f_center[:, :, 0] - ( - coors[:, 3].type_as(features).unsqueeze(1) * self.vx + - self.x_offset) + coors[:, 3].type_as(features).unsqueeze(1) * self.vx + + self.x_offset) f_center[:, :, 1] = f_center[:, :, 1] - ( - coors[:, 2].type_as(features).unsqueeze(1) * self.vy + - self.y_offset) + coors[:, 2].type_as(features).unsqueeze(1) * self.vy + + self.y_offset) if self.use_voxel_center_z: f_center[:, :, 2] = f_center[:, :, 2] - ( - coors[:, 1].type_as(features).unsqueeze(1) * self.vz + - self.z_offset) + coors[:, 1].type_as(features).unsqueeze(1) * self.vz + + self.z_offset) features_ls.append(f_center) if self._with_distance: @@ -162,4 +166,4 @@ def forward(self, features: Tensor, num_points: Tensor, coors: Tensor, for pfn in self.pfn_layers: features = pfn(features, num_points) - return features.squeeze(1) \ No newline at end of file + return features.squeeze(1) diff --git a/projects/AutowareCenterPoint/centerpoint_onnx_converter.py b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py index 7a6a293959..215e54d3f6 100644 --- a/projects/AutowareCenterPoint/centerpoint_onnx_converter.py +++ b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py @@ -1,18 +1,24 @@ import argparse import os +from typing import Any, Dict, List, Optional, Tuple + import torch -from typing import Any, Dict, Optional, Tuple, List from mmengine import Config from mmengine.registry import MODELS, Registry from mmengine.runner import Runner + from mmdet3d.apis import init_model -from mmdet3d.models.dense_heads.centerpoint_head import SeparateHead, CenterHead +from mmdet3d.models.dense_heads.centerpoint_head import (CenterHead, + SeparateHead) from mmdet3d.models.voxel_encoders.utils import get_paddings_indicator -from projects.AutowareCenterPoint.centerpoint.pillar_encoder_autoware import PillarFeatureNetAutoware +from projects.AutowareCenterPoint.centerpoint.pillar_encoder_autoware import \ + PillarFeatureNetAutoware def parse_args(): - parser = argparse.ArgumentParser(description='Create autoware compitable onnx file from torch checkpoint ') + parser = argparse.ArgumentParser( + description= + 'Create autoware compitable onnx file from torch checkpoint ') parser.add_argument('--cfg', help='train config file path') parser.add_argument('--ckpt', help='checkpoint weeight') parser.add_argument('--work-dir', help='the dir to save onnx files') @@ -22,15 +28,17 @@ def parse_args(): class CenterPointToONNX(object): + def __init__( - self, - config: Config, - checkpoint_path: Optional[str] = None, - output_path: Optional[str] = None, + self, + config: Config, + checkpoint_path: Optional[str] = None, + output_path: Optional[str] = None, ): - assert isinstance(config, Config), f"expected `mmcv.Config`, but got {type(config)}" + assert isinstance( + config, Config), f'expected `mmcv.Config`, but got {type(config)}' _, ext = os.path.splitext(checkpoint_path) - assert ext == ".pth", f"expected .pth model, but got {ext}" + assert ext == '.pth', f'expected .pth model, but got {ext}' self.config = config self.checkpoint_path = checkpoint_path @@ -40,40 +48,49 @@ def __init__( def save_onnx(self) -> None: # Overwrite models with Autoware's TensorRT compatible versions - self.config.model.pts_voxel_encoder.type = "PillarFeatureNetONNX" - self.config.model.pts_bbox_head.type = "CenterHeadONNX" - self.config.model.pts_bbox_head.separate_head.type = "SeparateHeadONNX" + self.config.model.pts_voxel_encoder.type = 'PillarFeatureNetONNX' + self.config.model.pts_bbox_head.type = 'CenterHeadONNX' + self.config.model.pts_bbox_head.separate_head.type = 'SeparateHeadONNX' - model = init_model(self.config, self.checkpoint_path, device="cuda:0") + model = init_model(self.config, self.checkpoint_path, device='cuda:0') dataloader = Runner.build_dataloader(self.config.test_dataloader) batch_dict = next(iter(dataloader)) - voxel_dict = model.data_preprocessor.voxelize(batch_dict["inputs"]["points"], batch_dict) - input_features = model.pts_voxel_encoder.get_input_features(voxel_dict["voxels"], voxel_dict["num_points"], - voxel_dict["coors"]).to("cuda:0") + voxel_dict = model.data_preprocessor.voxelize( + batch_dict['inputs']['points'], batch_dict) + input_features = model.pts_voxel_encoder.get_input_features( + voxel_dict['voxels'], voxel_dict['num_points'], + voxel_dict['coors']).to('cuda:0') # CenterPoint's PointPillar voxel encoder ONNX conversion - pth_onnx_pve = os.path.join(self.output_path, "pts_voxel_encoder_centerpoint_custom.onnx") + pth_onnx_pve = os.path.join( + self.output_path, 'pts_voxel_encoder_centerpoint_custom.onnx') torch.onnx.export( model.pts_voxel_encoder, - (input_features,), + (input_features, ), f=pth_onnx_pve, - input_names=("input_features",), - output_names=("pillar_features",), + input_names=('input_features', ), + output_names=('pillar_features', ), dynamic_axes={ - "input_features": {0: "num_voxels", 1: "num_max_points"}, - "pillar_features": {0: "num_voxels"}, + 'input_features': { + 0: 'num_voxels', + 1: 'num_max_points' + }, + 'pillar_features': { + 0: 'num_voxels' + }, }, verbose=False, opset_version=11, ) - print(f"Saved pts_voxel_encoder onnx model: {pth_onnx_pve}") + print(f'Saved pts_voxel_encoder onnx model: {pth_onnx_pve}') voxel_features = model.pts_voxel_encoder(input_features) voxel_features = voxel_features.squeeze() - batch_size = voxel_dict["coors"][-1, 0] + 1 - x = model.pts_middle_encoder(voxel_features, voxel_dict["coors"], batch_size) + batch_size = voxel_dict['coors'][-1, 0] + 1 + x = model.pts_middle_encoder(voxel_features, voxel_dict['coors'], + batch_size) # CenterPoint backbone's to neck ONNX conversion pts_backbone_neck_head = CenterPointHeadONNX( @@ -82,32 +99,42 @@ def save_onnx(self) -> None: model.pts_bbox_head, ) - pth_onnx_backbone_neck_head = os.path.join(self.output_path, "pts_backbone_neck_head_centerpoint_custom.onnx") + pth_onnx_backbone_neck_head = os.path.join( + self.output_path, 'pts_backbone_neck_head_centerpoint_custom.onnx') torch.onnx.export( pts_backbone_neck_head, - (x,), + (x, ), f=pth_onnx_backbone_neck_head, - input_names=("spatial_features",), + input_names=('spatial_features', ), output_names=tuple(model.pts_bbox_head.output_names), dynamic_axes={ - name: {0: "batch_size", 2: "H", 3: "W"} - for name in ["spatial_features"] + model.pts_bbox_head.output_names + name: { + 0: 'batch_size', + 2: 'H', + 3: 'W' + } + for name in ['spatial_features'] + + model.pts_bbox_head.output_names }, verbose=False, opset_version=11, ) - print(f"Saved pts_backbone_neck_head onnx model: {pth_onnx_backbone_neck_head}") + print( + f'Saved pts_backbone_neck_head onnx model: {pth_onnx_backbone_neck_head}' + ) @MODELS.register_module() class PillarFeatureNetONNX(PillarFeatureNetAutoware): + def __init__(self, **kwargs): super().__init__(**kwargs) - def get_input_features( - self, features: torch.Tensor, num_points: torch.Tensor, coors: torch.Tensor - ) -> torch.Tensor: + def get_input_features(self, features: torch.Tensor, + num_points: torch.Tensor, + coors: torch.Tensor) -> torch.Tensor: """Forward function. + Args: features (torch.Tensor): Point features or raw points in shape (N, M, C). @@ -122,7 +149,7 @@ def get_input_features( if self._with_cluster_center: points_mean = features[:, :, :3].sum( dim=1, keepdim=True) / num_points.type_as(features).view( - -1, 1, 1) + -1, 1, 1) f_cluster = features[:, :, :3] - points_mean features_ls.append(f_cluster) @@ -131,29 +158,30 @@ def get_input_features( if self._with_voxel_center: center_feature_size = 3 if self.use_voxel_center_z else 2 if not self.legacy: - f_center = torch.zeros_like(features[:, :, :center_feature_size]) + f_center = torch.zeros_like( + features[:, :, :center_feature_size]) f_center[:, :, 0] = features[:, :, 0] - ( - coors[:, 3].to(dtype).unsqueeze(1) * self.vx + - self.x_offset) + coors[:, 3].to(dtype).unsqueeze(1) * self.vx + + self.x_offset) f_center[:, :, 1] = features[:, :, 1] - ( - coors[:, 2].to(dtype).unsqueeze(1) * self.vy + - self.y_offset) + coors[:, 2].to(dtype).unsqueeze(1) * self.vy + + self.y_offset) if self.use_voxel_center_z: f_center[:, :, 2] = features[:, :, 2] - ( - coors[:, 1].to(dtype).unsqueeze(1) * self.vz + - self.z_offset) + coors[:, 1].to(dtype).unsqueeze(1) * self.vz + + self.z_offset) else: f_center = features[:, :, :center_feature_size] f_center[:, :, 0] = f_center[:, :, 0] - ( - coors[:, 3].type_as(features).unsqueeze(1) * self.vx + - self.x_offset) + coors[:, 3].type_as(features).unsqueeze(1) * self.vx + + self.x_offset) f_center[:, :, 1] = f_center[:, :, 1] - ( - coors[:, 2].type_as(features).unsqueeze(1) * self.vy + - self.y_offset) + coors[:, 2].type_as(features).unsqueeze(1) * self.vy + + self.y_offset) if self.use_voxel_center_z: f_center[:, :, 2] = f_center[:, :, 2] - ( - coors[:, 1].type_as(features).unsqueeze(1) * self.vz + - self.z_offset) + coors[:, 1].type_as(features).unsqueeze(1) * self.vz + + self.z_offset) features_ls.append(f_center) if self._with_distance: @@ -170,10 +198,11 @@ def get_input_features( return features def forward( - self, - features: torch.Tensor, + self, + features: torch.Tensor, ) -> torch.Tensor: """Forward function. + Args: features (torch.Tensor): Point features in shape (N, M, C). num_points (torch.Tensor): Number of points in each pillar. @@ -190,23 +219,25 @@ def forward( @MODELS.register_module() class SeparateHeadONNX(SeparateHead): + def __init__(self, **kwargs): super().__init__(**kwargs) # Order output's of the heads - rot_heads = {k: None for k in self.heads.keys() if "rot" in k} + rot_heads = {k: None for k in self.heads.keys() if 'rot' in k} self.heads: Dict[str, None] = { - "heatmap": None, - "reg": None, - "height": None, - "dim": None, + 'heatmap': None, + 'reg': None, + 'height': None, + 'dim': None, **rot_heads, - "vel": None, + 'vel': None, } @MODELS.register_module() class CenterHeadONNX(CenterHead): + def __init__(self, **kwargs): super().__init__(**kwargs) @@ -220,7 +251,9 @@ def forward(self, x: List[torch.Tensor]) -> Tuple[torch.Tensor]: Returns: pred (Tuple[torch.Tensor]): Output results for tasks. """ - assert len(x) == 1, "The input of CenterHeadONNX must be a single-level feature" + assert len( + x + ) == 1, 'The input of CenterHeadONNX must be a single-level feature' x = self.shared_conv(x[0]) head_tensors: Dict[str, torch.Tensor] = self.task_heads[0](x) @@ -234,7 +267,8 @@ def forward(self, x: List[torch.Tensor]) -> Tuple[torch.Tensor]: class CenterPointHeadONNX(torch.nn.Module): - def __init__(self, backbone: torch.nn.Module, neck: torch.nn.Module, bbox_head: torch.nn.Module): + def __init__(self, backbone: torch.nn.Module, neck: torch.nn.Module, + bbox_head: torch.nn.Module): super(CenterPointHeadONNX, self).__init__() self.backbone: torch.nn.Module = backbone self.neck: torch.nn.Module = neck @@ -265,7 +299,8 @@ def main(): args = parse_args() cfg = Config.fromfile(args.cfg) - det = CenterPointToONNX(config=cfg, checkpoint_path=args.ckpt, output_path=args.work_dir) + det = CenterPointToONNX( + config=cfg, checkpoint_path=args.ckpt, output_path=args.work_dir) det.save_onnx() diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom.py b/projects/AutowareCenterPoint/configs/centerpoint_custom.py index 9f466287a8..74c954fa6a 100755 --- a/projects/AutowareCenterPoint/configs/centerpoint_custom.py +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom.py @@ -1,5 +1,6 @@ custom_imports = dict( - imports=['projects.AutowareCenterPoint.centerpoint'], allow_failed_imports=False) + imports=['projects.AutowareCenterPoint.centerpoint'], + allow_failed_imports=False) auto_scale_lr = dict(base_batch_size=32, enable=False) backend_args = None @@ -844,4 +845,4 @@ 0.2, 8, ] -work_dir = './work_dirs/centerpoint_pillar02_second_secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d-CUSTOM' \ No newline at end of file +work_dir = './work_dirs/centerpoint_pillar02_second_secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d-CUSTOM' diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py index 55e3d72fcd..c83e941203 100644 --- a/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom_test.py @@ -1,15 +1,30 @@ custom_imports = dict( - imports=['projects.AutowareCenterPoint.centerpoint', - 'projects.AutowareCenterPoint.datasets', - 'projects.AutowareCenterPoint.evaluation'], allow_failed_imports=False) + imports=[ + 'projects.AutowareCenterPoint.centerpoint', + 'projects.AutowareCenterPoint.datasets', + 'projects.AutowareCenterPoint.evaluation' + ], + allow_failed_imports=False) auto_scale_lr = dict(base_batch_size=32, enable=False) backend_args = None -use_dim =[0,1,2,4,] +use_dim = [ + 0, + 1, + 2, + 4, +] use_dim_num = 5 load_dim_num = 5 -point_cloud_range = [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0,] +point_cloud_range = [ + -51.2, + -51.2, + -5.0, + 51.2, + 51.2, + 3.0, +] class_names = [ 'car', @@ -19,15 +34,19 @@ 'pedestrian', ] -voxel_size=[0.2, 0.2, 8.0,] +voxel_size = [ + 0.2, + 0.2, + 8.0, +] data_prefix = dict(img='', pts='', sweeps='') -data_root = "data/sample_dataset/" +data_root = 'data/sample_dataset/' dataset_type = 'T4Dataset' -train_ann_file = dataset_type + '_infos_train.pkl' -val_ann_file = dataset_type + '_infos_val.pkl' -test_ann_file = dataset_type + '_infos_train.pkl' +train_ann_file = dataset_type + '_infos_train.pkl' +val_ann_file = dataset_type + '_infos_val.pkl' +test_ann_file = dataset_type + '_infos_train.pkl' db_info_path = data_root + '/' + dataset_type + '_dbinfos_train.pkl' @@ -170,9 +189,7 @@ final_kernel=3, init_bias=-2.19, type='SeparateHead'), share_conv_channel=64, tasks=[ - dict( - class_names=class_names, - num_class=5), + dict(class_names=class_names, num_class=5), ], type='CenterHead'), pts_middle_encoder=dict( @@ -357,7 +374,7 @@ persistent_workers=True, sampler=dict(shuffle=False, type='DefaultSampler')) test_evaluator = dict( - ann_file= data_root + test_ann_file, + ann_file=data_root + test_ann_file, backend_args=None, data_root=data_root, metric='bbox', @@ -458,9 +475,7 @@ dict( point_cloud_range=point_cloud_range, type='ObjectRangeFilter'), - dict( - classes=class_names, - type='ObjectNameFilter'), + dict(classes=class_names, type='ObjectNameFilter'), dict(type='PointShuffle'), dict( keys=[ @@ -497,7 +512,7 @@ backend_args=None, classes=class_names, data_root=data_root, - info_path=data_root+ train_ann_file, + info_path=data_root + train_ann_file, points_loader=dict( backend_args=None, coord_type='LIDAR', @@ -534,16 +549,9 @@ flip_ratio_bev_vertical=0.5, sync_2d=False, type='RandomFlip3D'), - dict( - point_cloud_range=point_cloud_range - , - type='PointsRangeFilter'), - dict( - point_cloud_range=point_cloud_range, - type='ObjectRangeFilter'), - dict( - classes=class_names, - type='ObjectNameFilter'), + dict(point_cloud_range=point_cloud_range, type='PointsRangeFilter'), + dict(point_cloud_range=point_cloud_range, type='ObjectRangeFilter'), + dict(classes=class_names, type='ObjectNameFilter'), dict(type='PointShuffle'), dict( keys=[ @@ -604,4 +612,4 @@ dict(type='LocalVisBackend'), ]) -work_dir = './work_dirs/centerpoint_custom_test' \ No newline at end of file +work_dir = './work_dirs/centerpoint_custom_test' diff --git a/projects/AutowareCenterPoint/datasets/__init__.py b/projects/AutowareCenterPoint/datasets/__init__.py index 0f1b7de3dc..5bd4e494fc 100644 --- a/projects/AutowareCenterPoint/datasets/__init__.py +++ b/projects/AutowareCenterPoint/datasets/__init__.py @@ -1,3 +1,3 @@ from .tier4_dataset import T4Dataset -__all__ = [ 'T4Dataset'] \ No newline at end of file +__all__ = ['T4Dataset'] diff --git a/projects/AutowareCenterPoint/datasets/tier4_dataset.py b/projects/AutowareCenterPoint/datasets/tier4_dataset.py index b7dad19d50..3abda317ef 100644 --- a/projects/AutowareCenterPoint/datasets/tier4_dataset.py +++ b/projects/AutowareCenterPoint/datasets/tier4_dataset.py @@ -1,21 +1,22 @@ -from os import path as osp import os +from os import path as osp from typing import Callable, List, Union import numpy as np +# from .det3d_dataset import Det3DDataset +from mmdet3d.datasets.nuscenes_dataset import NuScenesDataset from mmdet3d.registry import DATASETS from mmdet3d.structures import LiDARInstance3DBoxes from mmdet3d.structures.bbox_3d.cam_box3d import CameraInstance3DBoxes -# from .det3d_dataset import Det3DDataset -from mmdet3d.datasets.nuscenes_dataset import NuScenesDataset @DATASETS.register_module() class T4Dataset(NuScenesDataset): METAINFO = { 'classes': ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), - 'version': 'v1.0-trainval', + 'version': + 'v1.0-trainval', 'palette': [ (255, 158, 0), # Orange (255, 99, 71), # Tomato @@ -25,12 +26,14 @@ class T4Dataset(NuScenesDataset): ] } - def __init__(self, - box_type_3d: str = 'LiDAR', - load_type: str = 'frame_based', - with_velocity: bool = True, - use_valid_flag: bool = False, - **kwargs,) -> None: + def __init__( + self, + box_type_3d: str = 'LiDAR', + load_type: str = 'frame_based', + with_velocity: bool = True, + use_valid_flag: bool = False, + **kwargs, + ) -> None: self.use_valid_flag = use_valid_flag self.with_velocity = with_velocity @@ -70,13 +73,15 @@ def parse_data_info(self, info: dict) -> dict: info['lidar_path'] = info['lidar_points']['lidar_path'] if 'lidar_sweeps' in info: for sweep in info['lidar_sweeps']: - file_suffix_splitted = sweep['lidar_points']['lidar_path'].split(os.sep) + file_suffix_splitted = sweep['lidar_points'][ + 'lidar_path'].split(os.sep) file_suffix = os.sep.join(file_suffix_splitted[-4:]) if 'samples' in sweep['lidar_points']['lidar_path']: sweep['lidar_points']['lidar_path'] = osp.join( self.data_prefix['pts'], file_suffix) else: - sweep['lidar_points']['lidar_path'] = info['lidar_points']['lidar_path'] + sweep['lidar_points']['lidar_path'] = info[ + 'lidar_points']['lidar_path'] if self.modality['use_camera']: for cam_id, img_info in info['images'].items(): @@ -85,8 +90,8 @@ def parse_data_info(self, info: dict) -> dict: cam_prefix = self.data_prefix[cam_id] else: cam_prefix = self.data_prefix.get('img', '') - img_info['img_path'] = osp.join(cam_prefix, - img_info['img_path']) + img_info['img_path'] = osp.join( + cam_prefix, img_info['img_path']) if self.default_cam_key is not None: info['img_path'] = info['images'][ self.default_cam_key]['img_path'] @@ -109,6 +114,3 @@ def parse_data_info(self, info: dict) -> dict: info['eval_ann_info'] = self.parse_ann_info(info) return info - - - diff --git a/projects/AutowareCenterPoint/evaluation/__init__.py b/projects/AutowareCenterPoint/evaluation/__init__.py index 75a2057d51..fbb4dd9c92 100644 --- a/projects/AutowareCenterPoint/evaluation/__init__.py +++ b/projects/AutowareCenterPoint/evaluation/__init__.py @@ -1,17 +1,16 @@ -from .functional.nuscenes_utils.eval import DetectionConfig, nuScenesDetectionEval +from .functional.nuscenes_utils.eval import (DetectionConfig, + nuScenesDetectionEval) from .functional.nuscenes_utils.utils import ( - class_mapping_kitti2nuscenes, - format_nuscenes_metrics, - format_nuscenes_metrics_table, - transform_det_annos_to_nusc_annos, -) - + class_mapping_kitti2nuscenes, format_nuscenes_metrics, + format_nuscenes_metrics_table, transform_det_annos_to_nusc_annos) from .metrics.nuscenes_custom_metric import NuScenesCustomMetric -__all__ = ['NuScenesCustomMetric,' - 'DetectionConfig' - 'nuScenesDetectionEval' - 'class_mapping_kitti2nuscenes' - 'format_nuscenes_metrics_table' - 'format_nuscenes_metrics' - 'transform_det_annos_to_nusc_annos'] +__all__ = [ + 'NuScenesCustomMetric,' + 'DetectionConfig' + 'nuScenesDetectionEval' + 'class_mapping_kitti2nuscenes' + 'format_nuscenes_metrics_table' + 'format_nuscenes_metrics' + 'transform_det_annos_to_nusc_annos' +] diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py index 85608f4c01..a45913e625 100644 --- a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/__init__.py @@ -1,16 +1,13 @@ from .eval import DetectionConfig, nuScenesDetectionEval -from .utils import ( - class_mapping_kitti2nuscenes, - format_nuscenes_metrics, - format_nuscenes_metrics_table, - transform_det_annos_to_nusc_annos, -) +from .utils import (class_mapping_kitti2nuscenes, format_nuscenes_metrics, + format_nuscenes_metrics_table, + transform_det_annos_to_nusc_annos) __all__ = [ - "DetectionConfig", - "nuScenesDetectionEval", - "class_mapping_kitti2nuscenes", - "format_nuscenes_metrics_table", - "format_nuscenes_metrics", - "transform_det_annos_to_nusc_annos", + 'DetectionConfig', + 'nuScenesDetectionEval', + 'class_mapping_kitti2nuscenes', + 'format_nuscenes_metrics_table', + 'format_nuscenes_metrics', + 'transform_det_annos_to_nusc_annos', ] diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py index 755b582910..7aa5b50b78 100644 --- a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py @@ -9,7 +9,9 @@ from nuscenes.eval.detection.data_classes import DetectionBox from nuscenes.eval.detection.evaluate import DetectionEval as _DetectionEval -def toEvalBoxes(nusc_boxes: Dict[str, List[Dict]], box_cls: EvalBox = DetectionBox) -> EvalBoxes: + +def toEvalBoxes(nusc_boxes: Dict[str, List[Dict]], + box_cls: EvalBox = DetectionBox) -> EvalBoxes: """ nusc_boxes = { @@ -56,7 +58,7 @@ def __init__( ): # assert set(class_range.keys()) == set(DETECTION_NAMES), "Class count mismatch." - assert dist_th_tp in dist_ths, "dist_th_tp must be in set of dist_ths." + assert dist_th_tp in dist_ths, 'dist_th_tp must be in set of dist_ths.' self.class_range = class_range self.dist_fcn = dist_fcn @@ -78,46 +80,48 @@ def __eq__(self, other): def serialize(self) -> dict: """Serialize instance into json-friendly format.""" return { - "class_names": self.class_names, - "class_range": self.class_range, - "dist_fcn": self.dist_fcn, - "dist_ths": self.dist_ths, - "dist_th_tp": self.dist_th_tp, - "min_recall": self.min_recall, - "min_precision": self.min_precision, - "max_boxes_per_sample": self.max_boxes_per_sample, - "mean_ap_weight": self.mean_ap_weight, + 'class_names': self.class_names, + 'class_range': self.class_range, + 'dist_fcn': self.dist_fcn, + 'dist_ths': self.dist_ths, + 'dist_th_tp': self.dist_th_tp, + 'min_recall': self.min_recall, + 'min_precision': self.min_precision, + 'max_boxes_per_sample': self.max_boxes_per_sample, + 'mean_ap_weight': self.mean_ap_weight, } @classmethod def deserialize(cls, content: dict): """Initialize from serialized dictionary.""" return cls( - content["class_names"], - content["class_range"], - content["dist_fcn"], - content["dist_ths"], - content["dist_th_tp"], - content["min_recall"], - content["min_precision"], - content["max_boxes_per_sample"], - content["mean_ap_weight"], + content['class_names'], + content['class_range'], + content['dist_fcn'], + content['dist_ths'], + content['dist_th_tp'], + content['min_recall'], + content['min_precision'], + content['max_boxes_per_sample'], + content['mean_ap_weight'], ) @property def dist_fcn_callable(self): - """Return the distance function corresponding to the dist_fcn string.""" - if self.dist_fcn == "center_distance": + """Return the distance function corresponding to the dist_fcn + string.""" + if self.dist_fcn == 'center_distance': return center_distance else: - raise Exception("Error: Unknown distance function %s!" % self.dist_fcn) + raise Exception('Error: Unknown distance function %s!' % + self.dist_fcn) class nuScenesDetectionEval(_DetectionEval): - """ - This is the official nuScenes detection evaluation code. - Results are written to the provided output_dir. - nuScenes uses the following detection metrics: + """This is the official nuScenes detection evaluation code. Results are + written to the provided output_dir. nuScenes uses the following detection + metrics: + - Mean Average Precision (mAP): Uses center-distance as matching criterion; averaged over distance thresholds. - True Positive (TP) metrics: Average of translation, velocity, scale, orientation and attribute errors. - nuScenes Detection Score (NDS): The weighted sum of the above. @@ -140,8 +144,8 @@ def __init__( output_dir: Optional[str] = None, verbose: bool = True, ): - """ - Initialize a DetectionEval object. + """Initialize a DetectionEval object. + :param config: A DetectionConfig object. :param result_boxes: result bounding boxes. :param gt_boxes: ground-truth bounding boxes. @@ -156,7 +160,7 @@ def __init__( self.verbose = verbose # Make dirs. - self.plot_dir = os.path.join(self.output_dir, "plots") + self.plot_dir = os.path.join(self.output_dir, 'plots') if not os.path.isdir(self.output_dir): os.makedirs(self.output_dir) if not os.path.isdir(self.plot_dir): diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py index 0e186dc52a..f9cb1d82e7 100644 --- a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py @@ -1,146 +1,146 @@ -""" -modified from https://github.com/open-mmlab/OpenPCDet/blob/v0.5.2/pcdet/datasets/nuscenes/nuscenes_utils.py -""" -from collections import defaultdict +"""modified from https://github.com/open- +mmlab/OpenPCDet/blob/v0.5.2/pcdet/datasets/nuscenes/nuscenes_utils.py.""" import operator +from collections import defaultdict from typing import Dict, List -from mmengine import print_log import numpy as np -from nuscenes.utils.data_classes import Box import pandas as pd +from mmengine import print_log +from nuscenes.utils.data_classes import Box from pyquaternion import Quaternion from terminaltables import AsciiTable class_mapping_kitti2nuscenes = { - "Car": "car", - "Cyclist": "bicycle", - "Pedestrian": "pedestrian", + 'Car': 'car', + 'Cyclist': 'bicycle', + 'Pedestrian': 'pedestrian', } cls_attr_dist = { - "barrier": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 0, - "vehicle.parked": 0, - "vehicle.stopped": 0, + 'barrier': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 0, + 'vehicle.parked': 0, + 'vehicle.stopped': 0, }, - "bicycle": { - "cycle.with_rider": 2791, - "cycle.without_rider": 8946, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 0, - "vehicle.parked": 0, - "vehicle.stopped": 0, + 'bicycle': { + 'cycle.with_rider': 2791, + 'cycle.without_rider': 8946, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 0, + 'vehicle.parked': 0, + 'vehicle.stopped': 0, }, - "bus": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 9092, - "vehicle.parked": 3294, - "vehicle.stopped": 3881, + 'bus': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 9092, + 'vehicle.parked': 3294, + 'vehicle.stopped': 3881, }, - "car": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 114304, - "vehicle.parked": 330133, - "vehicle.stopped": 46898, + 'car': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 114304, + 'vehicle.parked': 330133, + 'vehicle.stopped': 46898, }, - "construction_vehicle": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 882, - "vehicle.parked": 11549, - "vehicle.stopped": 2102, + 'construction_vehicle': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 882, + 'vehicle.parked': 11549, + 'vehicle.stopped': 2102, }, - "ignore": { - "cycle.with_rider": 307, - "cycle.without_rider": 73, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 165, - "vehicle.parked": 400, - "vehicle.stopped": 102, + 'ignore': { + 'cycle.with_rider': 307, + 'cycle.without_rider': 73, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 165, + 'vehicle.parked': 400, + 'vehicle.stopped': 102, }, - "motorcycle": { - "cycle.with_rider": 4233, - "cycle.without_rider": 8326, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 0, - "vehicle.parked": 0, - "vehicle.stopped": 0, + 'motorcycle': { + 'cycle.with_rider': 4233, + 'cycle.without_rider': 8326, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 0, + 'vehicle.parked': 0, + 'vehicle.stopped': 0, }, - "pedestrian": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 157444, - "pedestrian.sitting_lying_down": 13939, - "pedestrian.standing": 46530, - "vehicle.moving": 0, - "vehicle.parked": 0, - "vehicle.stopped": 0, + 'pedestrian': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 157444, + 'pedestrian.sitting_lying_down': 13939, + 'pedestrian.standing': 46530, + 'vehicle.moving': 0, + 'vehicle.parked': 0, + 'vehicle.stopped': 0, }, - "traffic_cone": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 0, - "vehicle.parked": 0, - "vehicle.stopped": 0, + 'traffic_cone': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 0, + 'vehicle.parked': 0, + 'vehicle.stopped': 0, }, - "trailer": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 3421, - "vehicle.parked": 19224, - "vehicle.stopped": 1895, + 'trailer': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 3421, + 'vehicle.parked': 19224, + 'vehicle.stopped': 1895, }, - "truck": { - "cycle.with_rider": 0, - "cycle.without_rider": 0, - "pedestrian.moving": 0, - "pedestrian.sitting_lying_down": 0, - "pedestrian.standing": 0, - "vehicle.moving": 21339, - "vehicle.parked": 55626, - "vehicle.stopped": 11097, + 'truck': { + 'cycle.with_rider': 0, + 'cycle.without_rider': 0, + 'pedestrian.moving': 0, + 'pedestrian.sitting_lying_down': 0, + 'pedestrian.standing': 0, + 'vehicle.moving': 21339, + 'vehicle.parked': 55626, + 'vehicle.stopped': 11097, }, } def boxes_lidar_to_nuscenes(det_info): - boxes3d = det_info["boxes_lidar"] - scores = det_info["score"] - labels = det_info["pred_labels"] + boxes3d = det_info['boxes_lidar'] + scores = det_info['score'] + labels = det_info['pred_labels'] box_list = [] for k in range(boxes3d.shape[0]): quat = Quaternion(axis=[0, 0, 1], radians=boxes3d[k, 6]) - velocity = (*boxes3d[k, 7:9], 0.0) if boxes3d.shape[1] == 9 else (0.0, 0.0, 0.0) + velocity = (*boxes3d[k, 7:9], + 0.0) if boxes3d.shape[1] == 9 else (0.0, 0.0, 0.0) box = Box( boxes3d[k, :3], boxes3d[k, 3:6], # wlh @@ -154,8 +154,9 @@ def boxes_lidar_to_nuscenes(det_info): def transform_det_annos_to_nusc_annos(det_annos, velocity_threshold=0.2): - """convert detections to nuscenes-format for evaluation - modified from: https://github.com/open-mmlab/OpenPCDet/blob/master/pcdet/datasets/nuscenes/nuscenes_utils.py#L423 + """convert detections to nuscenes-format for evaluation modified from: + https://github.com/open- + mmlab/OpenPCDet/blob/master/pcdet/datasets/nuscenes/nuscenes_utils.py#L423. Args: det_annos ([type]): [description] @@ -185,118 +186,117 @@ def transform_det_annos_to_nusc_annos(det_annos, velocity_threshold=0.2): box_list = boxes_lidar_to_nuscenes(det) for k, box in enumerate(box_list): - name = det["name"][k] - if np.sqrt(box.velocity[0] ** 2 + box.velocity[1] ** 2) > velocity_threshold: - if name in ["car", "construction_vehicle", "bus", "truck", "trailer"]: - attr = "vehicle.moving" - elif name in ["bicycle", "motorcycle"]: - attr = "cycle.with_rider" + name = det['name'][k] + if np.sqrt(box.velocity[0]**2 + + box.velocity[1]**2) > velocity_threshold: + if name in [ + 'car', 'construction_vehicle', 'bus', 'truck', + 'trailer' + ]: + attr = 'vehicle.moving' + elif name in ['bicycle', 'motorcycle']: + attr = 'cycle.with_rider' else: attr = None else: - if name in ["pedestrian"]: - attr = "pedestrian.standing" - elif name in ["bus"]: - attr = "vehicle.stopped" + if name in ['pedestrian']: + attr = 'pedestrian.standing' + elif name in ['bus']: + attr = 'vehicle.stopped' else: attr = None attr = ( - attr - if attr is not None - else max(cls_attr_dist[name].items(), key=operator.itemgetter(1))[0] - ) + attr if attr is not None else max( + cls_attr_dist[name].items(), + key=operator.itemgetter(1))[0]) nusc_anno = { - "sample_token": det["metadata"]["token"], - "translation": box.center.tolist(), - "size": box.wlh.tolist(), - "rotation": box.orientation.elements.tolist(), - "velocity": box.velocity[:2].tolist(), - "detection_name": name, - "detection_score": box.score, - "attribute_name": attr, + 'sample_token': det['metadata']['token'], + 'translation': box.center.tolist(), + 'size': box.wlh.tolist(), + 'rotation': box.orientation.elements.tolist(), + 'velocity': box.velocity[:2].tolist(), + 'detection_name': name, + 'detection_score': box.score, + 'attribute_name': attr, } annos.append(nusc_anno) - nusc_annos.update({det["metadata"]["token"]: annos}) + nusc_annos.update({det['metadata']['token']: annos}) return nusc_annos -def format_nuscenes_metrics(metrics: Dict, class_names: List[str], version="default"): - result = f"----------------nuScenes {version} results-----------------\n" +def format_nuscenes_metrics(metrics: Dict, + class_names: List[str], + version='default'): + result = f'----------------nuScenes {version} results-----------------\n' result_dict: Dict[str, Dict[str, float]] = defaultdict(dict) for name in class_names: - result_dict[name].update( - { - f"mAP": sum([v for v in metrics["label_aps"][name].values()]) - * 100 - / len(metrics["label_aps"][name]) - } - ) - result_dict[name].update( - {f"AP@{k}m": v * 100 for k, v in metrics["label_aps"][name].items()} - ) - result_dict[name].update( - {f"error@{k}": v for k, v in metrics["label_tp_errors"][name].items()} - ) - result_dict[name].pop("error@attr_err") - - df = pd.DataFrame.from_dict(result_dict, orient="index") - df.index.name = "class_name" - result += df.to_markdown(mode="str", floatfmt=".2f") + result_dict[name].update({ + f'mAP': + sum([v for v in metrics['label_aps'][name].values()]) * 100 / + len(metrics['label_aps'][name]) + }) + result_dict[name].update({ + f'AP@{k}m': v * 100 + for k, v in metrics['label_aps'][name].items() + }) + result_dict[name].update({ + f'error@{k}': v + for k, v in metrics['label_tp_errors'][name].items() + }) + result_dict[name].pop('error@attr_err') + + df = pd.DataFrame.from_dict(result_dict, orient='index') + df.index.name = 'class_name' + result += df.to_markdown(mode='str', floatfmt='.2f') details = {} - for key, val in metrics["tp_errors"].items(): + for key, val in metrics['tp_errors'].items(): details[key] = val - for key, val in metrics["mean_dist_aps"].items(): - details[f"mAP_{key}"] = val + for key, val in metrics['mean_dist_aps'].items(): + details[f'mAP_{key}'] = val - details.update( - { - "mAP": metrics["mean_ap"], - "NDS": metrics["nd_score"], - } - ) + details.update({ + 'mAP': metrics['mean_ap'], + 'NDS': metrics['nd_score'], + }) return result, details def format_nuscenes_metrics_table(metrics, class_names, logger=None): name = class_names[0] - thresholds = list(map(str, metrics["label_aps"][name].keys())) + thresholds = list(map(str, metrics['label_aps'][name].keys())) # not use error@attr - errors = [x.split("_")[0] for x in list(metrics["label_tp_errors"][name].keys())][:4] + errors = [ + x.split('_')[0] for x in list(metrics['label_tp_errors'][name].keys()) + ][:4] - APs_data = [ - ["class", "mAP"] + [f"AP@{e}" for e in thresholds] + [f"error@{e}" for e in errors] - ] + APs_data = [['class', 'mAP'] + [f'AP@{e}' for e in thresholds] + + [f'error@{e}' for e in errors]] for name in class_names: - ap_list = list(metrics["label_aps"][name].values()) - error_list = list(metrics["label_tp_errors"][name].values())[:4] - - mAP = round(metrics["mean_dist_aps"][name] * 100, 3) - AP = ( - [name, mAP] - + [round(ap * 100, 3) for ap in ap_list] - + [round(e, 3) for e in error_list] - ) + ap_list = list(metrics['label_aps'][name].values()) + error_list = list(metrics['label_tp_errors'][name].values())[:4] + + mAP = round(metrics['mean_dist_aps'][name] * 100, 3) + AP = ([name, mAP] + [round(ap * 100, 3) for ap in ap_list] + + [round(e, 3) for e in error_list]) APs_data.append(AP) APs_table = AsciiTable(APs_data) # APs_table.inner_footing_row_border = True - print_log("\n" + APs_table.table, logger=logger) + print_log('\n' + APs_table.table, logger=logger) details = {} - for key, val in metrics["tp_errors"].items(): + for key, val in metrics['tp_errors'].items(): details[key] = val - details.update( - { - "mAP": metrics["mean_ap"], - "NDS": metrics["nd_score"], - } - ) + details.update({ + 'mAP': metrics['mean_ap'], + 'NDS': metrics['nd_score'], + }) return details diff --git a/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py index 104194df45..06bc0378f1 100644 --- a/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py +++ b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py @@ -3,6 +3,7 @@ from os import path as osp from typing import Dict, List, Optional, Sequence, Tuple, Union +import mmcv import mmengine import numpy as np import pyquaternion @@ -18,11 +19,8 @@ from mmdet3d.registry import METRICS from mmdet3d.structures import (CameraInstance3DBoxes, LiDARInstance3DBoxes, bbox3d2result, xywhr2xyxyr) - from projects.AutowareCenterPoint.evaluation.functional import nuscenes_utils -import mmcv -import mmengine @METRICS.register_module() class NuScenesCustomMetric(BaseMetric): @@ -54,12 +52,12 @@ class NuScenesCustomMetric(BaseMetric): corresponding backend. Defaults to None. """ CLASSES = ( - "car", - "truck", - "bus", - "bicycle", - "pedestrian", - ) + 'car', + 'truck', + 'bus', + 'bicycle', + 'pedestrian', + ) NameMapping = { 'movable_object.barrier': 'barrier', @@ -242,13 +240,13 @@ def _evaluate_single( output_dir = osp.join(*osp.split(result_path)[:-1]) - result_dict = mmengine.load(result_path)["results"] - + result_dict = mmengine.load(result_path)['results'] tmp_dir = tempfile.TemporaryDirectory() assert tmp_dir is not None - gt_dict = mmengine.load(self._format_gt_to_nusc(tmp_dir.name))["results"] + gt_dict = mmengine.load(self._format_gt_to_nusc( + tmp_dir.name))['results'] tmp_dir.cleanup() nusc_eval = nuscenes_utils.nuScenesDetectionEval( @@ -256,7 +254,7 @@ def _evaluate_single( result_boxes=result_dict, gt_boxes=gt_dict, meta=self.modality, - eval_set="val", + eval_set='val', output_dir=output_dir, verbose=False, ) @@ -265,10 +263,9 @@ def _evaluate_single( metrics_summary = metrics.serialize() metrics_str, ap_dict = nuscenes_utils.format_nuscenes_metrics( - metrics_summary, sorted(set(self.CLASSES), key=self.CLASSES.index) - ) + metrics_summary, sorted(set(self.CLASSES), key=self.CLASSES.index)) - detail = dict(result = metrics_str) + detail = dict(result=metrics_str) return detail def format_results( @@ -304,7 +301,6 @@ def format_results( result_dict = dict() sample_idx_list = [result['sample_idx'] for result in results] - for name in results[0]: if 'pred' in name and '3d' in name and name[0] != '_': print(f'\nFormating bboxes of {name}') @@ -545,8 +541,9 @@ def _format_lidar_bbox(self, mmengine.dump(nusc_submissions, res_path) return res_path - - def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = None): + def _format_gt_to_nusc(self, + output_dir: str, + pipeline: Optional[List[Dict]] = None): """Convert ground-truth annotations to nuscenes Box format. Args: output_dir (str): the path to output directory @@ -579,7 +576,8 @@ def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = N gt_scores_3d = torch.cat((gt_scores_3d, score_as_torch), 0) bbox3d = gt_raw[i]['bbox_3d'] - bbox3d_np = np.append(np.array(bbox3d), np.array(gt_raw[i]['velocity'])) + bbox3d_np = np.append( + np.array(bbox3d), np.array(gt_raw[i]['velocity'])) if i == 0: bboxes_3d = np.array([bbox3d_np]) @@ -587,34 +585,33 @@ def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = N bboxes_3d = np.vstack([bboxes_3d, np.array(bbox3d_np)]) gt_bboxes_3d = LiDARInstance3DBoxes( - bboxes_3d, - box_dim=bboxes_3d.shape[-1], - origin=(0.5, 0.5, 0.5), - ) + bboxes_3d, + box_dim=bboxes_3d.shape[-1], + origin=(0.5, 0.5, 0.5), + ) instance_bboxes = dict( labels_3d=gt_labels_3d, - bboxes_3d= gt_bboxes_3d, + bboxes_3d=gt_bboxes_3d, ) if pipeline is not None: instance_bboxes = pipeline(instance_bboxes) instance_result = dict( - labels_3d=instance_bboxes["labels_3d"], + labels_3d=instance_bboxes['labels_3d'], scores_3d=gt_scores_3d, - bboxes_3d= instance_bboxes["bboxes_3d"], - sample_idx = i, + bboxes_3d=instance_bboxes['bboxes_3d'], + sample_idx=i, ) - annos = [] boxes, attrs = output_to_nusc_box(instance_result) - boxes = lidar_nusc_box_to_global(self.data_infos[sample_id], - boxes, self.dataset_meta['classes'], + boxes = lidar_nusc_box_to_global(self.data_infos[sample_id], boxes, + self.dataset_meta['classes'], self.eval_detection_configs) for i, box in enumerate(boxes): - name = self.dataset_meta['classes'][box.label] + name = self.dataset_meta['classes'][box.label] nusc_anno = dict( sample_token=sample_token, translation=box.center.tolist(), @@ -628,10 +625,9 @@ def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = N annos.append(nusc_anno) nusc_annos[sample_token] = annos - nusc_submissions = { - "meta": self.modality, - "results": nusc_annos, + 'meta': self.modality, + 'results': nusc_annos, } mmengine.mkdir_or_exist(output_dir) @@ -640,6 +636,7 @@ def _format_gt_to_nusc(self, output_dir: str, pipeline: Optional[List[Dict]] = N mmengine.dump(nusc_submissions, res_path) return res_path + def output_to_nusc_box( detection: dict) -> Tuple[List[NuScenesBox], Union[np.ndarray, None]]: """Convert the output to the box class in the nuScenes. @@ -710,7 +707,7 @@ def output_to_nusc_box( 'to standard NuScenesBoxes.') return box_list, attrs - + def lidar_nusc_box_to_global( info: dict, boxes: List[NuScenesBox], classes: List[str], diff --git a/tools/create_data.py b/tools/create_data.py index 4d423eb703..795f324e90 100644 --- a/tools/create_data.py +++ b/tools/create_data.py @@ -6,9 +6,8 @@ from tools.dataset_converters import kitti_converter as kitti from tools.dataset_converters import lyft_converter as lyft_converter from tools.dataset_converters import nuscenes_converter as nuscenes_converter -from tools.dataset_converters import tier4dataset_converter as tier4_converter - from tools.dataset_converters import semantickitti_converter +from tools.dataset_converters import tier4dataset_converter as tier4_converter from tools.dataset_converters.create_gt_database import ( GTDatabaseCreater, create_groundtruth_database) from tools.dataset_converters.update_infos_to_v2 import update_pkl_infos @@ -88,12 +87,13 @@ def nuscenes_data_prep(root_path, create_groundtruth_database(dataset_name, root_path, info_prefix, f'{info_prefix}_infos_train.pkl') + def tier4_data_prep(root_path, - info_prefix, - version, - dataset_name, - out_dir, - max_sweeps=1): + info_prefix, + version, + dataset_name, + out_dir, + max_sweeps=1): """Prepare data related to Tier4 dataset. Related data consists of '.pkl' files recording basic infos, diff --git a/tools/dataset_converters/create_gt_database.py b/tools/dataset_converters/create_gt_database.py index 5670e4ac6f..2b41c97ef7 100644 --- a/tools/dataset_converters/create_gt_database.py +++ b/tools/dataset_converters/create_gt_database.py @@ -195,8 +195,7 @@ def create_groundtruth_database(dataset_class_name, elif dataset_class_name == 'T4Dataset': dataset_cfg.update( use_valid_flag=True, - data_prefix=dict( - pts='', img='', sweeps=''), + data_prefix=dict(pts='', img='', sweeps=''), pipeline=[ dict( type='LoadPointsFromFile', diff --git a/tools/dataset_converters/tier4dataset_converter.py b/tools/dataset_converters/tier4dataset_converter.py index 88595cbb1d..d2b4ce4480 100644 --- a/tools/dataset_converters/tier4dataset_converter.py +++ b/tools/dataset_converters/tier4dataset_converter.py @@ -1,6 +1,5 @@ # Copyright (c) OpenMMLab. All rights reserved. import os -import yaml from collections import OrderedDict from os import path as osp from typing import List, Tuple, Union @@ -8,6 +7,7 @@ import mmcv import mmengine import numpy as np +import yaml from nuscenes.nuscenes import NuScenes from nuscenes.utils.geometry_utils import view_points from pyquaternion import Quaternion @@ -50,8 +50,8 @@ def process_scene(nusc, train_scenes, val_scenes, is_test=False): nusc, train_scenes, val_scenes, True, max_sweeps=max_sweeps) return train_nusc_infos - dataset_config = root_path + version + ".yaml" - with open(dataset_config, "r") as f: + dataset_config = root_path + version + '.yaml' + with open(dataset_config, 'r') as f: dataset_config_dict = yaml.safe_load(f) print(dataset_config_dict) @@ -59,31 +59,36 @@ def process_scene(nusc, train_scenes, val_scenes, is_test=False): all_val_nusc_infos = [] all_test_nusc_infos = [] - if dataset_config_dict["train"] is not None: - for scene in dataset_config_dict["train"]: - scene_path = root_path + "/" + scene - nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + if dataset_config_dict['train'] is not None: + for scene in dataset_config_dict['train']: + scene_path = root_path + '/' + scene + nusc = NuScenes( + version='annotation', dataroot=scene_path, verbose=True) available_scenes = get_available_scenes(nusc) available_scene_tokens = [s['token'] for s in available_scenes] - train_nusc_infos, _ = process_scene(nusc, available_scene_tokens, []) + train_nusc_infos, _ = process_scene(nusc, available_scene_tokens, + []) all_train_nusc_infos += train_nusc_infos - if dataset_config_dict["val"] is not None: - for scene in dataset_config_dict["val"]: - scene_path = root_path + "/" + scene - nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + if dataset_config_dict['val'] is not None: + for scene in dataset_config_dict['val']: + scene_path = root_path + '/' + scene + nusc = NuScenes( + version='annotation', dataroot=scene_path, verbose=True) available_scenes = get_available_scenes(nusc) available_scene_tokens = [s['token'] for s in available_scenes] - _, val_nusc_infos = process_scene(nusc,[], available_scene_tokens) + _, val_nusc_infos = process_scene(nusc, [], available_scene_tokens) all_val_nusc_infos += val_nusc_infos - if dataset_config_dict["test"] is not None: - for scene in dataset_config_dict["test"]: - scene_path = root_path + "/" + scene - nusc = NuScenes(version="annotation", dataroot=scene_path, verbose=True) + if dataset_config_dict['test'] is not None: + for scene in dataset_config_dict['test']: + scene_path = root_path + '/' + scene + nusc = NuScenes( + version='annotation', dataroot=scene_path, verbose=True) available_scenes = get_available_scenes(nusc) available_scene_tokens = [s['token'] for s in available_scenes] - train_nusc_infos = process_scene(nusc,available_scene_tokens, [], True) + train_nusc_infos = process_scene(nusc, available_scene_tokens, [], + True) all_test_nusc_infos += train_nusc_infos metadata = dict(version=version) @@ -173,7 +178,7 @@ def _fill_trainval_infos(nusc, sd_rec = nusc.get('sample_data', sample['data']['LIDAR_CONCAT']) cs_record = nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token']) - + pose_record = nusc.get('ego_pose', sd_rec['ego_pose_token']) lidar_path, boxes, _ = nusc.get_sample_data(lidar_token) @@ -340,9 +345,9 @@ def obtain_sensor2top(nusc, l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix R = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ ( - np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) + np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ ( - np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) + np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T ) + l2e_t @ np.linalg.inv(l2e_r_mat).T sweep['sensor2lidar_rotation'] = R.T # points @ R.T + T @@ -548,7 +553,7 @@ def get_2d_boxes(nusc, def post_process_coords( - corner_coords: List, imsize: Tuple[int, int] = (1600, 900) + corner_coords: List, imsize: Tuple[int, int] = (1600, 900) ) -> Union[Tuple[float, float, float, float], None]: """Get the intersection of the convex hull of the reprojected bbox corners and the image canvas, return None if no intersection. diff --git a/tools/dataset_converters/update_infos_to_v2.py b/tools/dataset_converters/update_infos_to_v2.py index f7d86276bb..72aef4158f 100644 --- a/tools/dataset_converters/update_infos_to_v2.py +++ b/tools/dataset_converters/update_infos_to_v2.py @@ -10,10 +10,10 @@ import argparse import copy +import os import time from os import path as osp from pathlib import Path -import os import mmengine import numpy as np @@ -388,6 +388,7 @@ def update_nuscenes_infos(pkl_path, out_dir): mmengine.dump(converted_data_info, out_path, 'pkl') + def update_tier4_dataset_infos(pkl_path, out_dir): camera_types = [ 'CAM_FRONT', @@ -405,8 +406,7 @@ def update_tier4_dataset_infos(pkl_path, out_dir): data_list = mmengine.load(pkl_path) METAINFO = { - 'classes': - ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), + 'classes': ('car', 'truck', 'bus', 'bicycle', 'pedestrian'), } print('Start updating:') From 90d15e9c085a56f1ce234931272830b66cd79801 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Tue, 27 Feb 2024 16:02:19 +0300 Subject: [PATCH 6/8] fix: refactor files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../centerpoint/pillar_encoder_autoware.py | 2 -- .../centerpoint_onnx_converter.py | 13 +++++------ .../configs/centerpoint_custom.py | 3 ++- .../datasets/tier4_dataset.py | 4 ---- .../evaluation/__init__.py | 3 ++- .../functional/nuscenes_utils/eval.py | 23 +++++++++++-------- .../functional/nuscenes_utils/utils.py | 2 +- .../metrics/nuscenes_custom_metric.py | 3 --- .../tier4dataset_converter.py | 9 ++++---- 9 files changed, 29 insertions(+), 33 deletions(-) diff --git a/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py index e697585579..aa7e3a4101 100644 --- a/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py +++ b/projects/AutowareCenterPoint/centerpoint/pillar_encoder_autoware.py @@ -1,8 +1,6 @@ from typing import Optional, Tuple import torch -from mmcv.cnn import build_norm_layer -from mmcv.ops import DynamicScatter from torch import Tensor, nn from mmdet3d.models.voxel_encoders.utils import (PFNLayer, diff --git a/projects/AutowareCenterPoint/centerpoint_onnx_converter.py b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py index 215e54d3f6..00a2658465 100644 --- a/projects/AutowareCenterPoint/centerpoint_onnx_converter.py +++ b/projects/AutowareCenterPoint/centerpoint_onnx_converter.py @@ -1,6 +1,6 @@ import argparse import os -from typing import Any, Dict, List, Optional, Tuple +from typing import Dict, List, Optional, Tuple import torch from mmengine import Config @@ -17,8 +17,7 @@ def parse_args(): parser = argparse.ArgumentParser( - description= - 'Create autoware compitable onnx file from torch checkpoint ') + 'Create autoware compatible onnx file from torch checkpoint ') parser.add_argument('--cfg', help='train config file path') parser.add_argument('--ckpt', help='checkpoint weeight') parser.add_argument('--work-dir', help='the dir to save onnx files') @@ -119,9 +118,8 @@ def save_onnx(self) -> None: verbose=False, opset_version=11, ) - print( - f'Saved pts_backbone_neck_head onnx model: {pth_onnx_backbone_neck_head}' - ) + print(f'Saved pts_backbone_neck_head onnx model:' + f' {pth_onnx_backbone_neck_head}') @MODELS.register_module() @@ -280,7 +278,8 @@ def forward(self, x: torch.Tensor) -> Tuple[List[Dict[str, torch.Tensor]]]: x (torch.Tensor): (B, C, H, W) Returns: tuple[list[dict[str, any]]]: - (num_classes x [num_detect x {'reg', 'height', 'dim', 'rot', 'vel', 'heatmap'}]) + (num_classes x [num_detect x + {'reg', 'height', 'dim', 'rot', 'vel', 'heatmap'}]) """ x = self.backbone(x) if self.neck is not None: diff --git a/projects/AutowareCenterPoint/configs/centerpoint_custom.py b/projects/AutowareCenterPoint/configs/centerpoint_custom.py index 74c954fa6a..ed3293ed08 100755 --- a/projects/AutowareCenterPoint/configs/centerpoint_custom.py +++ b/projects/AutowareCenterPoint/configs/centerpoint_custom.py @@ -845,4 +845,5 @@ 0.2, 8, ] -work_dir = './work_dirs/centerpoint_pillar02_second_secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d-CUSTOM' +work_dir = ('./work_dirs/centerpoint_pillar02_second_' + 'secfpn_head-circlenms_8xb4-cyclic-20e_nus-3d-CUSTOM') diff --git a/projects/AutowareCenterPoint/datasets/tier4_dataset.py b/projects/AutowareCenterPoint/datasets/tier4_dataset.py index 3abda317ef..cb1cc4ea17 100644 --- a/projects/AutowareCenterPoint/datasets/tier4_dataset.py +++ b/projects/AutowareCenterPoint/datasets/tier4_dataset.py @@ -1,14 +1,10 @@ import os from os import path as osp -from typing import Callable, List, Union import numpy as np -# from .det3d_dataset import Det3DDataset from mmdet3d.datasets.nuscenes_dataset import NuScenesDataset from mmdet3d.registry import DATASETS -from mmdet3d.structures import LiDARInstance3DBoxes -from mmdet3d.structures.bbox_3d.cam_box3d import CameraInstance3DBoxes @DATASETS.register_module() diff --git a/projects/AutowareCenterPoint/evaluation/__init__.py b/projects/AutowareCenterPoint/evaluation/__init__.py index fbb4dd9c92..182e20a449 100644 --- a/projects/AutowareCenterPoint/evaluation/__init__.py +++ b/projects/AutowareCenterPoint/evaluation/__init__.py @@ -1,3 +1,4 @@ +# flake8: noqa from .functional.nuscenes_utils.eval import (DetectionConfig, nuScenesDetectionEval) from .functional.nuscenes_utils.utils import ( @@ -6,7 +7,7 @@ from .metrics.nuscenes_custom_metric import NuScenesCustomMetric __all__ = [ - 'NuScenesCustomMetric,' + 'NuScenesCustomMetric' 'DetectionConfig' 'nuScenesDetectionEval' 'class_mapping_kitti2nuscenes' diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py index 7aa5b50b78..b6e3dae2a7 100644 --- a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/eval.py @@ -1,10 +1,8 @@ -import json import os -from typing import Dict, List, Optional, Tuple +from typing import Dict, List, Optional import numpy as np from nuscenes.eval.common.data_classes import EvalBox, EvalBoxes -from nuscenes.eval.common.loaders import load_prediction from nuscenes.eval.common.utils import center_distance from nuscenes.eval.detection.data_classes import DetectionBox from nuscenes.eval.detection.evaluate import DetectionEval as _DetectionEval @@ -57,8 +55,8 @@ def __init__( mean_ap_weight: int, ): - # assert set(class_range.keys()) == set(DETECTION_NAMES), "Class count mismatch." - assert dist_th_tp in dist_ths, 'dist_th_tp must be in set of dist_ths.' + assert dist_th_tp in dist_ths, \ + 'dist_th_tp must be in set of dist_ths.' self.class_range = class_range self.dist_fcn = dist_fcn @@ -122,15 +120,19 @@ class nuScenesDetectionEval(_DetectionEval): written to the provided output_dir. nuScenes uses the following detection metrics: - - Mean Average Precision (mAP): Uses center-distance as matching criterion; averaged over distance thresholds. - - True Positive (TP) metrics: Average of translation, velocity, scale, orientation and attribute errors. + - Mean Average Precision (mAP): Uses center-distance as matching criterion; + averaged over distance thresholds. + - True Positive (TP) metrics: Average of translation, velocity, scale, + orientation and attribute errors. - nuScenes Detection Score (NDS): The weighted sum of the above. Here is an overview of the functions in this method: - - init: Loads GT annotations and predictions stored in JSON format and filters the boxes. + - init: Loads GT annotations and predictions stored in + JSON format and filters the boxes. - run: Performs evaluation and dumps the metric data to disk. - render: Renders various plots and dumps to disk. We assume that: - - Every sample_token is given in the results, although there may be not predictions for that sample. + - Every sample_token is given in the results, although there may be + not predictions for that sample. Please see https://www.nuscenes.org/object-detection for more details. """ @@ -149,7 +151,8 @@ def __init__( :param config: A DetectionConfig object. :param result_boxes: result bounding boxes. :param gt_boxes: ground-truth bounding boxes. - :param eval_set: The dataset split to evaluate on, e.g. train, val or test. + :param eval_set: The dataset split to evaluate on, + e.g. train, val or test. :param output_dir: Folder to save plots and results to. :param verbose: Whether to print to stdout. """ diff --git a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py index f9cb1d82e7..f87fe782a3 100644 --- a/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py +++ b/projects/AutowareCenterPoint/evaluation/functional/nuscenes_utils/utils.py @@ -234,7 +234,7 @@ def format_nuscenes_metrics(metrics: Dict, result_dict: Dict[str, Dict[str, float]] = defaultdict(dict) for name in class_names: result_dict[name].update({ - f'mAP': + 'mAP': sum([v for v in metrics['label_aps'][name].values()]) * 100 / len(metrics['label_aps'][name]) }) diff --git a/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py index 06bc0378f1..505c893d5f 100644 --- a/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py +++ b/projects/AutowareCenterPoint/evaluation/metrics/nuscenes_custom_metric.py @@ -3,7 +3,6 @@ from os import path as osp from typing import Dict, List, Optional, Sequence, Tuple, Union -import mmcv import mmengine import numpy as np import pyquaternion @@ -235,8 +234,6 @@ def _evaluate_single( Returns: Dict[str, float]: Dictionary of evaluation details. """ - from nuscenes import NuScenes - from nuscenes.eval.detection.evaluate import NuScenesEval output_dir = osp.join(*osp.split(result_path)[:-1]) diff --git a/tools/dataset_converters/tier4dataset_converter.py b/tools/dataset_converters/tier4dataset_converter.py index d2b4ce4480..714daa96fc 100644 --- a/tools/dataset_converters/tier4dataset_converter.py +++ b/tools/dataset_converters/tier4dataset_converter.py @@ -216,7 +216,7 @@ def _fill_trainval_infos(nusc, for cam in camera_types: - if not cam in sample['data']: + if cam not in sample['data']: continue cam_token = sample['data'][cam] @@ -440,9 +440,10 @@ def get_2d_boxes(nusc, # Get the sample data and the sample corresponding to that sample data. sd_rec = nusc.get('sample_data', sample_data_token) - assert sd_rec[ - 'sensor_modality'] == 'camera', 'Error: get_2d_boxes only works' \ - ' for camera sample_data!' + assert sd_rec['sensor_modality'] == 'camera', ('Error: get_2d_boxes only ' + 'worksfor camera ' + 'sample_data!') + if not sd_rec['is_key_frame']: raise ValueError( 'The 2D re-projections are available only for keyframes.') From d167d59f8e867ae14a363b043ac2f851aa11c61f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Tue, 5 Mar 2024 18:32:59 +0300 Subject: [PATCH 7/8] feat: add test, docker file and readme Signed-off-by: Kaan Colak --- docker/Dockerfile | 16 +--- projects/AutowareCenterPoint/README.md | 11 +++ .../tests/test_pillar_encorder_autoware.py | 41 ++++++++++ .../tests/test_t4_dataset.py | 77 +++++++++++++++++++ 4 files changed, 133 insertions(+), 12 deletions(-) create mode 100644 projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py create mode 100644 projects/AutowareCenterPoint/tests/test_t4_dataset.py diff --git a/docker/Dockerfile b/docker/Dockerfile index 3f1f61ca87..53e615fdb4 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,5 +1,5 @@ -ARG PYTORCH="1.9.0" -ARG CUDA="11.1" +ARG PYTORCH="1.13.1" +ARG CUDA="11.6" ARG CUDNN="8" FROM pytorch/pytorch:${PYTORCH}-cuda${CUDA}-cudnn${CUDNN}-devel @@ -9,14 +9,6 @@ ENV TORCH_CUDA_ARCH_LIST="6.0 6.1 7.0 7.5 8.0 8.6+PTX" \ CMAKE_PREFIX_PATH="$(dirname $(which conda))/../" \ FORCE_CUDA="1" -# Avoid Public GPG key error -# https://github.com/NVIDIA/nvidia-docker/issues/1631 -RUN rm /etc/apt/sources.list.d/cuda.list \ - && rm /etc/apt/sources.list.d/nvidia-ml.list \ - && apt-key del 7fa2af80 \ - && apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu1804/x86_64/3bf863cc.pub \ - && apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu1804/x86_64/7fa2af80.pub - # (Optional, use Mirror to speed up downloads) # RUN sed -i 's/http:\/\/archive.ubuntu.com\/ubuntu\//http:\/\/mirrors.aliyun.com\/ubuntu\//g' /etc/apt/sources.list && \ # pip config set global.index-url https://pypi.tuna.tsinghua.edu.cn/simple @@ -29,11 +21,11 @@ RUN apt-get update \ # Install MMEngine, MMCV and MMDetection RUN pip install openmim && \ - mim install "mmengine" "mmcv>=2.0.0rc4" "mmdet>=3.0.0" + mim install "mmengine" "mmcv>=2.0.0rc4" "mmdet>=3.0.0rc5, <3.3.0" # Install MMDetection3D RUN conda clean --all \ - && git clone https://github.com/open-mmlab/mmdetection3d.git -b dev-1.x /mmdetection3d \ + && git clone https://github.com/autowarefoundation/mmdetection3d.git -b main /mmdetection3d \ && cd /mmdetection3d \ && pip install --no-cache-dir -e . diff --git a/projects/AutowareCenterPoint/README.md b/projects/AutowareCenterPoint/README.md index e69de29bb2..3667b39506 100644 --- a/projects/AutowareCenterPoint/README.md +++ b/projects/AutowareCenterPoint/README.md @@ -0,0 +1,11 @@ +## Introduction + +The **[mmdetection3d](https://github.com/open-mmlab/mmdetection3d)** repository includes an additional voxel encoder +feature for the CenterPoint 3D object detection model, known as voxel center z, +not originally used in the **[main implementation](https://github.com/tianweiy/CenterPoint)**, +Autoware maintains consistency with the input size of the original implementation. Consequently, +to ensure integration with Autoware's lidar centerpoint package, we have forked the original repository and made +the requisite code modifications. + +To train custom CenterPoint models and convert them into ONNX format for deployment in Autoware, please refer to the instructions provided in the README.md file included with +Autoware's **[lidar_centerpoint](https://autowarefoundation.github.io/autoware.universe/main/perception/lidar_centerpoint/)** package. These instructions will provide a step-by-step guide for training the CenterPoint model. diff --git a/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py b/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py new file mode 100644 index 0000000000..0086f37c95 --- /dev/null +++ b/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py @@ -0,0 +1,41 @@ +import pytest +import torch + +from mmdet3d.registry import MODELS +from projects.AutowareCenterPoint.centerpoint.pillar_encoder_autoware import \ + PillarFeatureNetAutoware # noqa: F401 + + +def test_pillar_feature_net_autoware(): + + use_voxel_center_z = False + if not torch.cuda.is_available(): + pytest.skip('test requires GPU and torch+cuda') + pillar_feature_net_autoware_cfg = dict( + type='PillarFeatureNetAutoware', + in_channels=4, + feat_channels=[64], + voxel_size=(0.2, 0.2, 8), + point_cloud_range=(-51.2, -51.2, -5.0, 51.2, 51.2, 3.0), + norm_cfg=dict(type='BN1d', eps=1e-3, momentum=0.01), + use_voxel_center_z=use_voxel_center_z, + with_distance=False, + ) + pillar_feature_net_autoware = MODELS.build(pillar_feature_net_autoware_cfg) + + features = torch.rand([97297, 20, 4]) + num_voxels = torch.randint(1, 100, [97297]) + coors = torch.randint(0, 100, [97297, 4]) + + features = pillar_feature_net_autoware(features, num_voxels, coors) + + if not use_voxel_center_z: + assert pillar_feature_net_autoware.pfn_layers[ + 0].linear.in_features == 9 + else: + assert pillar_feature_net_autoware.pfn_layers[ + 0].linear.in_features == 9 + + assert pillar_feature_net_autoware.pfn_layers[0].linear.out_features == 64 + + assert features.shape == torch.Size([97297, 64]) diff --git a/projects/AutowareCenterPoint/tests/test_t4_dataset.py b/projects/AutowareCenterPoint/tests/test_t4_dataset.py new file mode 100644 index 0000000000..3debd078ee --- /dev/null +++ b/projects/AutowareCenterPoint/tests/test_t4_dataset.py @@ -0,0 +1,77 @@ +import numpy as np +from mmcv.transforms.base import BaseTransform +from mmengine.registry import TRANSFORMS +from mmengine.structures import InstanceData + +from mmdet3d.structures import Det3DDataSample, LiDARInstance3DBoxes +from projects.AutowareCenterPoint.datasets.tier4_dataset import T4Dataset + + +def _generate_t4_dataset_config(): + data_root = 'data/sample_dataset/' + ann_file = 'T4Dataset_infos_train.pkl' + classes = ['car', 'truck', 'bus', 'bicycle', 'pedestrian'] + + if 'Identity' not in TRANSFORMS: + + @TRANSFORMS.register_module() + class Identity(BaseTransform): + + def transform(self, info): + packed_input = dict(data_samples=Det3DDataSample()) + if 'ann_info' in info: + packed_input[ + 'data_samples'].gt_instances_3d = InstanceData() + packed_input[ + 'data_samples'].gt_instances_3d.labels_3d = info[ + 'ann_info']['gt_labels_3d'] + return packed_input + + pipeline = [ + dict(type='Identity'), + ] + modality = dict(use_lidar=True, use_camera=True) + data_prefix = dict( + pts='samples/LIDAR_TOP', + img='samples/CAM_BACK_LEFT', + sweeps='sweeps/LIDAR_TOP') + return data_root, ann_file, classes, data_prefix, pipeline, modality + + +def test_getitem(): + np.random.seed(0) + data_root, ann_file, classes, data_prefix, pipeline, modality = \ + _generate_t4_dataset_config() + + t4_dataset = T4Dataset( + data_root=data_root, + ann_file=ann_file, + data_prefix=data_prefix, + pipeline=pipeline, + metainfo=dict(classes=classes), + modality=modality) + + t4_dataset.prepare_data(0) + input_dict = t4_dataset.get_data_info(0) + # assert the the path should contains data_prefix and data_root + assert data_prefix['pts'] in input_dict['lidar_points']['lidar_path'] + assert data_root in input_dict['lidar_points']['lidar_path'] + + for cam_id, img_info in input_dict['images'].items(): + if 'img_path' in img_info: + assert data_prefix['img'] in img_info['img_path'] + assert data_root in img_info['img_path'] + + ann_info = t4_dataset.parse_ann_info(input_dict) + + # assert the keys in ann_info and the type + assert 'gt_labels_3d' in ann_info + assert ann_info['gt_labels_3d'].dtype == np.int64 + assert len(ann_info['gt_labels_3d']) == 70 + + assert 'gt_bboxes_3d' in ann_info + assert isinstance(ann_info['gt_bboxes_3d'], LiDARInstance3DBoxes) + + assert len(t4_dataset.metainfo['classes']) == 5 + assert input_dict['token'] == '5f73a4f0dd74434260bf72821b24c8d4' + assert input_dict['timestamp'] == 1697190328.324525 From efa2c8a651b4fccf120d73aad21f879b85f7ca1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Tue, 5 Mar 2024 18:46:54 +0300 Subject: [PATCH 8/8] fix: fix flake MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../AutowareCenterPoint/tests/test_pillar_encorder_autoware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py b/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py index 0086f37c95..51ad7779aa 100644 --- a/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py +++ b/projects/AutowareCenterPoint/tests/test_pillar_encorder_autoware.py @@ -3,7 +3,7 @@ from mmdet3d.registry import MODELS from projects.AutowareCenterPoint.centerpoint.pillar_encoder_autoware import \ - PillarFeatureNetAutoware # noqa: F401 + PillarFeatureNetAutoware # noqa: F401 def test_pillar_feature_net_autoware():