Fixed trace for NavVis M6 devices. (#66)

Contrary to the VLX where `cam_head` is the `sensor_frame.xml`
coordinate system, in the M6 is `footprint`. This adapts the trace to M6
to `footprint`, while keeping the VLX in `cam_head`.

scantools/run_navvis_to_capture.py

@@ -148,14 +148,21 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         # 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.
+        # Rig to CamHead. Rig is in cam0 frame.
         cam0 = nv.get_cameras()["cam0"]
-        qvec, tvec = cam0["orientation"], cam0["position"]
-        camhead_from_rig = Pose(r=qvec, t=tvec)
+        camhead_from_rig = Pose(r=cam0["orientation"], t=cam0["position"])
+
+        # Rig to IMU.
+        imu_pose = Pose(*nv.get_imu_pose())
+        if nv.get_device() == 'VLX':
+            imu_from_camhead = imu_pose.inverse()
+            imu_from_rig = imu_from_camhead * camhead_from_rig
+        elif nv.get_device() == 'M6':
+            imu_from_footprint = imu_pose.inverse()
+            world_from_camhead = Pose(*nv.get_camhead(frame_id=0))
+            world_from_footprint = Pose(*nv.get_footprint(frame_id=0))
+            footprint_from_camhead = world_from_footprint.inverse() * world_from_camhead
+            imu_from_rig = imu_from_footprint * footprint_from_camhead * camhead_from_rig
 
         for trace in nv.get_trace():
             timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
@@ -169,7 +176,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             # 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
+            trace_pose = world_from_imu * imu_from_rig * tile0_pose
 
             if upright:
                 # Images are rotated by 90 degrees clockwise.

scantools/scanners/navvis/navvis.py

@@ -330,18 +330,18 @@ def get_camera_intrinsics(self):
     def __get_raw_pose(self, frame_id, cam_id):
         cam_id = self._convert_cam_id_to_str(cam_id)
         data = self.__frames[frame_id][cam_id]
-
-        # get pose
-        qvec = np.array(data["quaternion"])
-        tvec = np.array(data["position"])
-
-        return qvec, tvec
+        return np.array(data["quaternion"]), np.array(data["position"])
 
     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
+        return np.array(data["quaternion"]), np.array(data["position"])
+
+    def get_footprint(self, frame_id):
+        data = self.__frames[frame_id]["footprint"]
+        return np.array(data["quaternion"]), np.array(data["position"])
+
+    def get_imu_pose(self):
+        return self._imu["orientation"], self._imu["position"]
 
     # auxiliary function:
     #   fixes a camera-to-world qvec for upright fix