diff --git a/pipelines/pipeline_navvis_rig.py b/pipelines/pipeline_navvis_rig.py index 58b6a09..7cfab70 100644 --- a/pipelines/pipeline_navvis_rig.py +++ b/pipelines/pipeline_navvis_rig.py @@ -139,6 +139,7 @@ def run( tiles_format, session, export_as_rig=True, + export_trace=True, downsample_max_edge=downsample_max_edge, copy_pointcloud=True, ) diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py index a198e12..c2b528e 100644 --- a/scantools/run_navvis_to_capture.py +++ b/scantools/run_navvis_to_capture.py @@ -54,11 +54,18 @@ def convert_to_us(time_s): def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optional[str] = None, downsample_max_edge: int = None, upright: bool = True, export_as_rig: bool = False, - copy_pointcloud: bool = False): + export_trace: bool = False, copy_pointcloud: bool = False): if session_id is None: session_id = input_path.name - assert session_id not in capture.sessions + + if export_trace: + if not export_as_rig: + logger.warning( + "Trace export is only valid when 'export_as_rig' is set to True. " + "Automatically setting 'export_as_rig' to True." + ) + export_as_rig = True output_path = capture.data_path(session_id) nv = NavVis(input_path, output_path, tiles_format, upright) @@ -134,6 +141,55 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio pose = get_pose(nv, upright, frame_id, camera_id, tile_id) trajectory[timestamp_us, sensor_id] = pose + if export_trace: + # Add "trace" to the rig with identity pose. + rigs[rig_id, "trace"] = Pose() + + # Add "trace" as a sensor. + sensors['trace'] = create_sensor('trace', name='Mapping path') + + qvec, tvec = nv._imu["orientation"], nv._imu["position"] + camhead_from_imu = Pose(r=qvec, t=tvec) + imu_from_camhead = camhead_from_imu.inverse() + + # Rig is in cam0 frame. + cam0 = nv.get_cameras()["cam0"] + qvec, tvec = cam0["orientation"], cam0["position"] + camhead_from_rig = Pose(r=qvec, t=tvec) + + for trace in nv.get_trace(): + timestamp_us = int(trace["nsecs"]) // 1_000 # convert from ns to us + + # world_from_imu (trace.csv contains the IMU's poses) + qvec = np.array([trace["ori_w"], trace["ori_x"], trace["ori_y"], trace["ori_z"]], dtype=float) + tvec = np.array([trace["x"], trace["y"], trace["z"]], dtype=float) + world_from_imu = Pose(r=qvec, t=tvec) + + # Apply the transformation to the first tile's pose. + # The rig is located in cam_id=0, tile_id=0. + tile0_pose = Pose(r=nv.get_tile_rotation(0), t=np.zeros(3)).inverse() + + trace_pose = world_from_imu * imu_from_camhead * camhead_from_rig * tile0_pose + + if upright: + # Images are rotated by 90 degrees clockwise. + # Rotate coordinates counter-clockwise: sin(-pi/2) = -1, cos(-pi/2) = 0 + R_fix = np.array([ + [0, 1, 0], + [-1, 0, 0], + [0, 0, 1] + ]) + R = trace_pose.R @ R_fix + trace_pose = Pose(r=R, t=trace_pose.t) + # Additionally, cam0 is (physically) mounted upside down on VLX. + if nv.get_device() == 'VLX': + trace_pose = fix_vlx_extrinsics(trace_pose) + + trajectory[timestamp_us, 'trace'] = trace_pose + + # Sort the trajectory by timestamp. + trajectory = Trajectories(dict(sorted(trajectory.items()))) + pointcloud_id = 'point_cloud_final' sensors[pointcloud_id] = create_sensor('lidar', name='final NavVis point cloud') pointclouds = RecordsLidar() @@ -205,6 +261,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio parser.add_argument('--downsample_max_edge', type=int, default=None) add_bool_arg(parser, 'upright', default=True) add_bool_arg(parser, 'export_as_rig', default=False) + add_bool_arg(parser, 'export_trace', default=False) parser.add_argument('--copy_pointcloud', action='store_true') args = parser.parse_args().__dict__ diff --git a/scantools/scanners/navvis/navvis.py b/scantools/scanners/navvis/navvis.py index 6c79e0c..28ad70d 100644 --- a/scantools/scanners/navvis/navvis.py +++ b/scantools/scanners/navvis/navvis.py @@ -27,6 +27,8 @@ def __init__(self, input_path: Path, output_path: Optional[Path] = None, self._input_json_path = None self._camera_file_path = None self._pointcloud_file_path = None + self._trace_path = None + self._imu = None self._output_path = None self._output_image_path = None @@ -34,6 +36,7 @@ def __init__(self, input_path: Path, output_path: Optional[Path] = None, self.__cameras = {} self.__frames = {} + self.__trace = {} # upright fix self.__upright = upright @@ -60,6 +63,10 @@ def __init__(self, input_path: Path, output_path: Optional[Path] = None, # set tiles format self.set_tiles_format(tiles_format) + # mapping path: position, orientation, and magnetic field information in + # frequent intervals + self.load_trace() + def _set_dataset_paths(self, input_path: Path, output_path: Optional[Path], tiles_format: str): # Dataset path self._input_path = Path(input_path).absolute() @@ -76,6 +83,10 @@ def _set_dataset_paths(self, input_path: Path, output_path: Optional[Path], tile if not self._input_json_path.exists(): raise FileNotFoundError(f'Input json path {self._input_json_path}.') + # Mapping Path: file trace.csv contains position, orientation, and + # magnetic field information in frequent intervals + self._trace_path = self._input_path / "artifacts" / "trace.csv" + self._camera_file_path = self._input_path / 'sensor_frame.xml' # Pointcloud file path (default) @@ -163,38 +174,87 @@ def load_cameras(self): camera_models = xml.find_all("cameramodel") for cam in camera_models: # current camera dict - ocam_model = {} + cam_info = {} # cam2world coeff = cam.cam2world.find_all("coeff") - ocam_model['pol'] = [float(d.string) for d in coeff] - ocam_model['length_pol'] = len(coeff) + cam_info['pol'] = [float(d.string) for d in coeff] + cam_info['length_pol'] = len(coeff) # world2cam coeff = cam.world2cam.find_all("coeff") - ocam_model['invpol'] = [float(d.string) for d in coeff] - ocam_model['length_invpol'] = len(coeff) - - ocam_model['xc'] = float(cam.cx.string) - ocam_model['yc'] = float(cam.cy.string) - ocam_model['c'] = float(cam.c.string) - ocam_model['d'] = float(cam.d.string) - ocam_model['e'] = float(cam.e.string) - ocam_model['height'] = int(cam.height.string) - ocam_model['width'] = int(cam.width.string) + cam_info['invpol'] = [float(d.string) for d in coeff] + cam_info['length_invpol'] = len(coeff) + + cam_info['xc'] = float(cam.cx.string) + cam_info['yc'] = float(cam.cy.string) + cam_info['c'] = float(cam.c.string) + cam_info['d'] = float(cam.d.string) + cam_info['e'] = float(cam.e.string) + cam_info['height'] = int(cam.height.string) + cam_info['width'] = int(cam.width.string) if self.__upright: # only switch height and width to update undistortion sizes # rest stays the same since we pre-rotate the target coordinates - ocam_model['height'], ocam_model['width'] = ( - ocam_model['width'], ocam_model['height']) - ocam_model['upright'] = self.__upright - - sensorname = cam.sensorname.string - cameras[sensorname] = ocam_model + cam_info['height'], cam_info['width'] = ( + cam_info['width'], cam_info['height']) + cam_info['upright'] = self.__upright + + # Rig information from sensor_frame.xml. + cam_info['position'] = np.array([ + cam.pose.position.x.string, + cam.pose.position.y.string, + cam.pose.position.z.string], dtype=float) + cam_info['orientation'] = np.array([ + cam.pose.orientation.w.string, + cam.pose.orientation.x.string, + cam.pose.orientation.y.string, + cam.pose.orientation.z.string], dtype=float) + + cameras[cam.sensorname.string] = cam_info # save metadata inside the class self.__cameras = cameras + # IMU information from sensor_frame.xml. + imu = {} + imu['position'] = np.array([ + xml.imu.pose.position.x.string, + xml.imu.pose.position.y.string, + xml.imu.pose.position.z.string], dtype=float) + imu['orientation'] = np.array([ + xml.imu.pose.orientation.w.string, + xml.imu.pose.orientation.x.string, + xml.imu.pose.orientation.y.string, + xml.imu.pose.orientation.z.string], dtype=float) + self._imu = imu + + def load_trace(self): + expected_columns = [ + "nsecs", + "x", + "y", + "z", + "ori_w", + "ori_x", + "ori_y", + "ori_z", + "mag_x", + "mag_y", + "mag_z", + ] + input_filepath = self._input_path / "artifacts" / "trace.csv" + rows = read_csv(input_filepath) + rows = rows[1:] # remove header + + # convert to dict + trace = [] + for row in rows: + row_dict = {column: value for column, value in zip(expected_columns, row)} + trace.append(row_dict) + + self.__trace = trace + def get_input_path(self): return self._input_path @@ -229,6 +289,9 @@ def get_frame_values(self): def get_cameras(self): return self.__cameras + def get_trace(self): + return self.__trace + def get_camera(self, camera_id): cam_id = self._convert_cam_id_to_str(camera_id) return self.__cameras[cam_id] @@ -274,6 +337,12 @@ def __get_raw_pose(self, frame_id, cam_id): return qvec, tvec + def get_camhead(self, frame_id): + data = self.__frames[frame_id]["cam_head"] + qvec = np.array(data["quaternion"]) + tvec = np.array(data["position"]) + return qvec, tvec + # auxiliary function: # fixes a camera-to-world qvec for upright fix def __upright_fix(self, qvec): @@ -294,14 +363,7 @@ def __upright_fix(self, qvec): return qvec - # get pose of a particular frame and camera id - # - # Example: - # frame_id = 1 - # get_pose(frame_id, "cam1") - # get_pose(frame_id, 1) - # - def get_pose(self, frame_id, cam_id, tile_id=0): + def get_tile_rotation(self, tile_id): tiles = self.get_tiles() angles = tiles.angles[tile_id] @@ -322,12 +384,21 @@ def get_pose(self, frame_id, cam_id, tile_id=0): # R_tile = Ry @ Rx @ Rz # even though it looks like a bug it is correct! - # get Rotation from tile angles + return R_tile + + # get pose of a particular frame and camera id + # + # Example: + # frame_id = 1 + # get_pose(frame_id, "cam1") + # get_pose(frame_id, 1) + # + def get_pose(self, frame_id, cam_id, tile_id=0): + # get tile rotation + R_tile = self.get_tile_rotation(tile_id) T_rot_only = transform.create_transform_4x4( R_tile, np.array([0, 0, 0])) - - # inverse of tile rotations T_rot_only_inv = np.linalg.inv(T_rot_only) # extrinsics: [R t]