Load trace.csv, navvis artifact. (#58)

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,
             )

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__
 

scantools/scanners/navvis/navvis.py

@@ -27,13 +27,16 @@ 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
         self._output_LUT_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]