def plot_argoverse_trajectory(ts1: int, ts2: int, dataset_dir: str, log_id: str): """ """ city_SE3_egot1 = get_city_SE3_egovehicle_at_sensor_t(ts1, dataset_dir, log_id) city_SE3_egot2 = get_city_SE3_egovehicle_at_sensor_t(ts2, dataset_dir, log_id) # plot the vehicle movement sensor_folder_wildcard = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle*.json" lidar_timestamps = get_timestamps_from_sensor_folder(sensor_folder_wildcard) colors_arr = np.array( [ [color_obj.rgb] for color_obj in Color("red").range_to(Color("green"), len(lidar_timestamps) // 49 ) ] ).squeeze() plt.close('all') for k,ts in enumerate(lidar_timestamps[::50]): city_SE3_egov_t = get_city_SE3_egovehicle_at_sensor_t(ts, dataset_dir, log_id) t = city_SE3_egov_t.translation plt.scatter(t[0], t[1], 20, marker='.', color=colors_arr[k]) t1 = city_SE3_egot1.translation t2 = city_SE3_egot2.translation print('t1 =', t1) print('t2 =', t2) plt.scatter(t1[0], t1[1], 10, marker='o', color='m') plt.scatter(t2[0], t2[1], 10, marker='o', color='c') posx_1 = city_SE3_egot1.transform_point_cloud(np.array([[3,0,0]])).squeeze() posy_1 = city_SE3_egot1.transform_point_cloud(np.array([[0,3,0]])).squeeze() posx_2 = city_SE3_egot2.transform_point_cloud(np.array([[3,0,0]])).squeeze() posy_2 = city_SE3_egot2.transform_point_cloud(np.array([[0,3,0]])).squeeze() plt.plot([t1[0], posx_1[0]], [t1[1], posx_1[1]], 'b') plt.plot([t1[0], posy_1[0]], [t1[1], posy_1[1]], 'k') plt.plot([t2[0], posx_2[0]], [t2[1], posx_2[1]], 'b') plt.plot([t2[0], posy_2[0]], [t2[1], posy_2[1]], 'k') plt.axis('equal') plt.title('Egovehicle trajectory') plt.xlabel('x city coordinate') plt.ylabel('y city coordinate') plt.show() egot1_SE3_city = city_SE3_egot1.inverse() egot1_SE3_egot2 = egot1_SE3_city.right_multiply_with_se3(city_SE3_egot2) # rotates i1's frame to i2's frame # 1R2 bring points in 2's frame into 1's frame # 1R2 is the relative rotation from 1's frame to 2's frame egot1_R_egot2 = egot1_SE3_egot2.rotation egot1_t_egot2 = egot1_SE3_egot2.translation r = Rotation.from_matrix(egot1_R_egot2) print('egot1_R_egot2 from ground truth: ', r.as_euler('zyx', degrees=True)) print('egot1_t_egot2 = ', np.round(egot1_t_egot2,2))
def plot_argoverse_epilines_from_ground_truth_poses(ts1: int, ts2: int, img1, img2, K): """ """ city_SE3_egot1 = get_city_SE3_egovehicle_at_sensor_t(ts1, dataset_dir, log_id) city_SE3_egot2 = get_city_SE3_egovehicle_at_sensor_t(ts2, dataset_dir, log_id) camera_T_egovehicle = calib_dict['ring_front_center'].extrinsic camera_T_egovehicle = SE3(rotation=camera_T_egovehicle[:3,:3], translation=camera_T_egovehicle[:3,3]) egovehicle_T_camera = camera_T_egovehicle.inverse() city_SE3_cam1 = city_SE3_egot1.compose(egovehicle_T_camera) city_SE3_cam2 = city_SE3_egot2.compose(egovehicle_T_camera) cam1_SE3_city = city_SE3_cam1.inverse() cam1_SE3_cam2 = cam1_SE3_city.compose(city_SE3_cam2) # rotates i1's frame to i2's frame # 1R2 bring points in 2's frame into 1's frame # 1R2 is the relative rotation from 1's frame to 2's frame cam1_R_cam2 = cam1_SE3_cam2.rotation cam1_t_cam2 = cam1_SE3_cam2.translation r = Rotation.from_matrix(cam1_R_cam2) print('cam1_R_cam2 =', r.as_euler('zyx', degrees=True)) print('Recover t=', cam1_t_cam2, ' up to scale ', cam1_t_cam2 / np.linalg.norm(cam1_t_cam2)) cam2_SE3_cam1 = cam1_SE3_cam2.inverse() cam2_R_cam1 = cam2_SE3_cam1.rotation cam2_t_cam1 = cam2_SE3_cam1.translation # use correct ground truth relationship to generate gt_E # and then generate correspondences using cam2_E_cam1 = compute_essential_matrix(cam2_R_cam1, cam2_t_cam1) cam2_F_cam1 = get_fmat_from_emat(cam2_E_cam1, K1=K, K2=K) img_h, img_w, _ = img1.shape pkl_fpath = f'/Users/johnlambert/Downloads/visual-odometry-tutorial/labeled_correspondences/argoverse_1_E_0.pkl' corr_data = load_pkl_correspondences(pkl_fpath) pts_left = np.hstack([corr_data.X1.reshape(-1,1), corr_data.Y1.reshape(-1,1) ]).astype(np.int32) pts_right = np.hstack([corr_data.X2.reshape(-1,1), corr_data.Y2.reshape(-1,1)]).astype(np.int32) pdb.set_trace() draw_epilines(pts_left, pts_right, img1, img2, cam2_F_cam1) plt.show() draw_epipolar_lines(cam2_F_cam1, img1, img2, pts_left, pts_right) pts_left = cartesian_to_homogeneous(pts_left) pts_right = cartesian_to_homogeneous(pts_right) for (pt1, pt2) in zip(pts_left, pts_right): epi_error = pt2.dot(cam2_F_cam1).dot(pt1) print('Error: ', epi_error)
def convert_bbox_to_city_frame( self, lidar_timestamp_ns: int, dataset_dir: str, log_id: str, bbox_ego_frame: np.ndarray, ) -> Tuple[np.ndarray, Dict[str, np.ndarray]]: """Convert bounding box to city frame. Args: lidar_timestamp_ns (int): Lidar timestamp. dataset_dir (str): representing full path to the log_ids. log_id (str): e.g. '3ced8dba-62d0-3930-8f60-ebeea2feabb8'. bbox_ego_frame (np.ndarray): Numpy array of shape (4,3), representing bounding box in egovehicle frame Returned: bbox_city_fr: Numpy array of shape (4,3), representing bounding box in CITY frame pose_city_to_ego: dictionary, has two fields: 'translation' and 'rotation' describing the SE(3) for p_city = city_to_egovehicle_se3 * p_egovehicle """ city_to_egovehicle_se3 = get_city_SE3_egovehicle_at_sensor_t( lidar_timestamp_ns, dataset_dir, log_id) if city_to_egovehicle_se3 is None: raise RuntimeError( f"Could not get city to egovehicle coordinate transformation at timestamp {lidar_timestamp_ns}" ) bbox_city_fr = city_to_egovehicle_se3.transform_point_cloud( bbox_ego_frame) pose_city_to_ego = { "rotation": city_to_egovehicle_se3.rotation, "translation": city_to_egovehicle_se3.translation, } return bbox_city_fr, pose_city_to_ego
def get_city_SE3_egovehicle(self, log_id: str, timestamp: int) -> Optional[SE3]: """ Args: log_id: str, unique ID of vehicle log timestamp: int, timestamp of sensor observation, in nanoseconds Returns: city_SE3_egovehicle: SE3 transformation to bring points in egovehicle frame into city frame. """ return get_city_SE3_egovehicle_at_sensor_t(timestamp, self.data_dir, log_id)
def get_pose(self, idx: int, log_id: Optional[str] = None) -> Optional[SE3]: """Get pose corresponding to an index in a particular log_id. Args: idx: Lidar frame index log_id: ID of log to search (default: current log) Returns: Pose for a particular index """ if log_id is None: log_id = self.current_log self._ensure_lidar_timestamp_list_populated() assert self._lidar_timestamp_list is not None timestamp = self._lidar_timestamp_list[log_id][idx] return get_city_SE3_egovehicle_at_sensor_t(timestamp, self.root_dir, log_id)
def group_and_SE3(cloud_dict, dataset=DATASET_DIR, log_id=LOG_ID): timestamps = [] for key in sorted(cloud_dict.keys()): timestamps.append(key) if len(timestamps) == 5: t0_to_map_SE3 = get_city_SE3_egovehicle_at_sensor_t( str(timestamps[0]), dataset, log_id) map_to_t0_SE3 = t0_to_map_SE3.inverse() rotation = np.eye(3) translation = np.array([-72, -40, 0]) t0_to_occ_SE3 = se3.SE3(rotation, translation) for ts in timestamps: pc = cloud_dict[ts] pc = map_to_t0_SE3.transform_point_cloud(pc) pc = t0_to_occ_SE3.transform_point_cloud(pc) cloud_dict[ts] = pc timestamps = [] for ts in timestamps: del cloud_dict[ts] return cloud_dict
def test_filter_objs_to_roi(): """ Use the map to filter out an object that lies outside the ROI in a parking lot """ avm = ArgoverseMap() # should be outside of ROI outside_obj = { "center": {"x": -14.102872067388489, "y": 19.466695178746022, "z": 0.11740010190455852}, "rotation": {"x": 0.0, "y": 0.0, "z": -0.038991328555453404, "w": 0.9992395490058831}, "length": 4.56126567460171, "width": 1.9370055686754908, "height": 1.5820081349372281, "track_label_uuid": "03a321bf955a4d7781682913884abf06", "timestamp": 315970611820366000, "label_class": "VEHICLE", } # should be inside the ROI inside_obj = { "center": {"x": -20.727430239506702, "y": 3.4488006757501353, "z": 0.4036619561689685}, "rotation": {"x": 0.0, "y": 0.0, "z": 0.0013102003738908123, "w": 0.9999991416871218}, "length": 4.507580779458834, "width": 1.9243189627993598, "height": 1.629934978730058, "track_label_uuid": "bb0f40e4f68043e285d64a839f2f092c", "timestamp": 315970611820366000, "label_class": "VEHICLE", } log_city_name = "PIT" lidar_ts = 315970611820366000 dataset_dir = TEST_DATA_LOC / "roi_based_test" log_id = "21e37598-52d4-345c-8ef9-03ae19615d3d" city_SE3_egovehicle = get_city_SE3_egovehicle_at_sensor_t(lidar_ts, dataset_dir, log_id) dts = np.array([json_label_dict_to_obj_record(item) for item in [outside_obj, inside_obj]]) dts_filtered = filter_objs_to_roi(dts, avm, city_SE3_egovehicle, log_city_name) assert dts_filtered.size == 1 assert dts_filtered.dtype == "O" # array of objects assert isinstance(dts_filtered, np.ndarray) assert dts_filtered[0].track_id == "bb0f40e4f68043e285d64a839f2f092c"
def project_lidar_to_img_motion_compensated( pts_h_lidar_time: np.ndarray, calib_data: Dict[str, Any], camera_name: str, cam_timestamp: int, lidar_timestamp: int, dataset_dir: str, log_id: str, return_K: bool = False, ) -> Union[_ReturnWithOptConfig, _ReturnWithoutOptConfig]: """ Because of the high frame rate, motion compensation's role between the sensors is not very significant, moving points only by millimeters to centimeters. If the vehicle is moving at 25 miles per hour, equivalent to 11 meters/sec, then in 17 milliseconds (the max time between a lidar sweep and camera image capture) we should be able to move up to 187 millimeters. This can be verified in practice as the mean_change: mean_change = np.amax(pts_h_cam_time.T[:,:3] - pts_h_lidar_time ,axis=0) Adjust LiDAR points for ego-vehicle motion. This function accepts the egovehicle's pose in the city map both at camera time and also at the LiDAR time. We perform the following transformation, where "ego" stands for egovehicle reference frame pt_ego_cam_t = ego_cam_t_SE3_map * map_SE3_ego_lidar_t * pt_ego_lidar_t Note that both "cam_time_pts_h" and "lidar_time_pts_h" are 3D points in the vehicle coordinate frame, but captured at different times. These LiDAR points always live in the vehicle frame, but just in different timestamps. If we take a lidar point in the egovehicle frame, captured at lidar time, and bring it into the map at this lidar timestamp, then we know the transformation from map to egovehicle reference frame at the time when the camera image was captured. Thus, we move from egovehicle @ lidar time, to the map (which is time agnostic), then we move from map to egovehicle @camera time. Now we suddenly have lidar points living in the egovehicle frame @ camera time. Args: pts_h_lidar_time: Numpy array of shape (4,N) calib_data: Python dictionary camera_name: string, representing name of camera cam_timestamp: integer, representing time in nanoseconds when camera image was recorded lidar_timestamp: integer, representing time in nanoseconds when LiDAR sweep was recorded dataset_dir: string, representing path to where dataset is stored log_id: string, representing unique ID of vehicle log return_K: return a copy of the Returns: uv: Numpy array of shape (N,2) with dtype np.float32 uv_cam: Numpy array of shape (N,3) with dtype np.float32 valid_pts_bool: Numpy array of shape (N,) with dtype bool """ # get transformation to bring point in egovehicle frame to city frame, # at the time when camera image was recorded. city_SE3_ego_cam_t = get_city_SE3_egovehicle_at_sensor_t( cam_timestamp, dataset_dir, log_id) # get transformation to bring point in egovehicle frame to city frame, # at the time when the LiDAR sweep was recorded. city_SE3_ego_lidar_t = get_city_SE3_egovehicle_at_sensor_t( lidar_timestamp, dataset_dir, log_id) if city_SE3_ego_cam_t is None or city_SE3_ego_lidar_t is None: if return_K: return None, None, None, None else: return None, None, None # convert back from homogeneous pts_h_lidar_time = pts_h_lidar_time.T[:, :3] ego_cam_t_SE3_ego_lidar_t = city_SE3_ego_cam_t.inverse( ).right_multiply_with_se3(city_SE3_ego_lidar_t) pts_h_cam_time = ego_cam_t_SE3_ego_lidar_t.transform_point_cloud( pts_h_lidar_time) pts_h_cam_time = point_cloud_to_homogeneous(pts_h_cam_time).T return project_lidar_to_img(pts_h_cam_time, calib_data, camera_name, return_K)
def accumulate( dt_root_fpath: Path, gt_fpath: Path, cfg: DetectionCfg, avm: Optional[ArgoverseMap] ) -> Tuple[DefaultDict[str, np.ndarray], DefaultDict[str, int]]: """Accumulate the true/false positives (boolean flags) and true positive errors for each class. Args: dt_root_fpath: Detections root folder file path. gt_fpath: Ground truth file path. cfg: Detection configuration. Returns: cls_to_accum: Class to accumulated statistics dictionary of shape |C| -> (N, K + S) where C is the number of detection classes, K is the number of true positive thresholds used for AP computation, and S is the number of true positive errors. cls_to_ninst: Mapping of shape |C| -> (1,) the class names to the number of instances in the ground truth dataset. """ log_id = gt_fpath.parents[1].stem logger.info(f"log_id = {log_id}") ts = int(gt_fpath.stem.split("_")[-1]) dt_fpath = dt_root_fpath / f"{log_id}/per_sweep_annotations_amodal/" f"tracked_object_labels_{ts}.json" dts = np.array(read_label(str(dt_fpath))) gts = np.array(read_label(str(gt_fpath))) if cfg.eval_only_roi_instances and avm is not None: # go up 3 levels, because hierarchy is as follows: # {gt_root_fpath}/{log_id}/per_sweep_annotations_amodal/{gt_root_fname} gt_root_fpath = Path(gt_fpath).parents[2] city_SE3_egovehicle = get_city_SE3_egovehicle_at_sensor_t( ts, str(gt_root_fpath), log_id) if city_SE3_egovehicle is not None: log_city_name = read_city_name( os.path.join(gt_root_fpath, log_id, "city_info.json")) dts = filter_objs_to_roi(dts, avm, city_SE3_egovehicle, log_city_name) gts = filter_objs_to_roi(gts, avm, city_SE3_egovehicle, log_city_name) cls_to_accum = defaultdict(list) cls_to_ninst = defaultdict(int) for class_name in cfg.dt_classes: dt_filtered = filter_instances( dts, class_name, filter_metric=cfg.dt_metric, max_detection_range=cfg.max_dt_range, ) gt_filtered = filter_instances( gts, class_name, filter_metric=cfg.dt_metric, max_detection_range=cfg.max_dt_range, ) gt_filtered = remove_duplicate_instances(gt_filtered, cfg) logger.info(f"{dt_filtered.shape[0]} detections") logger.info(f"{gt_filtered.shape[0]} ground truth") if dt_filtered.shape[0] > 0: ranked_detections, scores = rank(dt_filtered) metrics = assign(ranked_detections, gt_filtered, cfg) cls_to_accum[class_name] = np.hstack((metrics, scores)) cls_to_ninst[class_name] = gt_filtered.shape[0] return cls_to_accum, cls_to_ninst
def perform_SE3(cloud_dict, dataset=DATASET_DIR, log_id=LOG_ID): for k, v in cloud_dict.items(): SE3 = get_city_SE3_egovehicle_at_sensor_t(str(k), dataset, log_id) cloud_dict[k] = SE3.transform_point_cloud(v) return cloud_dict