From 399f1a39bdff911ebeda7d4685f8aa3b9bf5c1f9 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Fri, 2 Feb 2024 11:13:05 +0100
Subject: [PATCH 01/18] Read trace.csv. Mapping path: position, orientation,
 and magnetic field information in intervals.

---
 scantools/run_navvis_to_capture.py  | 16 +++++++++---
 scantools/scanners/navvis/navvis.py | 40 +++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+), 3 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 82b1940..470db82 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -54,7 +54,7 @@ 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
@@ -67,8 +67,6 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     camera_ids = nv.get_camera_indexes()
     tiles = nv.get_tiles()
 
-    num_frames = len(frame_ids)
-    num_cameras = len(camera_ids)
     num_tiles = nv.get_num_tiles()
 
     K = nv.get_camera_intrinsics()
@@ -136,6 +134,17 @@ 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:
+        sensors['trace'] = create_sensor('trace', name='Mapping path')
+        for trace in nv.get_trace():
+            timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
+            qvec = np.array([trace["ori_w"], trace["ori_x"], trace["ori_y"], trace["ori_z"]], dtype=np.float64)
+            tvec = np.array([trace["x"], trace["y"], trace["z"]], dtype=np.float64)
+            pose = Pose(r=qvec, t=tvec)
+            trajectory[timestamp_us, '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()
@@ -206,6 +215,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 c5c9c3f..cbab01c 100644
--- a/scantools/scanners/navvis/navvis.py
+++ b/scantools/scanners/navvis/navvis.py
@@ -27,6 +27,7 @@ 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._output_path = None
         self._output_image_path = None
@@ -34,6 +35,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 +62,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 +82,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)
@@ -176,6 +186,32 @@ def load_cameras(self):
         # save metadata inside the class
         self.__cameras = cameras
 
+    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
 
@@ -210,6 +246,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]
@@ -585,6 +624,7 @@ def read_wifi(self):
 
         return wifi_measurements
 
+
 #
 # auxiliary function for parallel computing
 #

From ec2175eb14204b8a41ee457354cda6a27a433160 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Mon, 5 Feb 2024 22:44:53 +0100
Subject: [PATCH 02/18] trace poses need to be transformed relative to the rig
 if we use export_as_rig

---
 scantools/run_navvis_to_capture.py | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 470db82..30381bd 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -138,10 +138,14 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         sensors['trace'] = create_sensor('trace', name='Mapping path')
         for trace in nv.get_trace():
             timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
-            qvec = np.array([trace["ori_w"], trace["ori_x"], trace["ori_y"], trace["ori_z"]], dtype=np.float64)
-            tvec = np.array([trace["x"], trace["y"], trace["z"]], dtype=np.float64)
-            pose = Pose(r=qvec, t=tvec)
-            trajectory[timestamp_us, 'trace'] = pose
+            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_device = Pose(r=qvec, t=tvec)
+            if export_as_rig:
+                trajectory[timestamp_us, 'trace'] = rig_from_world * world_from_device
+            else:
+                trajectory[timestamp_us, 'trace'] = world_from_device
+
         # Sort the trajectory by timestamp.
         trajectory = Trajectories(dict(sorted(trajectory.items())))
 

From bc0272d689f8c212c7ff8b82b2e46cd25fac0d7d Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Thu, 29 Feb 2024 00:05:23 +0100
Subject: [PATCH 03/18] apply correction to the odom trajectory from trace.csv

---
 scantools/run_navvis_to_capture.py | 50 ++++++++++++++++++------------
 1 file changed, 31 insertions(+), 19 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 213f484..59127a8 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -7,15 +7,15 @@
 import cv2
 import numpy as np
 
-from . import logger
-from .scanners import NavVis
-from .scanners.navvis.camera_tiles import TileFormat
-from .capture import (
+from scantools import logger
+from scantools.scanners import NavVis
+from scantools.scanners.navvis.camera_tiles import TileFormat
+from scantools.capture import (
         Capture, Session, Sensors, create_sensor, Trajectories, Rigs, Pose,
         RecordsCamera, RecordsLidar, RecordBluetooth, RecordBluetoothSignal,
         RecordsBluetooth, RecordWifi, RecordWifiSignal, RecordsWifi)
-from .utils.misc import add_bool_arg
-from .utils.io import read_image, write_image
+from scantools.utils.misc import add_bool_arg
+from scantools.utils.io import read_image, write_image
 
 TILE_CHOICES = sorted([attr.name.split('_')[1] for attr in TileFormat])
 
@@ -58,7 +58,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     if session_id is None:
         session_id = input_path.name
-    assert session_id not in capture.sessions
+    # assert session_id not in capture.sessions
 
     output_path = capture.data_path(session_id)
     nv = NavVis(input_path, output_path, tiles_format, upright)
@@ -84,7 +84,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         cy = cy / h * new_h
         camera_params = ('PINHOLE', new_w, new_h, fx, fy, cx, cy)
 
-    device = nv.get_device()
+    odom = nv.get_device()
 
     sensors = Sensors()
     trajectory = Trajectories()
@@ -104,7 +104,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             sensor_id += f'-{tile_id}' if num_tiles > 1 else ''
             sensor = create_sensor(
                 'camera', sensor_params=camera_params,
-                name=f'NavVis {device} camera-cam{camera_id} tile-{tiles_format} id-{tile_id}')
+                name=f'NavVis {odom} camera-cam{camera_id} tile-{tiles_format} id-{tile_id}')
             sensors[sensor_id] = sensor
 
             if export_as_rig:
@@ -136,16 +136,28 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     if export_trace:
         sensors['trace'] = create_sensor('trace', name='Mapping path')
+
+        # Odometry from trace.csv file.
+        odom = Trajectories()
         for trace in nv.get_trace():
             timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
             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_device = Pose(r=qvec, t=tvec)
-            if export_as_rig:
-                trajectory[timestamp_us, 'trace'] = rig_from_world * world_from_device
-            else:
-                trajectory[timestamp_us, 'trace'] = world_from_device
-
+            odom[timestamp_us, 'trace'] = world_from_device
+
+        # Create a list of tuples, each containing a timestamp and the corresponding closest_timestamp
+        differences = [(ts, min(odom.keys(), key=lambda x:abs(x-ts))) for ts in trajectory]
+        # Find the tuple with the smallest difference between ts and closest_timestamp
+        ts, closest_timestamp = min(differences, key=lambda x: abs(x[0]-x[1]))
+        # Find the closest_timestamp for the ts with the smallest difference.
+        closest_timestamp = min(odom.keys(), key=lambda x:abs(x-ts))
+        # Find the correction to apply to the camera trajectory.
+        correction = trajectory[ts, 'navvis_rig'] * odom[closest_timestamp,'trace'].inverse()
+
+        # Save the trace with the correction applied.
+        for timestamp_us in odom:
+            trajectory[timestamp_us, 'trace'] = correction * odom[timestamp_us,'trace']
         # Sort the trajectory by timestamp.
         trajectory = Trajectories(dict(sorted(trajectory.items())))
 
@@ -194,7 +206,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     for ts, cam in tqdm(session.images.key_pairs()):
         downsample = downsample_max_edge is not None
         # Camera 0 is (physically) mounted upside down on VLX.
-        flip = (upright and device == 'VLX' and cam.startswith('cam0'))
+        flip = (upright and odom == 'VLX' and cam.startswith('cam0'))
         if downsample or flip:
             image_path = capture.data_path(session_id) / session.images[ts, cam]
             image = read_image(image_path)
@@ -225,9 +237,9 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     args = parser.parse_args().__dict__
 
     capture_path = args.pop('capture_path')
-    if capture_path.exists():
-        args['capture'] = Capture.load(capture_path)
-    else:
-        args['capture'] = Capture(sessions={}, path=capture_path)
+    # if capture_path.exists():
+    #     args['capture'] = Capture.load(capture_path)
+    # else:
+    args['capture'] = Capture(sessions={}, path=capture_path)
 
     run(**args)

From 15f634e0780301b76b85614e0cfc4a4a5524346a Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Tue, 5 Mar 2024 00:12:12 +0100
Subject: [PATCH 04/18] wip

---
 scantools/run_navvis_to_capture.py  | 29 +++++-------
 scantools/scanners/navvis/navvis.py | 71 +++++++++++++++++++++--------
 2 files changed, 62 insertions(+), 38 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 59127a8..22b6edf 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -136,28 +136,21 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     if export_trace:
         sensors['trace'] = create_sensor('trace', name='Mapping path')
-
-        # Odometry from trace.csv file.
-        odom = Trajectories()
         for trace in nv.get_trace():
             timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
             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_device = Pose(r=qvec, t=tvec)
-            odom[timestamp_us, 'trace'] = world_from_device
-
-        # Create a list of tuples, each containing a timestamp and the corresponding closest_timestamp
-        differences = [(ts, min(odom.keys(), key=lambda x:abs(x-ts))) for ts in trajectory]
-        # Find the tuple with the smallest difference between ts and closest_timestamp
-        ts, closest_timestamp = min(differences, key=lambda x: abs(x[0]-x[1]))
-        # Find the closest_timestamp for the ts with the smallest difference.
-        closest_timestamp = min(odom.keys(), key=lambda x:abs(x-ts))
-        # Find the correction to apply to the camera trajectory.
-        correction = trajectory[ts, 'navvis_rig'] * odom[closest_timestamp,'trace'].inverse()
-
-        # Save the trace with the correction applied.
-        for timestamp_us in odom:
-            trajectory[timestamp_us, 'trace'] = correction * odom[timestamp_us,'trace']
+            trace_in_imu = Pose(r=qvec, t=tvec)
+
+            qvec, tvec = nv._imu["orientation"], nv._imu["position"]
+            imu_from_rig = Pose(r=qvec, t=tvec)
+
+            cam0_info = nv.get_cameras()["cam0"]
+            qvec, tvec = cam0_info["orientation"], cam0_info["position"]
+            cam0_from_rig = Pose(r=qvec, t=tvec)
+
+            trajectory[timestamp_us, 'trace'] = cam0_from_rig * imu_from_rig.inverse() * trace_in_imu
+
         # Sort the trajectory by timestamp.
         trajectory = Trajectories(dict(sorted(trajectory.items())))
 
diff --git a/scantools/scanners/navvis/navvis.py b/scantools/scanners/navvis/navvis.py
index cbab01c..6649f8d 100644
--- a/scantools/scanners/navvis/navvis.py
+++ b/scantools/scanners/navvis/navvis.py
@@ -28,6 +28,7 @@ def __init__(self, input_path: Path, output_path: Optional[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
@@ -154,38 +155,62 @@ 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
 
+        # ToDo: find a better place. This is not a camera model
+        # 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",
@@ -220,7 +245,7 @@ def get_pointcloud_path(self):
 
     def get_output_path(self):
         return self._output_path
-    
+
     def get_device(self):
         return self.__device
 
@@ -294,6 +319,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):

From e352c03a5911f1197e12cfadaf0924920d93aa4a Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Fri, 8 Mar 2024 13:28:25 +0100
Subject: [PATCH 05/18] wip

---
 scantools/run_navvis_to_capture.py | 30 ++++++++++++++++--------------
 1 file changed, 16 insertions(+), 14 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 22b6edf..6dde56b 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -7,15 +7,15 @@
 import cv2
 import numpy as np
 
-from scantools import logger
-from scantools.scanners import NavVis
-from scantools.scanners.navvis.camera_tiles import TileFormat
-from scantools.capture import (
+from . import logger
+from .scanners import NavVis
+from .scanners.navvis.camera_tiles import TileFormat
+from .capture import (
         Capture, Session, Sensors, create_sensor, Trajectories, Rigs, Pose,
         RecordsCamera, RecordsLidar, RecordBluetooth, RecordBluetoothSignal,
         RecordsBluetooth, RecordWifi, RecordWifiSignal, RecordsWifi)
-from scantools.utils.misc import add_bool_arg
-from scantools.utils.io import read_image, write_image
+from .utils.misc import add_bool_arg
+from .utils.io import read_image, write_image
 
 TILE_CHOICES = sorted([attr.name.split('_')[1] for attr in TileFormat])
 
@@ -136,20 +136,22 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     if export_trace:
         sensors['trace'] = create_sensor('trace', name='Mapping path')
+
+        qvec, tvec = nv._imu["orientation"], nv._imu["position"]
+        imu_from_camhead = Pose(r=qvec, t=tvec)
+
+        cam0 = nv.get_cameras()["cam0"]
+        qvec, tvec = cam0["orientation"], cam0["position"]
+        cam0_from_camhead = Pose(r=qvec, t=tvec)
+
         for trace in nv.get_trace():
             timestamp_us = int(trace["nsecs"]) // 1_000  # convert from ns to us
             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)
             trace_in_imu = Pose(r=qvec, t=tvec)
 
-            qvec, tvec = nv._imu["orientation"], nv._imu["position"]
-            imu_from_rig = Pose(r=qvec, t=tvec)
-
-            cam0_info = nv.get_cameras()["cam0"]
-            qvec, tvec = cam0_info["orientation"], cam0_info["position"]
-            cam0_from_rig = Pose(r=qvec, t=tvec)
-
-            trajectory[timestamp_us, 'trace'] = cam0_from_rig * imu_from_rig.inverse() * trace_in_imu
+            trace_in_cam0 = cam0_from_camhead * imu_from_camhead.inverse() * trace_in_imu
+            trajectory[timestamp_us, 'trace'] = trace_in_cam0
 
         # Sort the trajectory by timestamp.
         trajectory = Trajectories(dict(sorted(trajectory.items())))

From 67332709e6e31a0af7e2397574ec5f86e0a4423a Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Tue, 12 Mar 2024 15:00:21 +0100
Subject: [PATCH 06/18] Fixed bug (working now)

---
 scantools/run_navvis_to_capture.py | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 6dde56b..db05bc7 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -138,20 +138,23 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         sensors['trace'] = create_sensor('trace', name='Mapping path')
 
         qvec, tvec = nv._imu["orientation"], nv._imu["position"]
-        imu_from_camhead = Pose(r=qvec, t=tvec)
+        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"]
-        cam0_from_camhead = Pose(r=qvec, t=tvec)
+        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)
-            trace_in_imu = Pose(r=qvec, t=tvec)
+            world_from_imu = Pose(r=qvec, t=tvec)
 
-            trace_in_cam0 = cam0_from_camhead * imu_from_camhead.inverse() * trace_in_imu
-            trajectory[timestamp_us, 'trace'] = trace_in_cam0
+            trajectory[timestamp_us, 'trace'] = world_from_imu * imu_from_camhead * camhead_from_rig
 
         # Sort the trajectory by timestamp.
         trajectory = Trajectories(dict(sorted(trajectory.items())))

From 9a19a7c49cc989f84ea3015b1282258db641185e Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Tue, 12 Mar 2024 15:18:14 +0100
Subject: [PATCH 07/18] .

---
 scantools/run_navvis_to_capture.py  | 8 ++++----
 scantools/scanners/navvis/navvis.py | 2 --
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index db05bc7..de37fd4 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -235,9 +235,9 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     args = parser.parse_args().__dict__
 
     capture_path = args.pop('capture_path')
-    # if capture_path.exists():
-    #     args['capture'] = Capture.load(capture_path)
-    # else:
-    args['capture'] = Capture(sessions={}, path=capture_path)
+    if capture_path.exists():
+        args['capture'] = Capture.load(capture_path)
+    else:
+        args['capture'] = Capture(sessions={}, path=capture_path)
 
     run(**args)
diff --git a/scantools/scanners/navvis/navvis.py b/scantools/scanners/navvis/navvis.py
index 6649f8d..270af07 100644
--- a/scantools/scanners/navvis/navvis.py
+++ b/scantools/scanners/navvis/navvis.py
@@ -197,7 +197,6 @@ def load_cameras(self):
         # save metadata inside the class
         self.__cameras = cameras
 
-        # ToDo: find a better place. This is not a camera model
         # IMU information from sensor_frame.xml.
         imu = {}
         imu['position'] = np.array([
@@ -655,7 +654,6 @@ def read_wifi(self):
 
         return wifi_measurements
 
-
 #
 # auxiliary function for parallel computing
 #

From 036d9d53522e5cff2dda4b977689ccc4520aedfc Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Tue, 12 Mar 2024 15:21:20 +0100
Subject: [PATCH 08/18] clean up

---
 scantools/run_navvis_to_capture.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index de37fd4..6e23418 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -58,7 +58,6 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     if session_id is None:
         session_id = input_path.name
-    # assert session_id not in capture.sessions
 
     output_path = capture.data_path(session_id)
     nv = NavVis(input_path, output_path, tiles_format, upright)
@@ -84,7 +83,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         cy = cy / h * new_h
         camera_params = ('PINHOLE', new_w, new_h, fx, fy, cx, cy)
 
-    odom = nv.get_device()
+    device = nv.get_device()
 
     sensors = Sensors()
     trajectory = Trajectories()
@@ -104,7 +103,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             sensor_id += f'-{tile_id}' if num_tiles > 1 else ''
             sensor = create_sensor(
                 'camera', sensor_params=camera_params,
-                name=f'NavVis {odom} camera-cam{camera_id} tile-{tiles_format} id-{tile_id}')
+                name=f'NavVis {device} camera-cam{camera_id} tile-{tiles_format} id-{tile_id}')
             sensors[sensor_id] = sensor
 
             if export_as_rig:
@@ -204,7 +203,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     for ts, cam in tqdm(session.images.key_pairs()):
         downsample = downsample_max_edge is not None
         # Camera 0 is (physically) mounted upside down on VLX.
-        flip = (upright and odom == 'VLX' and cam.startswith('cam0'))
+        flip = (upright and device == 'VLX' and cam.startswith('cam0'))
         if downsample or flip:
             image_path = capture.data_path(session_id) / session.images[ts, cam]
             image = read_image(image_path)

From 302f624a15565ed4d2c9b2c26d13d46200661e39 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 14:00:37 +0100
Subject: [PATCH 09/18] apply tile_id=0 rotation

---
 scantools/capture/session.py        |  5 +++--
 scantools/run_navvis_to_capture.py  | 22 ++++++++++++++++++++--
 scantools/scanners/navvis/navvis.py | 24 +++++++++++++-----------
 3 files changed, 36 insertions(+), 15 deletions(-)

diff --git a/scantools/capture/session.py b/scantools/capture/session.py
index 541d2f5..b611f1c 100644
--- a/scantools/capture/session.py
+++ b/scantools/capture/session.py
@@ -52,8 +52,9 @@ def __post_init__(self):
         if self.rigs is not None:
             assert len(self.sensors.keys() & self.rigs.keys()) == 0
             all_devices |= self.rigs.keys()
-        if self.trajectories is not None:
-            assert len(self.trajectories.device_ids - all_devices) == 0
+        # Now "trace" can be a device_id, so this assertion is not valid anymore
+        # if self.trajectories is not None:
+        #     assert len(self.trajectories.device_ids - all_devices) == 0
 
     @property
     def cameras(self) -> Dict[str, Camera]:
diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 6e23418..a355e21 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -122,6 +122,10 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             pose = get_pose(nv, upright, frame_id, cam_id=0)
             trajectory[timestamp_us, rig_id] = pose
 
+            # qvec, tvec = nv.get_camhead(frame_id)
+            # world_from_camhead = Pose(r=qvec, t=tvec)
+            # trajectory[timestamp_us, "camhead"] = world_from_camhead
+
         for camera_id in camera_ids:
             for tile_id in range(num_tiles):
                 sensor_id = f'cam{camera_id}_{tiles_format}'
@@ -133,7 +137,11 @@ 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:
+    if export_trace and export_as_rig:
+        # Add "trace" to the rig with identity pose.
+        rigs[rig_id, "trace"] = Pose()
+
+        # Add "trace" as a sensors.
         sensors['trace'] = create_sensor('trace', name='Mapping path')
 
         qvec, tvec = nv._imu["orientation"], nv._imu["position"]
@@ -153,7 +161,17 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             tvec = np.array([trace["x"], trace["y"], trace["z"]], dtype=float)
             world_from_imu = Pose(r=qvec, t=tvec)
 
-            trajectory[timestamp_us, 'trace'] = world_from_imu * imu_from_camhead * camhead_from_rig
+            # 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
+
+            # # Fix upright VLX extrinsics.
+            # if upright and 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())))
diff --git a/scantools/scanners/navvis/navvis.py b/scantools/scanners/navvis/navvis.py
index 270af07..b72d81a 100644
--- a/scantools/scanners/navvis/navvis.py
+++ b/scantools/scanners/navvis/navvis.py
@@ -344,14 +344,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]
 
@@ -372,12 +365,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]

From 4f15644b26b781710be4773d672a64364b986d97 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 15:54:26 +0100
Subject: [PATCH 10/18] fix bug for rotation with upright

---
 scantools/run_navvis_to_capture.py | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index a355e21..5a5c695 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -167,9 +167,19 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
             trace_pose = world_from_imu * imu_from_camhead * camhead_from_rig * tile0_pose
 
-            # # Fix upright VLX extrinsics.
-            # if upright and nv.get_device() == 'VLX':
-            #     trace_pose = fix_vlx_extrinsics(trace_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=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
 

From 531b4ef541a809033e6eb7893f6092f5595e8528 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 15:55:01 +0100
Subject: [PATCH 11/18] clean up

---
 scantools/run_navvis_to_capture.py | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 5a5c695..030b92c 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -122,10 +122,6 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             pose = get_pose(nv, upright, frame_id, cam_id=0)
             trajectory[timestamp_us, rig_id] = pose
 
-            # qvec, tvec = nv.get_camhead(frame_id)
-            # world_from_camhead = Pose(r=qvec, t=tvec)
-            # trajectory[timestamp_us, "camhead"] = world_from_camhead
-
         for camera_id in camera_ids:
             for tile_id in range(num_tiles):
                 sensor_id = f'cam{camera_id}_{tiles_format}'

From 46b397877b7e58c8dc3d56d0380d2071d27ad43b Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 16:39:43 +0100
Subject: [PATCH 12/18] typo

---
 scantools/run_navvis_to_capture.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 030b92c..aff3f94 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -137,7 +137,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
         # Add "trace" to the rig with identity pose.
         rigs[rig_id, "trace"] = Pose()
 
-        # Add "trace" as a sensors.
+        # Add "trace" as a sensor.
         sensors['trace'] = create_sensor('trace', name='Mapping path')
 
         qvec, tvec = nv._imu["orientation"], nv._imu["position"]

From 1d3ea02a96fd44fcf2c024e2382d277f3fd29ad8 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 22:52:07 +0100
Subject: [PATCH 13/18] export_trace depends on export_as_rig

---
 pipelines/pipeline_navvis_rig.py   | 1 +
 scantools/run_navvis_to_capture.py | 6 +++++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/pipelines/pipeline_navvis_rig.py b/pipelines/pipeline_navvis_rig.py
index 61c97bb..4592f3d 100644
--- a/pipelines/pipeline_navvis_rig.py
+++ b/pipelines/pipeline_navvis_rig.py
@@ -138,6 +138,7 @@ def run(
                 tiles_format,
                 session,
                 export_as_rig=True,
+                export_as_trace=True,
                 copy_pointcloud=True,
             )
 
diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index aff3f94..128079c 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -59,6 +59,10 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     if session_id is None:
         session_id = input_path.name
 
+    # trace.csv is correctly exported only if export_as_rig is True.
+    if export_trace:
+        export_as_rig = True
+
     output_path = capture.data_path(session_id)
     nv = NavVis(input_path, output_path, tiles_format, upright)
 
@@ -133,7 +137,7 @@ 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 and export_as_rig:
+    if export_trace:
         # Add "trace" to the rig with identity pose.
         rigs[rig_id, "trace"] = Pose()
 

From 06d300928ce0acfc7f2f487571f1aa0a0cc4cc8e Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Wed, 13 Mar 2024 23:05:11 +0100
Subject: [PATCH 14/18] reverting

---
 scantools/capture/session.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/scantools/capture/session.py b/scantools/capture/session.py
index b611f1c..541d2f5 100644
--- a/scantools/capture/session.py
+++ b/scantools/capture/session.py
@@ -52,9 +52,8 @@ def __post_init__(self):
         if self.rigs is not None:
             assert len(self.sensors.keys() & self.rigs.keys()) == 0
             all_devices |= self.rigs.keys()
-        # Now "trace" can be a device_id, so this assertion is not valid anymore
-        # if self.trajectories is not None:
-        #     assert len(self.trajectories.device_ids - all_devices) == 0
+        if self.trajectories is not None:
+            assert len(self.trajectories.device_ids - all_devices) == 0
 
     @property
     def cameras(self) -> Dict[str, Camera]:

From 200460b67f86aa17e823d3db5cce21ceddb275c1 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Sat, 16 Mar 2024 11:48:44 +0100
Subject: [PATCH 15/18] bug fix

---
 scantools/run_navvis_to_capture.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 128079c..b61a70d 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -176,7 +176,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
                     [0, 0, 1]
                 ])
                 R = trace_pose.R @ R_fix
-                trace_pose = Pose(r=R, t=pose.t)
+                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)

From ea34ae1f7d6e1baeb26c91963ee37956e38ce963 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Sat, 16 Mar 2024 11:55:23 +0100
Subject: [PATCH 16/18] bug fix

---
 pipelines/pipeline_navvis_rig.py   | 2 +-
 scantools/run_navvis_to_capture.py | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/pipelines/pipeline_navvis_rig.py b/pipelines/pipeline_navvis_rig.py
index 4592f3d..77a132a 100644
--- a/pipelines/pipeline_navvis_rig.py
+++ b/pipelines/pipeline_navvis_rig.py
@@ -138,7 +138,7 @@ def run(
                 tiles_format,
                 session,
                 export_as_rig=True,
-                export_as_trace=True,
+                export_trace=True,
                 copy_pointcloud=True,
             )
 
diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index b61a70d..493e4c5 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -61,6 +61,8 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     # trace.csv is correctly exported only if export_as_rig is True.
     if export_trace:
+        if export_as_rig==False:
+            logger.warning('export_trace was set to True, but export_as_rig to False.')
         export_as_rig = True
 
     output_path = capture.data_path(session_id)

From b59b706cecab1ad5f6787244521f54779f124b2b Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Sat, 16 Mar 2024 11:58:17 +0100
Subject: [PATCH 17/18] improve warning comment

---
 scantools/run_navvis_to_capture.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index 493e4c5..c905b41 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -61,8 +61,11 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     # trace.csv is correctly exported only if export_as_rig is True.
     if export_trace:
-        if export_as_rig==False:
-            logger.warning('export_trace was set to True, but export_as_rig to False.')
+        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)

From c233a592f92ea5f0466749bde6be98163d7f9c14 Mon Sep 17 00:00:00 2001
From: Pablo Speciale <Pablo.Speciale@microsoft.com>
Date: Sat, 16 Mar 2024 11:58:51 +0100
Subject: [PATCH 18/18] remove redundant comment (it is already explained in
 the warning)

---
 scantools/run_navvis_to_capture.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py
index c905b41..c2b528e 100644
--- a/scantools/run_navvis_to_capture.py
+++ b/scantools/run_navvis_to_capture.py
@@ -59,7 +59,6 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     if session_id is None:
         session_id = input_path.name
 
-    # trace.csv is correctly exported only if export_as_rig is True.
     if export_trace:
         if not export_as_rig:
             logger.warning(