import json
import pickle
import time
from copy import deepcopy
from pathlib import Path

import numpy as np
import torch
import tqdm
from PIL import Image
from nuscenes.eval.common.config import config_factory
from nuscenes.eval.detection.data_classes import DetectionBox
from nuscenes.eval.tracking.data_classes import TrackingBox
from nuscenes.eval.detection.evaluate import DetectionEval
from nuscenes.eval.tracking.evaluate import TrackingEval
from nuscenes.nuscenes import NuScenes
from nuscenes.utils import splits, geometry_utils
from nuscenes.utils.data_classes import LidarPointCloud, Box
from pyquaternion.quaternion import Quaternion

from .nuscenes_utils import map_name_from_general_to_detection
from ..dataset import DatasetTemplate


class NuScenesIDTrackingDataset(DatasetTemplate):
    def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None):
        """
        Args:
            root_path:
            dataset_cfg:
            class_names:
            training:
            logger:
        """
        super().__init__(
            dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger
        )
        self.camera_list = dataset_cfg.CAM_LIST
        pk_path = Path(dataset_cfg.INFO_PATH)
        pk_path.mkdir(parents=True, exist_ok=True)
        pk_file = pk_path / f'{dataset_cfg.VERSION}_{"train" if training else "val"}_{"_".join(self.class_names)}.pkl'
        if pk_file.is_file():
            with open(pk_file, 'rb') as f:
                self.sample_info_dict = pickle.load(f)
                self.id_dict = self.sample_info_dict.pop('id_dict')
                self.sample_token_list = self.sample_info_dict.pop('token_list')
        else:
            start_time = time.time()
            self.create_sample_info_dict()
            print('time cost:', time.time() - start_time)
            self.sample_info_dict['id_dict'] = self.id_dict
            self.sample_info_dict['token_list'] = self.sample_token_list
            with open(pk_file, 'wb') as f:
                pickle.dump(self.sample_info_dict, f)
            self.sample_info_dict.pop('id_dict')
            self.sample_info_dict.pop('token_list')

    def create_sample_info_dict(self):
        nusc = NuScenes(version=self.dataset_cfg.VERSION, dataroot=str(self.root_path), verbose=True)
        self.sample_token_list = []
        self.sample_info_dict = {}
        self.id_dict = {}
        split_scene_names = splits.train if self.training else splits.val
        instances = set()
        print(f'Create {"train" if self.training else "val"} info dict')
        for scene in tqdm.tqdm(nusc.scene):
            if scene['name'] in split_scene_names:
                cur_sample_token = scene['first_sample_token']
                while True:
                    self.sample_token_list.append((scene['name'], cur_sample_token))

                    cur_sample_info_dict = {}
                    sample = nusc.get('sample', cur_sample_token)
                    lidar_token = sample['data']['LIDAR_TOP']
                    lidar = nusc.get('sample_data', lidar_token)
                    cur_sample_info_dict['lidar'] = lidar['filename']

                    calib_dict = {}
                    cs_record = nusc.get('calibrated_sensor', lidar['calibrated_sensor_token'])
                    calib_dict['LIDAR_TOP'] = (cs_record['translation'], cs_record['rotation'])

                    ego_dict = {}
                    ego_record = nusc.get('ego_pose', lidar['ego_pose_token'])
                    ego_dict['LIDAR_TOP'] = (ego_record['translation'], ego_record['rotation'])

                    camera_dict = {}
                    intrinsic_dict = {}
                    imsize_dict = {}
                    for camera_name in self.camera_list:
                        camera_token = sample['data'][camera_name]
                        sd_record = nusc.get('sample_data', camera_token)
                        camera_dict[camera_name] = sd_record['filename']

                        cs_record = nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
                        calib_dict[camera_name] = (cs_record['translation'], cs_record['rotation'])

                        ego_record = nusc.get('ego_pose', sd_record['ego_pose_token'])
                        ego_dict[camera_name] = (ego_record['translation'], ego_record['rotation'])

                        intrinsic_dict[camera_name] = np.array(cs_record['camera_intrinsic'])
                        imsize_dict[camera_name] = (sd_record['width'], sd_record['height'])

                    cur_sample_info_dict['camera'] = camera_dict
                    cur_sample_info_dict['calib'] = calib_dict
                    cur_sample_info_dict['ego'] = ego_dict
                    cur_sample_info_dict['intrinsic'] = intrinsic_dict

                    box_list = {camera_name: [] for camera_name in self.camera_list}
                    id_list = {camera_name: [] for camera_name in self.camera_list}
                    name_list = {camera_name: [] for camera_name in self.camera_list}
                    anno_tokens = sample['anns']
                    for anno_token in anno_tokens:
                        anno = nusc.get('sample_annotation', anno_token)
                        if map_name_from_general_to_detection[anno['category_name']] in self.class_names \
                                and anno['num_lidar_pts'] > 0:
                            box = Box(anno['translation'], anno['size'], Quaternion(anno['rotation']))
                            box_name = map_name_from_general_to_detection[anno['category_name']]
                            tracking_id = anno['instance_token']
                            instances.add(tracking_id)

                            lidar_box = deepcopy(box)
                            # Move box to ego vehicle coord system.
                            translation, rotation = ego_dict['LIDAR_TOP']
                            lidar_box.translate(-np.array(translation))
                            lidar_box.rotate(Quaternion(rotation).inverse)
                            # Move box to lidar coord system.
                            translation, rotation = calib_dict['LIDAR_TOP']
                            lidar_box.translate(-np.array(translation))
                            lidar_box.rotate(Quaternion(rotation).inverse)

                            xyz = lidar_box.center
                            dxdydz = (lidar_box.wlh[1], lidar_box.wlh[0], lidar_box.wlh[2])  # lwh
                            v = np.dot(lidar_box.orientation.rotation_matrix, np.array([1, 0, 0]))
                            heading = np.arctan2(v[1], v[0])

                            for camera_name in self.camera_list:
                                cam_box = deepcopy(box)
                                # Move box from global to ego vehicle coord system.
                                translation, rotation = ego_dict[camera_name]
                                cam_box.translate(-np.array(translation))
                                cam_box.rotate(Quaternion(rotation).inverse)
                                # Move box to camera coord system.
                                translation, rotation = calib_dict[camera_name]
                                cam_box.translate(-np.array(translation))
                                cam_box.rotate(Quaternion(rotation).inverse)

                                if geometry_utils.box_in_image(
                                        cam_box, intrinsic_dict[camera_name], imsize_dict[camera_name]):
                                    box_list[camera_name].append(np.array((*xyz, *dxdydz, heading)))
                                    name_list[camera_name].append(box_name)
                                    id_list[camera_name].append(tracking_id)

                    cur_sample_info_dict['box'] = box_list
                    cur_sample_info_dict['name'] = name_list
                    cur_sample_info_dict['id'] = id_list

                    self.sample_info_dict[cur_sample_token] = cur_sample_info_dict

                    if sample['next'] != '':
                        cur_sample_token = sample['next']
                    else:
                        break
        for i, instance in enumerate(instances):
            self.id_dict[instance] = i

    def get_points_and_image_with_projection(self, cur_sample_info_dict, camera_name):
        pc = LidarPointCloud.from_file(str(self.root_path / cur_sample_info_dict['lidar']))
        im = Image.open(self.root_path / cur_sample_info_dict['camera'][camera_name])
        points = np.copy(pc.points[:4].T).astype(np.float32)

        # Points live in the point sensor frame. So they need to be transformed via global to the image plane.
        # First step: transform the point cloud to the ego vehicle frame for the timestamp of the sweep.
        translation, rotation = cur_sample_info_dict['calib']['LIDAR_TOP']
        pc.rotate(Quaternion(rotation).rotation_matrix)
        pc.translate(np.array(translation))

        # Second step: transform from ego to the global frame.
        translation, rotation = cur_sample_info_dict['ego']['LIDAR_TOP']
        pc.rotate(Quaternion(rotation).rotation_matrix)
        pc.translate(np.array(translation))

        # Third step: transform from global into the ego vehicle frame for the timestamp of the image.
        translation, rotation = cur_sample_info_dict['ego'][camera_name]
        pc.translate(-np.array(translation))
        pc.rotate(Quaternion(rotation).rotation_matrix.T)

        # Fourth step: transform from ego into the camera.
        translation, rotation = cur_sample_info_dict['calib'][camera_name]
        pc.translate(-np.array(translation))
        pc.rotate(Quaternion(rotation).rotation_matrix.T)

        # Fifth step: actually take a "picture" of the point cloud.
        # Grab the depths (camera frame z axis points away from the camera).
        depths = pc.points[2, :]

        # Take the actual picture (matrix multiplication with camera-matrix + renormalization).
        points2d = geometry_utils.view_points(pc.points[:3, :],
                                              np.array(cur_sample_info_dict['intrinsic'][camera_name]),
                                              normalize=True)

        # Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
        # Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera
        # casing for non-keyframes which are slightly out of sync.
        mask = np.ones(depths.shape[0], dtype=bool)
        mask = np.logical_and(mask, depths > 1)
        mask = np.logical_and(mask, points2d[0, :] > 1)
        mask = np.logical_and(mask, points2d[0, :] < im.size[0] - 1)
        mask = np.logical_and(mask, points2d[1, :] > 1)
        mask = np.logical_and(mask, points2d[1, :] < im.size[1] - 1)

        points = points[mask]
        points_img_coords = points2d[:2, mask].T
        image = np.asarray(im).astype(np.float32)  # h, w, c
        image /= 255.0
        image -= [0.485, 0.456, 0.406]
        image /= [0.229, 0.224, 0.225]

        return points, points_img_coords, image

    def __len__(self):
        return len(self.sample_token_list) * len(self.camera_list)

    def __getitem__(self, index):
        scene_name, sample_token = self.sample_token_list[index % len(self.sample_token_list)]
        camera = self.camera_list[index // len(self.sample_token_list)]
        cur_sample_info_dict = self.sample_info_dict[sample_token]

        gt_boxes = cur_sample_info_dict['box'][camera]

        if self.training and not gt_boxes:
            return self.__getitem__(np.random.randint(self.__len__()))

        gt_boxes = np.stack(gt_boxes) if gt_boxes else np.empty((0, 7))

        points, points_img_coords, image = self.get_points_and_image_with_projection(cur_sample_info_dict, camera)
        input_dict = {
            'frame_id': sample_token,
            'metadata': scene_name,
            'points': points,
            'images': image,
            'points_img_coords': points_img_coords,
            'gt_boxes': gt_boxes,
            'gt_names': np.array(cur_sample_info_dict['name'][camera]),
            'gt_tracking_ids': np.array([self.id_dict[key] for key in cur_sample_info_dict['id'][camera]])
        }

        data_dict = self.prepare_data(data_dict=input_dict)

        return data_dict

    @staticmethod
    def get_template_prediction(num_samples):
        ret_dict = {
            'name': np.zeros(num_samples, dtype=int), 'velocity': np.zeros([num_samples, 3]),
            'score': np.zeros(num_samples), 'boxes_lidar': np.zeros([num_samples, 7])
        }
        return ret_dict

    def generate_single_sample_dict(self, box_dict, frame_id, tracker=None):
        if box_dict['pred_scores'].shape[0] == 0:
            pred_dict = self.get_template_prediction(0)
            if tracker is not None:
                pred_dict['track_id'] = np.empty(0)
                pred_dict['feature'] = np.empty(0, dtype=np.float32)
        else:
            pred_scores = box_dict['pred_scores'].cpu().numpy()
            pred_boxes = box_dict['pred_boxes'].cpu().numpy()
            pred_labels = box_dict['pred_labels'].cpu().numpy()

            pred_dict = {
                'score': pred_scores, 'boxes_lidar': pred_boxes
            }

            class_names = np.array(self.class_names)
            if tracker is None:
                pred_dict['name'] = class_names[pred_labels - 1]
                pred_dict['velocity'] = np.zeros([len(pred_scores), 3])
            else:
                pred_dict['name'] = pred_labels
                track_dicts = [{key: val[i] for key, val in pred_dict.items()} for i in range(len(pred_scores))]
                pred_features = box_dict['pred_features']
                track_results = tracker.update(pred_boxes, pred_scores, pred_features, track_dicts, int(frame_id))
                pred_dict = self.get_template_prediction(len(track_results))
                pred_dict['track_id'] = np.zeros(len(track_results), dtype=int)
                final_features = []
                for i, track_result in enumerate(track_results):
                    for key, val in track_result['meta_data'].items():
                        pred_dict[key][i] = val
                    pred_dict['track_id'][i] = track_result['id']
                    final_features.append(track_result['feature'])
                    pred_dict['velocity'][i] = track_result['velocity']
                pred_dict['name'] = class_names[pred_dict['name'] - 1]
                if len(track_results) > 0:
                    pred_dict['feature'] = torch.stack(final_features).cpu().numpy()
                else:
                    pred_dict['feature'] = np.empty(0)

        return pred_dict

    def generate_prediction_dicts(self, batch_dict, pred_dicts, output_path=None, tracker=None):
        results = {}
        for index, box_dict in enumerate(pred_dicts):
            frame_id = str(batch_dict['frame_id'][index])
            if tracker is not None:
                seq_id = batch_dict['metadata'][index]
                if seq_id != tracker.seq:
                    tracker.reset(seq_id)
            single_pred_dict = self.generate_single_sample_dict(box_dict, frame_id, tracker)

            translation, rotation = self.sample_info_dict[frame_id]['calib']['LIDAR_TOP']
            calib_rotation = Quaternion(rotation)
            calib_translation = np.array(translation)

            translation, rotation = self.sample_info_dict[frame_id]['ego']['LIDAR_TOP']
            ego_rotation = Quaternion(rotation)
            ego_translation = np.array(translation)

            boxes_global = []
            boxes_lidar = single_pred_dict['boxes_lidar']
            for i in range(len(boxes_lidar)):
                box = Box(
                    center=boxes_lidar[i, :3],
                    size=boxes_lidar[i, (4, 3, 5)],  # lwh -> wlh
                    orientation=Quaternion(axis=(0, 0, 1), angle=boxes_lidar[i, 6]),
                    velocity=single_pred_dict['velocity'][i]
                )
                # First step: transform to the ego vehicle frame.
                box.rotate(calib_rotation)
                box.translate(calib_translation)
                translation_to_ego = box.center.copy()
                # Second step: transform from ego to the global frame.
                box.rotate(ego_rotation)
                box.translate(ego_translation)
                eval_box = DetectionBox(
                    sample_token=frame_id,
                    translation=box.center,
                    size=box.wlh,
                    rotation=box.orientation.q,
                    velocity=box.velocity[:2],
                    ego_translation=translation_to_ego,
                    detection_name=single_pred_dict['name'][i],
                    detection_score=float(single_pred_dict['score'][i])
                ) if tracker is None else TrackingBox(
                    sample_token=frame_id,
                    translation=box.center,
                    size=box.wlh,
                    rotation=box.orientation.q,
                    velocity=box.velocity[:2],
                    ego_translation=translation_to_ego,
                    tracking_id=single_pred_dict['track_id'][i],
                    tracking_name=single_pred_dict['name'][i],
                    tracking_score=float(single_pred_dict['score'][i])
                )
                boxes_global.append(eval_box)

            if tracker is None:
                results[frame_id] = [{
                    'sample_token': box.sample_token,
                    'translation': box.translation.tolist(),
                    'size': box.size.tolist(),
                    'rotation': box.rotation.tolist(),
                    'velocity': box.velocity.tolist(),
                    'detection_name': box.detection_name,
                    'detection_score': box.detection_score,
                    'attribute_name': ''
                } for i, box in enumerate(boxes_global)]
            else:
                results[frame_id] = [{
                    'sample_token': box.sample_token,
                    'translation': box.translation.tolist(),
                    'size': box.size.tolist(),
                    'rotation': box.rotation.tolist(),
                    'velocity': box.velocity.tolist(),
                    'tracking_id': box.tracking_id,
                    'tracking_name': box.tracking_name,
                    'tracking_score': box.tracking_score,
                } for i, box in enumerate(boxes_global)]

        if output_path is not None:
            with open(output_path / 'submission.json', 'w') as f:
                json.dump({
                    'meta': {
                        'use_camera': True,
                        'use_lidar': True,
                        'use_radar': False,
                        'use_map': False,
                        'use_external': False
                    },
                    'results': results
                }, f)

        return results

    def evaluation(self, output_path, **kwargs):
        DetectionEval(
            NuScenes(version=self.dataset_cfg.VERSION, dataroot=str(self.root_path), verbose=True),
            config=config_factory('detection_cvpr_2019'),
            result_path=output_path / 'submission.json',
            eval_set='val',
            output_dir=output_path,
            verbose=True
        ).main(plot_examples=5, render_curves=True)
        return '', {}

    def evaluate_tracking(self, output_path):
        TrackingEval(
            config=config_factory('tracking_nips_2019'),
            result_path=output_path / 'submission.json',
            eval_set='val',
            output_dir=output_path,
            nusc_version=self.dataset_cfg.VERSION,
            nusc_dataroot=str(self.root_path),
            verbose=True
        ).main(render_curves=True)
        return '', {}

import json
import pickle
import time
from copy import deepcopy
from pathlib import Path

import numpy as np
import torch
import tqdm
from PIL import Image
from nuscenes.eval.common.config import config_factory
from nuscenes.eval.detection.data_classes import DetectionBox
from nuscenes.eval.tracking.data_classes import TrackingBox
from nuscenes.eval.detection.evaluate import DetectionEval
from nuscenes.eval.tracking.evaluate import TrackingEval
from nuscenes.nuscenes import NuScenes
from nuscenes.utils import splits, geometry_utils
from nuscenes.utils.data_classes import LidarPointCloud, Box
from pyquaternion.quaternion import Quaternion

from .nuscenes_utils import map_name_from_general_to_detection
from ..dataset import DatasetTemplate


class NuScenesIDTrackingDataset(DatasetTemplate):
    def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None):
        """
        Args:
            root_path:
            dataset_cfg:
            class_names:
            training:
            logger:
        """
        super().__init__(
            dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger
        )
        self.camera_list = dataset_cfg.CAM_LIST
        pk_path = Path(dataset_cfg.INFO_PATH)
        pk_path.mkdir(parents=True, exist_ok=True)
        pk_file = pk_path / f'{dataset_cfg.VERSION}_{"train" if training else "val"}_{"_".join(self.class_names)}.pkl'
        if pk_file.is_file():
            with open(pk_file, 'rb') as f:
                self.sample_info_dict = pickle.load(f)
                self.id_dict = self.sample_info_dict.pop('id_dict')
                self.sample_token_list = self.sample_info_dict.pop('token_list')
        else:
            start_time = time.time()
            self.create_sample_info_dict()
            print('time cost:', time.time() - start_time)
            self.sample_info_dict['id_dict'] = self.id_dict
            self.sample_info_dict['token_list'] = self.sample_token_list
            with open(pk_file, 'wb') as f:
                pickle.dump(self.sample_info_dict, f)
            self.sample_info_dict.pop('id_dict')
            self.sample_info_dict.pop('token_list')

    def create_sample_info_dict(self):
        nusc = NuScenes(version=self.dataset_cfg.VERSION, dataroot=str(self.root_path), verbose=True)
        self.sample_token_list = []
        self.sample_info_dict = {}
        self.id_dict = {}
        split_scene_names = splits.train if self.training else splits.val
        instances = set()
        print(f'Create {"train" if self.training else "val"} info dict')
        for scene in tqdm.tqdm(nusc.scene):
            if scene['name'] in split_scene_names:
                cur_sample_token = scene['first_sample_token']
                while True:
                    self.sample_token_list.append((scene['name'], cur_sample_token))

                    cur_sample_info_dict = {}
                    sample = nusc.get('sample', cur_sample_token)
                    lidar_token = sample['data']['LIDAR_TOP']
                    lidar = nusc.get('sample_data', lidar_token)
                    cur_sample_info_dict['lidar'] = lidar['filename']

                    calib_dict = {}
                    cs_record = nusc.get('calibrated_sensor', lidar['calibrated_sensor_token'])
                    calib_dict['LIDAR_TOP'] = (cs_record['translation'], cs_record['rotation'])

                    ego_dict = {}
                    ego_record = nusc.get('ego_pose', lidar['ego_pose_token'])
                    ego_dict['LIDAR_TOP'] = (ego_record['translation'], ego_record['rotation'])

                    camera_dict = {}
                    intrinsic_dict = {}
                    imsize_dict = {}
                    for camera_name in self.camera_list:
                        camera_token = sample['data'][camera_name]
                        sd_record = nusc.get('sample_data', camera_token)
                        camera_dict[camera_name] = sd_record['filename']

                        cs_record = nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
                        calib_dict[camera_name] = (cs_record['translation'], cs_record['rotation'])

                        ego_record = nusc.get('ego_pose', sd_record['ego_pose_token'])
                        ego_dict[camera_name] = (ego_record['translation'], ego_record['rotation'])

                        intrinsic_dict[camera_name] = np.array(cs_record['camera_intrinsic'])
                        imsize_dict[camera_name] = (sd_record['width'], sd_record['height'])

                    cur_sample_info_dict['camera'] = camera_dict
                    cur_sample_info_dict['calib'] = calib_dict
                    cur_sample_info_dict['ego'] = ego_dict
                    cur_sample_info_dict['intrinsic'] = intrinsic_dict

                    box_list = {camera_name: [] for camera_name in self.camera_list}
                    id_list = {camera_name: [] for camera_name in self.camera_list}
                    name_list = {camera_name: [] for camera_name in self.camera_list}
                    anno_tokens = sample['anns']
                    for anno_token in anno_tokens:
                        anno = nusc.get('sample_annotation', anno_token)
                        if map_name_from_general_to_detection[anno['category_name']] in self.class_names \
                                and anno['num_lidar_pts'] > 0:
                            box = Box(anno['translation'], anno['size'], Quaternion(anno['rotation']))
                            box_name = map_name_from_general_to_detection[anno['category_name']]
                            tracking_id = anno['instance_token']
                            instances.add(tracking_id)

                            lidar_box = deepcopy(box)
                            # Move box to ego vehicle coord system.
                            translation, rotation = ego_dict['LIDAR_TOP']
                            lidar_box.translate(-np.array(translation))
                            lidar_box.rotate(Quaternion(rotation).inverse)
                            # Move box to lidar coord system.
                            translation, rotation = calib_dict['LIDAR_TOP']
                            lidar_box.translate(-np.array(translation))
                            lidar_box.rotate(Quaternion(rotation).inverse)

                            xyz = lidar_box.center
                            dxdydz = (lidar_box.wlh[1], lidar_box.wlh[0], lidar_box.wlh[2])  # lwh
                            v = np.dot(lidar_box.orientation.rotation_matrix, np.array([1, 0, 0]))
                            heading = np.arctan2(v[1], v[0])

                            for camera_name in self.camera_list:
                                cam_box = deepcopy(box)
                                # Move box from global to ego vehicle coord system.
                                translation, rotation = ego_dict[camera_name]
                                cam_box.translate(-np.array(translation))
                                cam_box.rotate(Quaternion(rotation).inverse)
                                # Move box to camera coord system.
                                translation, rotation = calib_dict[camera_name]
                                cam_box.translate(-np.array(translation))
                                cam_box.rotate(Quaternion(rotation).inverse)

                                if geometry_utils.box_in_image(
                                        cam_box, intrinsic_dict[camera_name], imsize_dict[camera_name]):
                                    box_list[camera_name].append(np.array((*xyz, *dxdydz, heading)))
                                    name_list[camera_name].append(box_name)
                                    id_list[camera_name].append(tracking_id)

                    cur_sample_info_dict['box'] = box_list
                    cur_sample_info_dict['name'] = name_list
                    cur_sample_info_dict['id'] = id_list

                    self.sample_info_dict[cur_sample_token] = cur_sample_info_dict

                    if sample['next'] != '':
                        cur_sample_token = sample['next']
                    else:
                        break
        for i, instance in enumerate(instances):
            self.id_dict[instance] = i

    def get_points_and_image_with_projection(self, cur_sample_info_dict, camera_name):
        pc = LidarPointCloud.from_file(str(self.root_path / cur_sample_info_dict['lidar']))
        im = Image.open(self.root_path / cur_sample_info_dict['camera'][camera_name])
        points = np.copy(pc.points[:4].T).astype(np.float32)

        # Points live in the point sensor frame. So they need to be transformed via global to the image plane.
        # First step: transform the point cloud to the ego vehicle frame for the timestamp of the sweep.
        translation, rotation = cur_sample_info_dict['calib']['LIDAR_TOP']
        pc.rotate(Quaternion(rotation).rotation_matrix)
        pc.translate(np.array(translation))

        # Second step: transform from ego to the global frame.
        translation, rotation = cur_sample_info_dict['ego']['LIDAR_TOP']
        pc.rotate(Quaternion(rotation).rotation_matrix)
        pc.translate(np.array(translation))

        # Third step: transform from global into the ego vehicle frame for the timestamp of the image.
        translation, rotation = cur_sample_info_dict['ego'][camera_name]
        pc.translate(-np.array(translation))
        pc.rotate(Quaternion(rotation).rotation_matrix.T)

        # Fourth step: transform from ego into the camera.
        translation, rotation = cur_sample_info_dict['calib'][camera_name]
        pc.translate(-np.array(translation))
        pc.rotate(Quaternion(rotation).rotation_matrix.T)

        # Fifth step: actually take a "picture" of the point cloud.
        # Grab the depths (camera frame z axis points away from the camera).
        depths = pc.points[2, :]

        # Take the actual picture (matrix multiplication with camera-matrix + renormalization).
        points2d = geometry_utils.view_points(pc.points[:3, :],
                                              np.array(cur_sample_info_dict['intrinsic'][camera_name]),
                                              normalize=True)

        # Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
        # Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera
        # casing for non-keyframes which are slightly out of sync.
        mask = np.ones(depths.shape[0], dtype=bool)
        mask = np.logical_and(mask, depths > 1)
        mask = np.logical_and(mask, points2d[0, :] > 1)
        mask = np.logical_and(mask, points2d[0, :] < im.size[0] - 1)
        mask = np.logical_and(mask, points2d[1, :] > 1)
        mask = np.logical_and(mask, points2d[1, :] < im.size[1] - 1)

        points = points[mask]
        points_img_coords = points2d[:2, mask].T
        image = np.asarray(im).astype(np.float32)  # h, w, c
        image /= 255.0
        image -= [0.485, 0.456, 0.406]
        image /= [0.229, 0.224, 0.225]

        return points, points_img_coords, image

    def __len__(self):
        return len(self.sample_token_list) * len(self.camera_list)

    def __getitem__(self, index):
        scene_name, sample_token = self.sample_token_list[index % len(self.sample_token_list)]
        camera = self.camera_list[index // len(self.sample_token_list)]
        cur_sample_info_dict = self.sample_info_dict[sample_token]

        gt_boxes = cur_sample_info_dict['box'][camera]

        if self.training and not gt_boxes:
            return self.__getitem__(np.random.randint(self.__len__()))

        gt_boxes = np.stack(gt_boxes) if gt_boxes else np.empty((0, 7))

        points, points_img_coords, image = self.get_points_and_image_with_projection(cur_sample_info_dict, camera)
        input_dict = {
            'frame_id': sample_token,
            'metadata': scene_name,
            'points': points,
            'images': image,
            'points_img_coords': points_img_coords,
            'gt_boxes': gt_boxes,
            'gt_names': np.array(cur_sample_info_dict['name'][camera]),
            'gt_tracking_ids': np.array([self.id_dict[key] for key in cur_sample_info_dict['id'][camera]])
        }

        data_dict = self.prepare_data(data_dict=input_dict)

        return data_dict

    @staticmethod
    def get_template_prediction(num_samples):
        ret_dict = {
            'name': np.zeros(num_samples, dtype=int), 'velocity': np.zeros([num_samples, 3]),
            'score': np.zeros(num_samples), 'boxes_lidar': np.zeros([num_samples, 7])
        }
        return ret_dict

    def generate_single_sample_dict(self, box_dict, frame_id, tracker=None):
        if box_dict['pred_scores'].shape[0] == 0:
            pred_dict = self.get_template_prediction(0)
            if tracker is not None:
                pred_dict['track_id'] = np.empty(0)
                pred_dict['feature'] = np.empty(0, dtype=np.float32)
        else:
            pred_scores = box_dict['pred_scores'].cpu().numpy()
            pred_boxes = box_dict['pred_boxes'].cpu().numpy()
            pred_labels = box_dict['pred_labels'].cpu().numpy()

            pred_dict = {
                'score': pred_scores, 'boxes_lidar': pred_boxes
            }

            class_names = np.array(self.class_names)
            if tracker is None:
                pred_dict['name'] = class_names[pred_labels - 1]
                pred_dict['velocity'] = np.zeros([len(pred_scores), 3])
            else:
                pred_dict['name'] = pred_labels
                track_dicts = [{key: val[i] for key, val in pred_dict.items()} for i in range(len(pred_scores))]
                pred_features = box_dict['pred_features']
                track_results = tracker.update(pred_boxes, pred_scores, pred_features, track_dicts, int(frame_id))
                pred_dict = self.get_template_prediction(len(track_results))
                pred_dict['track_id'] = np.zeros(len(track_results), dtype=int)
                final_features = []
                for i, track_result in enumerate(track_results):
                    for key, val in track_result['meta_data'].items():
                        pred_dict[key][i] = val
                    pred_dict['track_id'][i] = track_result['id']
                    final_features.append(track_result['feature'])
                    pred_dict['velocity'][i] = track_result['velocity']
                pred_dict['name'] = class_names[pred_dict['name'] - 1]
                if len(track_results) > 0:
                    pred_dict['feature'] = torch.stack(final_features).cpu().numpy()
                else:
                    pred_dict['feature'] = np.empty(0)

        return pred_dict

    def generate_prediction_dicts(self, batch_dict, pred_dicts, output_path=None, tracker=None):
        results = {}
        for index, box_dict in enumerate(pred_dicts):
            frame_id = str(batch_dict['frame_id'][index])
            if tracker is not None:
                seq_id = batch_dict['metadata'][index]
                if seq_id != tracker.seq:
                    tracker.reset(seq_id)
            single_pred_dict = self.generate_single_sample_dict(box_dict, frame_id, tracker)

            translation, rotation = self.sample_info_dict[frame_id]['calib']['LIDAR_TOP']
            calib_rotation = Quaternion(rotation)
            calib_translation = np.array(translation)

            translation, rotation = self.sample_info_dict[frame_id]['ego']['LIDAR_TOP']
            ego_rotation = Quaternion(rotation)
            ego_translation = np.array(translation)

            boxes_global = []
            boxes_lidar = single_pred_dict['boxes_lidar']
            for i in range(len(boxes_lidar)):
                box = Box(
                    center=boxes_lidar[i, :3],
                    size=boxes_lidar[i, (4, 3, 5)],  # lwh -> wlh
                    orientation=Quaternion(axis=(0, 0, 1), angle=boxes_lidar[i, 6]),
                    velocity=single_pred_dict['velocity'][i]
                )
                # First step: transform to the ego vehicle frame.
                box.rotate(calib_rotation)
                box.translate(calib_translation)
                translation_to_ego = box.center.copy()
                # Second step: transform from ego to the global frame.
                box.rotate(ego_rotation)
                box.translate(ego_translation)
                eval_box = DetectionBox(
                    sample_token=frame_id,
                    translation=box.center,
                    size=box.wlh,
                    rotation=box.orientation.q,
                    velocity=box.velocity[:2],
                    ego_translation=translation_to_ego,
                    detection_name=single_pred_dict['name'][i],
                    detection_score=float(single_pred_dict['score'][i])
                ) if tracker is None else TrackingBox(
                    sample_token=frame_id,
                    translation=box.center,
                    size=box.wlh,
                    rotation=box.orientation.q,
                    velocity=box.velocity[:2],
                    ego_translation=translation_to_ego,
                    tracking_id=single_pred_dict['track_id'][i],
                    tracking_name=single_pred_dict['name'][i],
                    tracking_score=float(single_pred_dict['score'][i])
                )
                boxes_global.append(eval_box)

            if tracker is None:
                results[frame_id] = [{
                    'sample_token': box.sample_token,
                    'translation': box.translation.tolist(),
                    'size': box.size.tolist(),
                    'rotation': box.rotation.tolist(),
                    'velocity': box.velocity.tolist(),
                    'detection_name': box.detection_name,
                    'detection_score': box.detection_score,
                    'attribute_name': ''
                } for i, box in enumerate(boxes_global)]
            else:
                results[frame_id] = [{
                    'sample_token': box.sample_token,
                    'translation': box.translation.tolist(),
                    'size': box.size.tolist(),
                    'rotation': box.rotation.tolist(),
                    'velocity': box.velocity.tolist(),
                    'tracking_id': box.tracking_id,
                    'tracking_name': box.tracking_name,
                    'tracking_score': box.tracking_score,
                } for i, box in enumerate(boxes_global)]

        if output_path is not None:
            with open(output_path / 'submission.json', 'w') as f:
                json.dump({
                    'meta': {
                        'use_camera': True,
                        'use_lidar': True,
                        'use_radar': False,
                        'use_map': False,
                        'use_external': False
                    },
                    'results': results
                }, f)

        return results

    def evaluation(self, output_path, **kwargs):
        DetectionEval(
            NuScenes(version=self.dataset_cfg.VERSION, dataroot=str(self.root_path), verbose=True),
            config=config_factory('detection_cvpr_2019'),
            result_path=output_path / 'submission.json',
            eval_set='val',
            output_dir=output_path,
            verbose=True
        ).main(plot_examples=5, render_curves=True)
        return '', {}

    def evaluate_tracking(self, output_path):
        TrackingEval(
            config=config_factory('tracking_nips_2019'),
            result_path=output_path / 'submission.json',
            eval_set='val',
            output_dir=output_path,
            nusc_version=self.dataset_cfg.VERSION,
            nusc_dataroot=str(self.root_path),
            verbose=True
        ).main(render_curves=True)
        return '', {}

Args:             root_path:             dataset_cfg:             class_names:             training:             logger: # Move box to ego vehicle coord system. # Move box to lidar coord system. # lwh # Move box from global to ego vehicle coord system. # Move box to camera coord system. # Points live in the point sensor frame. So they need to be transformed via global to the image plane. # First step: transform the point cloud to the ego vehicle frame for the timestamp of the sweep. # Second step: transform from ego to the global frame. # Third step: transform from global into the ego vehicle frame for the timestamp of the image. # Fourth step: transform from ego into the camera. # Fifth step: actually take a "picture" of the point cloud. # Grab the depths (camera frame z axis points away from the camera). # Take the actual picture (matrix multiplication with camera-matrix + renormalization). # Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons. # Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera # casing for non-keyframes which are slightly out of sync. # h, w, c # lwh -> wlh # First step: transform to the ego vehicle frame. # Second step: transform from ego to the global frame.