def transform_xyz(xyz: np.ndarray, pose1: SE3, pose2: SE3) -> np.ndarray: # transform xyz from pose1 to pose2 # convert to city coordinate city_coord = pose1.transform_point_cloud(xyz) return pose2.inverse_transform_point_cloud(city_coord)
def test_SE3_chaining_transforms() -> None: """Test chaining two transformations to restore 3 points back to their original position.""" theta0 = np.pi / 4 R0 = get_yaw_angle_rotmat(theta0) theta1 = np.pi R1 = get_yaw_angle_rotmat(theta1) t0 = np.zeros(3) t1 = np.zeros(3) fr2_se3_fr1 = SE3(rotation=R0, translation=t0) fr1_se3_fr0 = SE3(rotation=R1, translation=t1) fr2_se3_fr0 = fr2_se3_fr1.right_multiply_with_se3(fr1_se3_fr0) pts = np.array([[1.0, 0.0, 4.0], [1.0, 0.0, 3.0]]) transformed_pts = fr2_se3_fr0.transform_point_cloud(pts.copy()) gt_transformed_pts = np.array([ [-np.sqrt(2) / 2, -np.sqrt(2) / 2, 4.0], [-np.sqrt(2) / 2, -np.sqrt(2) / 2, 3.0], ]) assert np.allclose(transformed_pts, gt_transformed_pts) combined_R = get_yaw_angle_rotmat(theta0 + theta1) assert np.allclose(fr2_se3_fr0.rotation, combined_R) assert np.allclose(fr2_se3_fr0.translation, np.zeros(3))
def plot_lane_centerlines_in_img( lidar_pts: np.ndarray, city_to_egovehicle_se3: SE3, img: np.ndarray, city_name: str, avm: ArgoverseMap, camera_config: CameraConfig, planes: Iterable[Tuple[np.array, np.array, np.array, np.array, np.array]], color: Tuple[int, int, int] = (0, 255, 255), linewidth: Number = 10, ) -> np.ndarray: """ Args: city_to_egovehicle_se3: SE3 transformation representing egovehicle to city transformation img: Array of shape (M,N,3) representing updated image city_name: str, string representing city name, i.e. 'PIT' or 'MIA' avm: instance of ArgoverseMap camera_config: instance of CameraConfig planes: five frustum clipping planes color: RGB-tuple representing color linewidth: Number = 10) -> np.ndarray Returns: img: Array of shape (M,N,3) representing updated image """ R = camera_config.extrinsic[:3, :3] t = camera_config.extrinsic[:3, 3] cam_SE3_egovehicle = SE3(rotation=R, translation=t) query_x, query_y, _ = city_to_egovehicle_se3.translation local_centerlines = avm.find_local_lane_centerlines( query_x, query_y, city_name) for centerline_city_fr in local_centerlines: color = [ intensity + np.random.randint(0, LANE_COLOR_NOISE) - LANE_COLOR_NOISE // 2 for intensity in color ] ground_heights = avm.get_ground_height_at_xy(centerline_city_fr, city_name) valid_idx = np.isnan(ground_heights) centerline_city_fr = centerline_city_fr[~valid_idx] centerline_egovehicle_fr = city_to_egovehicle_se3.inverse( ).transform_point_cloud(centerline_city_fr) centerline_uv_cam = cam_SE3_egovehicle.transform_point_cloud( centerline_egovehicle_fr) # can also clip point cloud to nearest LiDAR point depth centerline_uv_cam = clip_point_cloud_to_visible_region( centerline_uv_cam, lidar_pts) for i in range(centerline_uv_cam.shape[0] - 1): draw_clipped_line_segment(img, centerline_uv_cam[i], centerline_uv_cam[i + 1], camera_config, linewidth, planes, color) return img
def egomotion_unit_test(): """ 1R2 bring points in 2's frame into 1's frame 1R2 is the relative rotation from 1's frame to 2's frame """ city_SE3_egot1 = SE3(rotation=np.eye(3), translation=np.array([3040,0,0])) city_SE3_egot2 = SE3(rotation=np.eye(3), translation=np.array([3050,0,0])) egot1_SE3_city = city_SE3_egot1.inverse() egot1_SE3_egot2 = egot1_SE3_city.right_multiply_with_se3(city_SE3_egot2) assert np.allclose(egot1_SE3_egot2.translation, np.array([10,0,0]))
def form_calibration_json( calib_data: google.protobuf.pyext._message.RepeatedCompositeContainer, ) -> Dict: """ Argoverse expects to receive "egovehicle_T_camera", i.e. from camera -> egovehicle, with rotation parameterized as quaternion. Waymo provides the same SE(3) transformation, but with rotation parmaeterized as 3x3 matrix """ calib_dict = {"camera_data_": []} for camera_calib in calib_data: cam_name = CAMERA_NAMES[camera_calib.name] # They provide "Camera frame to vehicle frame." # https://github.com/waymo-research/waymo-open-dataset/blob/master/waymo_open_dataset/dataset.proto egovehicle_SE3_waymocam = np.array(camera_calib.extrinsic.transform).reshape( 4, 4 ) standardcam_R_waymocam = rotY(-90).dot(rotX(90)) standardcam_SE3_waymocam = SE3( rotation=standardcam_R_waymocam, translation=np.zeros(3) ) egovehicle_SE3_waymocam = SE3( rotation=egovehicle_SE3_waymocam[:3, :3], translation=egovehicle_SE3_waymocam[:3, 3], ) standardcam_SE3_egovehicle = standardcam_SE3_waymocam.compose( egovehicle_SE3_waymocam.inverse() ) egovehicle_SE3_standardcam = standardcam_SE3_egovehicle.inverse() egovehicle_q_camera = rotmat2quat(egovehicle_SE3_standardcam.rotation) x, y, z = egovehicle_SE3_standardcam.translation qw, qx, qy, qz = egovehicle_q_camera f_u, f_v, c_u, c_v, k1, k2, p1, p2, k3 = camera_calib.intrinsic cam_dict = { "key": "image_raw_" + cam_name, "value": { "focal_length_x_px_": f_u, "focal_length_y_px_": f_v, "focal_center_x_px_": c_u, "focal_center_y_px_": c_v, "skew_": 0, "distortion_coefficients_": [0, 0, 0], "vehicle_SE3_camera_": { "rotation": {"coefficients": [qw, qx, qy, qz]}, "translation": [x, y, z], }, }, } calib_dict["camera_data_"] += [cam_dict] return calib_dict
def convert_3dbox_to_8corner(bbox3d_input: np.ndarray) -> np.ndarray: ''' Args: - bbox3d_input: Numpy array of shape (7,) representing tx,ty,tz,yaw,l,w,h. (tx,ty,tz,yaw) tell us how to transform points to get from the object frame to the egovehicle frame. Returns: - corners_3d: (8,3) array in egovehicle frame ''' # compute rotational matrix around yaw axis bbox3d = copy.copy(bbox3d_input) yaw = bbox3d[3] t = bbox3d[:3] # 3d bounding box dimensions l = bbox3d[4] w = bbox3d[5] h = bbox3d[6] # 3d bounding box corners x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2] y_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2] z_corners = [h / 2, h / 2, h / 2, h / 2, -h / 2, -h / 2, -h / 2, -h / 2] # rotate and translate 3d bounding box corners_3d_obj_fr = np.vstack([x_corners, y_corners, z_corners]).T egovehicle_SE3_object = SE3(rotation=rotMatZ_3D(yaw), translation=t) corners_3d_ego_fr = egovehicle_SE3_object.transform_point_cloud( corners_3d_obj_fr) return corners_3d_ego_fr
def get_B_SE2_A(B_SE3_A: SE3): """ Can take city_SE3_egovehicle -> city_SE2_egovehicle Can take egovehicle_SE3_object -> egovehicle_SE2_object Doesn't matter if we stretch square by h,w,l since triangles will be similar regardless Args: - B_SE3_A Returns: - B_SE2_A - B_yaw_A """ x_corners = np.array([1, 1, 1, 1, -1, -1, -1, -1]) y_corners = np.array([1, -1, -1, 1, 1, -1, -1, 1]) z_corners = np.array([1, 1, -1, -1, 1, 1, -1, -1]) corners_A_frame = np.vstack((x_corners, y_corners, z_corners)).T corners_B_frame = B_SE3_A.transform_point_cloud(corners_A_frame) p1 = corners_B_frame[1] p5 = corners_B_frame[5] dy = p1[1] - p5[1] dx = p1[0] - p5[0] # the orientation angle of the car B_yaw_A = np.arctan2(dy, dx) t = B_SE3_A.transform_matrix[:2, 3] # get x,y only B_SE2_A = SE2(rotation=rotmat2d(B_yaw_A), translation=t) return B_SE2_A, B_yaw_A
def filter_objs_to_roi(instances: np.ndarray, avm: ArgoverseMap, city_SE3_egovehicle: SE3, city_name: str) -> np.ndarray: """Filter objects to the region of interest (5 meter dilation of driveable area). We ignore instances outside of region of interest (ROI) during evaluation. Args: instances: Numpy array of shape (N,) with ObjectLabelRecord entries avm: Argoverse map object city_SE3_egovehicle: pose of egovehicle within city map at time of sweep city_name: name of city where log was captured Returns: instances_roi: objects with any of 4 cuboid corners located within ROI """ # for each cuboid, get its 4 corners in the egovehicle frame corners_egoframe = np.vstack([dt.as_2d_bbox() for dt in instances]) corners_cityframe = city_SE3_egovehicle.transform_point_cloud( corners_egoframe) corner_within_roi = avm.get_raster_layer_points_boolean( corners_cityframe, city_name, "roi") # check for each cuboid if any of its 4 corners lies within the ROI is_within_roi = corner_within_roi.reshape(-1, 4).any(axis=1) instances_roi = instances[is_within_roi] return instances_roi
def test_SE3_transform_point_cloud_by_quaternion() -> None: """Test rotating points by a given quaternion, and then adding translation vector to each point.""" pts = np.array([[1.0, 1.0, 1.1], [1.0, 1.0, 2.1], [1.0, 1.0, 3.1], [1.0, 1.0, 4.1]]) # x, y, z of cuboid center t = np.array([-34.7128603513203, 5.29461762417753, 0.10328996181488]) # quaternion has order (w,x,y,z) q = np.array([0.700322174885275, 0.0, 0.0, -0.713826905743933]) R = quat2rotmat(q) dst_se3_src = SE3(rotation=R.copy(), translation=t.copy()) transformed_pts = dst_se3_src.transform_point_cloud(pts.copy()) gt_transformed_pts = pts.dot(R.T) gt_transformed_pts += t print(gt_transformed_pts) assert np.allclose(transformed_pts, gt_transformed_pts) gt_transformed_pts_explicit = np.array([ [-33.73214043, 4.2757023, 1.20328996], [-33.73214043, 4.2757023, 2.20328996], [-33.73214043, 4.2757023, 3.20328996], [-33.73214043, 4.2757023, 4.20328996], ]) assert np.allclose(transformed_pts, gt_transformed_pts_explicit)
def as_3d_bbox(self) -> np.ndarray: r"""Calculate the 8 bounding box corners. Args: None Returns: Numpy array of shape (8,3) Corner numbering:: 5------4 |\\ |\\ | \\ | \\ 6--\\--7 \\ \\ \\ \\ \\ l \\ 1-------0 h e \\ || \\ || e n \\|| \\|| i g \\2------3 g t width. h h. t. First four corners are the ones facing forward. The last four are the ones facing backwards. """ # 3D bounding box corners. (Convention: x points forward, y to the left, z up.) x_corners = self.length / 2 * np.array([1, 1, 1, 1, -1, -1, -1, -1]) y_corners = self.width / 2 * np.array([1, -1, -1, 1, 1, -1, -1, 1]) z_corners = self.height / 2 * np.array([1, 1, -1, -1, 1, 1, -1, -1]) corners_object_frame = np.vstack((x_corners, y_corners, z_corners)).T egovehicle_SE3_object = SE3(rotation=quat2rotmat(self.quaternion), translation=self.translation) corners_egovehicle_frame = egovehicle_SE3_object.transform_point_cloud(corners_object_frame) return corners_egovehicle_frame
def get_z_icp(x_prev, transf): """ Apply transformation matrix from icp to previous pose to get measurement """ pose_new = x_prev[0:3] + transf[0:3, 3] pose_head = np.array([np.cos(x_prev[3]), np.sin(x_prev[3]), 0])[np.newaxis, :] transf_se3 = SE3(rotation=transf[0:3, 0:3], translation=np.zeros((3))) pose_head_new = transf_se3.transform_point_cloud(pose_head) theta_new = np.arctan2(pose_head_new[0, 1], pose_head_new[0, 0]) # clip rotation angle given turning radius dx = pose_new[0] - x_prev[0] dy = pose_new[1] - x_prev[1] delta_theta = theta_new - x_prev[3] theta_th = np.sqrt(dx**2 + dy**2) / CAR_TURNING_RADIUS if delta_theta > 0: delta_theta = min(delta_theta, theta_th) else: delta_theta = max(delta_theta, -theta_th) output = x_prev + np.array([0, 0, 0, delta_theta]) output[0:3] = pose_new[0:3] return output
def visualize_ground_lidar_pts(log_id: str, dataset_dir: str, experiment_prefix: str): """Process a log by drawing the LiDAR returns that are classified as belonging to the ground surface in a red to green colormap in the image. Args: log_id: The ID of a log dataset_dir: Where the dataset is stored experiment_prefix: Output prefix """ sdb = SynchronizationDB(dataset_dir, collect_single_log_id=log_id) city_info_fpath = f"{dataset_dir}/{log_id}/city_info.json" city_info = read_json_file(city_info_fpath) city_name = city_info["city_name"] avm = ArgoverseMap() ply_fpaths = sorted(glob.glob(f"{dataset_dir}/{log_id}/lidar/PC_*.ply")) for i, ply_fpath in enumerate(ply_fpaths): if i % 500 == 0: print(f"\tOn file {i} of {log_id}") lidar_timestamp_ns = ply_fpath.split("/")[-1].split(".")[0].split( "_")[-1] pose_fpath = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle_{lidar_timestamp_ns}.json" if not Path(pose_fpath).exists(): continue pose_data = read_json_file(pose_fpath) rotation = np.array(pose_data["rotation"]) translation = np.array(pose_data["translation"]) city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation), translation=translation) lidar_pts = load_ply(ply_fpath) lidar_timestamp_ns = int(lidar_timestamp_ns) draw_ground_pts_in_image( sdb, lidar_pts, city_to_egovehicle_se3, avm, log_id, lidar_timestamp_ns, city_name, dataset_dir, experiment_prefix, ) for camera_name in CAMERA_LIST: if "stereo" in camera_name: fps = 5 else: fps = 10 cmd = f"ffmpeg -r {fps} -f image2 -i '{experiment_prefix}_ground_viz/{log_id}/{camera_name}/%*.jpg' {experiment_prefix}_ground_viz/{experiment_prefix}_{log_id}_{camera_name}_{fps}fps.mp4" print(cmd) run_command(cmd)
def test_SE3_inverse_transform_point_cloud_identity() -> None: """Test taking a transformed point cloud and undoing the transformation. Since the transformation was the identity, the points should not be affected. """ transformed_pts = np.array([[1.0, 1.0, 1.1], [1.0, 1.0, 2.1], [1.0, 1.0, 3.1], [1.0, 1.0, 4.1]]) dst_se3_src = SE3(rotation=np.eye(3), translation=np.zeros(3)) pts = dst_se3_src.inverse_transform_point_cloud(transformed_pts.copy()) assert np.allclose(pts, transformed_pts)
def test_SE3_transform_point_cloud_identity() -> None: """Test taking a point cloud and performing an SE3 transformation. Since the transformation is the identity, the points should not be affected. """ pts = np.array([[1.0, 1.0, 1.1], [1.0, 1.0, 2.1], [1.0, 1.0, 3.1]]) dst_se3_src = SE3(rotation=np.eye(3), translation=np.zeros(3)) transformed_pts = dst_se3_src.transform_point_cloud(pts.copy()) assert np.allclose(transformed_pts, pts)
def render_bev_labels_mpl_tmp( self, city_name: str, ax: plt.Axes, axis: str, lidar_pts: np.ndarray, local_lane_polygons: np.ndarray, local_das: np.ndarray, city_to_egovehicle_se3: SE3, avm: ArgoverseMap, ) -> None: """Plot nearby lane polygons and nearby driveable areas (da) on the Matplotlib axes. Args: city_name: The name of a city, e.g. `"PIT"` ax: Matplotlib axes axis: string, either 'ego_axis' or 'city_axis' to demonstrate the lidar_pts: Numpy array of shape (N,3) local_lane_polygons: Polygons representing the local lane set local_das: Numpy array of objects of shape (N,) where each object is of shape (M,3) city_to_egovehicle_se3: Transformation from egovehicle frame to city frame avm: ArgoverseMap instance """ if axis is not "city_axis": # rendering instead in the egovehicle reference frame for da_idx, local_da in enumerate(local_das): local_da = city_to_egovehicle_se3.inverse_transform_point_cloud(local_da) local_das[da_idx] = rotate_polygon_about_pt(local_da, city_to_egovehicle_se3.rotation, np.zeros(3)) for lane_idx, local_lane_polygon in enumerate(local_lane_polygons): local_lane_polygon = city_to_egovehicle_se3.inverse_transform_point_cloud(local_lane_polygon) local_lane_polygons[lane_idx] = rotate_polygon_about_pt( local_lane_polygon, city_to_egovehicle_se3.rotation, np.zeros(3) ) draw_lane_polygons(ax, local_lane_polygons) draw_lane_polygons(ax, local_das, color="tab:pink") if axis is not "city_axis": lidar_pts = rotate_polygon_about_pt(lidar_pts, city_to_egovehicle_se3.rotation, np.zeros((3,))) draw_point_cloud_bev(ax, lidar_pts)
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 test_leave_only_roi_region(): """ test leave_only_roi_region function (lidar_pts,egovehicle_to_city_se3,ground_removal_method, city_name='MIA') """ pc = ply_loader.load_ply(TEST_DATA_LOC / "1/lidar/PC_0.ply") pose_data = read_json_file(TEST_DATA_LOC / "1/poses/city_SE3_egovehicle_0.json") rotation = np.array(pose_data["rotation"]) translation = np.array(pose_data["translation"]) ego_R = quat2rotmat(rotation) ego_t = translation egovehicle_to_city_se3 = SE3(rotation=ego_R, translation=ego_t) pts = eval_utils.leave_only_roi_region(pc, egovehicle_to_city_se3, ground_removal_method="map")
def load_camera_calibration(self, log_id: str, camera_name: str) -> None: """Load extrinsics and intrinsics from disk.""" calib_data = self._dl.get_log_calibration_data(log_id) cam_config = get_calibration_config(calib_data, camera_name) self._K = cam_config.intrinsic[:3, :3] # square pixels, so fx and fy should be (nearly) identical assert np.isclose(self._K[0, 0], self._K[1, 1], atol=0.1) self._camera_SE3_egovehicle = SE3( rotation=cam_config.extrinsic[:3, :3], translation=cam_config.extrinsic[:3, 3]) self._egovehicle_SE3_camera = self._camera_SE3_egovehicle.inverse()
def test_SE3_transform_point_cloud_by_yaw_angle() -> None: """Test rotating points by yaw angle of pi/4 in the xy plane, then adding a translation vector to them all.""" pts = np.array([[1.0, 0.0, 4.0], [1.0, 0.0, 3.0]]) theta = np.pi / 4 R = get_yaw_angle_rotmat(theta) t = np.array([1.0, 2.0, 3.0]) dst_SE3_src = SE3(rotation=R.copy(), translation=t.copy()) transformed_pts = dst_SE3_src.transform_point_cloud(pts.copy()) gt_transformed_pts = np.array([[np.sqrt(2) / 2, np.sqrt(2) / 2, 4.0], [np.sqrt(2) / 2, np.sqrt(2) / 2, 3.0]]) gt_transformed_pts += t assert np.allclose(transformed_pts, gt_transformed_pts)
def rotate_orientation(x, rotation): """ convert rotation matrix to orientation angle """ pose_head = np.array([np.cos(x[3]), np.sin(x[3]), 0])[np.newaxis, :] transf_se3 = SE3(rotation=rotation[0:3, 0:3], translation=np.zeros((3))) pose_head_new = transf_se3.transform_point_cloud(pose_head) theta_new = np.arctan2(pose_head_new[0, 1], pose_head_new[0, 0]) return theta_new
def plot_argoverse_epilines_from_annotated_correspondences( img1: np.ndarray, img2: np.ndarray, K: np.ndarray, use_normalized_coords: bool ): """ """ pkl_fpath = f'/Users/johnlambert/Downloads/visual-odometry-tutorial/labeled_correspondences/argoverse_1_E_0.pkl' corr_data = load_pkl_correspondences(pkl_fpath) corr_img = show_correspondence_lines(img1, img2, corr_data.X1, corr_data.Y1, corr_data.X2, corr_data.Y2) plt.imshow(corr_img) plt.show() img1_kpts = np.hstack([ corr_data.X1.reshape(-1,1), corr_data.Y1.reshape(-1,1) ]).astype(np.int32) img2_kpts = np.hstack([ corr_data.X2.reshape(-1,1), corr_data.Y2.reshape(-1,1) ]).astype(np.int32) if use_normalized_coords: img1_norm_kpts = cv2.undistortPoints(img1_kpts.astype(np.float32), K, None) img2_norm_kpts = cv2.undistortPoints(img2_kpts.astype(np.float32), K, None) K_I = np.eye(3) # since normalized coordinates are between 0 and 1, need much smaller threshold! cam2_E_cam1, inlier_mask = cv2.findEssentialMat(img1_norm_kpts, img2_norm_kpts, K_I, method=cv2.RANSAC, threshold=0.001) else: cam2_E_cam1, inlier_mask = cv2.findEssentialMat(img1_kpts, img2_kpts, K, method=cv2.RANSAC, threshold=0.1) print('Num inliers: ', inlier_mask.sum()) cam2_F_cam1 = get_fmat_from_emat(cam2_E_cam1, K1=K, K2=K) _num_inlier, cam2_R_cam1, cam2_t_cam1, _ = cv2.recoverPose(cam2_E_cam1, img1_kpts, img2_kpts, mask=inlier_mask) r = Rotation.from_matrix(cam2_R_cam1) print('cam2_R_cam1 recovered from correspondences', r.as_euler('zyx', degrees=True)) print('cam2_t_cam1: ', np.round(cam2_t_cam1.squeeze(), 2)) cam2_SE3_cam1 = SE3(cam2_R_cam1, cam2_t_cam1.squeeze() ) cam1_SE3_cam2 = cam2_SE3_cam1.inverse() 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)) ## prints "[-0.32 33.11 -0.45]" print('cam1_t_cam2: ', np.round(cam1_t_cam2,2)) ## [0.21 0. 0.98] pdb.set_trace() draw_epilines(img1_kpts, img2_kpts, img1, img2, cam2_F_cam1) plt.show() draw_epipolar_lines(cam2_F_cam1, img1, img2, img1_kpts, img2_kpts) plt.show()
def get_future_traj(self, axes: Axes, log_id: str, timestamp_ind: int, city_to_egovehicle_se3: SE3): traj = np.zeros((FUTURE_TIME_STEP, 2)) for ind, i in enumerate( range(timestamp_ind + 1, timestamp_ind + FUTURE_TIME_STEP + 1)): timestamp = self.timestamps[i] pose_city_to_ego = self.log_egopose_dict[log_id][timestamp] relative_pose = city_to_egovehicle_se3.inverse_transform_point_cloud( pose_city_to_ego["translation"]) traj[ind, :] = relative_pose[0:2] # axes.scatter(traj[:, 0].tolist(), traj[:, 1].tolist(), s=5, c='g') return traj
def test_SE3_transform_point_cloud_optimized() -> None: """Ensure that our transform_point_cloud() implementation is faster than a naive implementation. We use 1k trials, with a point cloud of 100k points. The chosen SE(3) transformation is arbitrary. """ def transform_point_cloud_slow(point_cloud: np.ndarray, transform_matrix: np.ndarray) -> np.ndarray: """Slow, unoptimized method -- allocated unnecessary array for homogeneous coordinates.""" # convert to homogeneous num_pts = point_cloud.shape[0] homogeneous_pts = np.hstack([point_cloud, np.ones((num_pts, 1))]) transformed_point_cloud = homogeneous_pts.dot(transform_matrix.T) return transformed_point_cloud[:, :3] # number of trials n_trials = 1000 num_pts = 100000 pts_src = np.random.randn(num_pts, 3) # x, y, z of cuboid center t = np.array([-34.7128603513203, 5.29461762417753, 0.10328996181488]) # quaternion has order (w,x,y,z) q = np.array([0.700322174885275, 0.0, 0.0, -0.713826905743933]) R = quat2rotmat(q) dst_se3_src = SE3(rotation=R.copy(), translation=t.copy()) def measure_time_elapsed(n_trials: int, fn: Callable, *args: Any) -> float: """Compute time in seconds to execute a function `n_trials` times.""" start = time.time() for i in range(n_trials): pts_dst = fn(*args) end = time.time() duration = end - start return duration optimized_duration = measure_time_elapsed( n_trials, dst_se3_src.transform_point_cloud, pts_src) unoptimized_duration = measure_time_elapsed( n_trials, transform_point_cloud_slow, pts_src, dst_se3_src.transform_matrix, ) assert optimized_duration < unoptimized_duration
def project_lidar_to_undistorted_img( lidar_points_h: np.ndarray, calib_data: Dict[str, Any], camera_name: str, remove_nan: bool = False, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, CameraConfig]: camera_config = get_calibration_config(calib_data, camera_name) R = camera_config.extrinsic[:3, :3] t = camera_config.extrinsic[:3, 3] cam_SE3_egovehicle = SE3(rotation=R, translation=t) points_egovehicle = lidar_points_h.T[:, :3] uv_cam = cam_SE3_egovehicle.transform_point_cloud(points_egovehicle) return proj_cam_to_uv(uv_cam, camera_config)
def test_SE3_constructor() -> None: """Test making an arbitrary SE2 transformation for a pedestrian cuboid.""" # x, y, z of cuboid center t = np.array([-34.7128603513203, 5.29461762417753, 0.10328996181488]) # quaternion has order (w,x,y,z) q = np.array([0.700322174885275, 0.0, 0.0, -0.713826905743933]) R = quat2rotmat(q) dst_se3_src = SE3(rotation=R.copy(), translation=t.copy()) T_mat_gt = np.eye(4) T_mat_gt[:3, :3] = R T_mat_gt[:3, 3] = t assert np.allclose(dst_se3_src.rotation, R) assert np.allclose(dst_se3_src.translation, t) assert np.allclose(dst_se3_src.transform_matrix, T_mat_gt)
def render_ego_past_traj( self, axes: Axes, log_id: str, timestamp_ind: int, city_to_egovehicle_se3: SE3, ) -> None: x = [0] y = [0] for i in range(timestamp_ind - EGO_TIME_STEP, timestamp_ind): timestamp = self.timestamps[i] pose_city_to_ego = self.log_egopose_dict[log_id][timestamp] relative_pose = city_to_egovehicle_se3.inverse_transform_point_cloud( pose_city_to_ego["translation"]) x.append(relative_pose[0]) y.append(relative_pose[1]) axes.scatter(x, y, s=5, c='b', alpha=1, zorder=2)
def get_city_to_egovehicle_se3(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_to_egovehicle_se3: SE3 transformation to bring egovehicle frame point into city frame. """ pose_fpath = f"{self.data_dir}/{log_id}/poses/city_SE3_egovehicle_{timestamp}.json" if not Path(pose_fpath).exists(): return None pose_data = read_json_file(pose_fpath) rotation = np.array(pose_data["rotation"]) translation = np.array(pose_data["translation"]) city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation), translation=translation) return city_to_egovehicle_se3
def test_SE3_inverse_transform_point_cloud() -> None: """Test taking a transformed point cloud and undoing the transformation to recover the original points.""" transformed_pts = np.array([ [-33.73214043, 4.2757023, 1.20328996], [-33.73214043, 4.2757023, 2.20328996], [-33.73214043, 4.2757023, 3.20328996], [-33.73214043, 4.2757023, 4.20328996], ]) # x, y, z of cuboid center t = np.array([-34.7128603513203, 5.29461762417753, 0.10328996181488]) # quaternion has order (w,x,y,z) q = np.array([0.700322174885275, 0.0, 0.0, -0.713826905743933]) R = quat2rotmat(q) dst_se3_src = SE3(rotation=R, translation=t) pts = dst_se3_src.inverse_transform_point_cloud(transformed_pts) gt_pts = np.array([[1.0, 1.0, 1.1], [1.0, 1.0, 2.1], [1.0, 1.0, 3.1], [1.0, 1.0, 4.1]]) assert np.allclose(pts, gt_pts)
def get_city_SE3_egovehicle_at_sensor_t(sensor_timestamp: int, dataset_dir: str, log_id: str) -> Optional[SE3]: """Get transformation from ego-vehicle to city coordinates at a given timestamp. Args: sensor_timestamp: integer representing timestamp when sensor measurement captured, in nanoseconds dataset_dir: log_id: string representing unique log identifier Returns: SE3 for translating ego-vehicle coordinates to city coordinates if found, else None. """ pose_fpath = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle_{sensor_timestamp}.json" if not Path(pose_fpath).exists(): logger.error(f"missing pose {sensor_timestamp}") return None pose_city_to_ego = read_json_file(pose_fpath) rotation = np.array(pose_city_to_ego["rotation"]) translation = np.array(pose_city_to_ego["translation"]) city_to_egovehicle_se3 = SE3(rotation=quat2rotmat(rotation), translation=translation) return city_to_egovehicle_se3
def get_camera_extrinsic_matrix(config: Dict[str, Any]) -> np.ndarray: """Load camera calibration rotation and translation. Note that the camera calibration file contains the SE3 for sensor frame to the vehicle frame, i.e. pt_egovehicle = egovehicle_SE3_sensor * pt_sensor Then build extrinsic matrix from rotation matrix and translation, a member of SE3. Then we return the inverse of the SE3 transformation. Args: config: Calibration config in json, or calibration file path. Returns: Camera rotation and translation matrix. """ vehicle_SE3_sensor = config["vehicle_SE3_camera_"] egovehicle_t_camera = np.array(vehicle_SE3_sensor["translation"]) egovehicle_q_camera = vehicle_SE3_sensor["rotation"]["coefficients"] egovehicle_R_camera = quat2rotmat(egovehicle_q_camera) egovehicle_T_camera = SE3(rotation=egovehicle_R_camera, translation=egovehicle_t_camera) return egovehicle_T_camera.inverse().transform_matrix