Parses navvis origin file and converts into Capture::Lamar csv file

CAPTURE.md

@@ -23,6 +23,7 @@ location1/                                  # a Capture directory
 │   │   ├── sensors.txt                     # list of all sensors with specs
 │   │   ├── trajectories.txt                # pose for each (timestamp, sensor)
 │   │   ├── wifi.txt                        # list of wifi measurements
+|   |   ├── origins.txt                     # list of NavVis Session origins (name, alginment pose) 
 │   │   ├── raw_data/                       # root path of images, point clouds, etc.
 │   │   │   ├── images_undistorted/
 │   │   │   ├── render/                     # root path for the rgb and depth maps renderings

scantools/capture/__init__.py

@@ -9,4 +9,5 @@
     RecordWifi, RecordWifiSignal, RecordsWifi)
 from .pose import Pose
 from .proc import Proc
+from .proc import GlobalAlignment
 from .misc import KeyType

scantools/capture/records.py

@@ -276,3 +276,6 @@ class RecordsBluetooth(RecordsArray[RecordBluetooth]):
             records[timestamp, sensor_id] = <RecordBluetooth>
     """
     record_type = RecordBluetooth
+
+
+

scantools/capture/session.py

@@ -8,7 +8,7 @@
 from .rigs import Rigs
 from .trajectories import Trajectories
 from .records import RecordsBluetooth, RecordsCamera, RecordsDepth, RecordsLidar, RecordsWifi
-from .proc import Proc
+from .proc import Proc, GlobalAlignment
 from .pose import Pose
 
 logger = logging.getLogger(__name__)
@@ -42,6 +42,7 @@ class Session:
     bt: Optional[RecordsBluetooth] = None
     proc: Optional[Proc] = None
     id: Optional[str] = None
+    origins: Optional[GlobalAlignment] = None
 
     data_dirname = 'raw_data'
     proc_dirname = 'proc'

scantools/run_navvis_to_capture.py

@@ -13,7 +13,7 @@
 from .capture import (
         Capture, Session, Sensors, create_sensor, Trajectories, Rigs, Pose,
         RecordsCamera, RecordsLidar, RecordBluetooth, RecordBluetoothSignal,
-        RecordsBluetooth, RecordWifi, RecordWifiSignal, RecordsWifi)
+        RecordsBluetooth, RecordWifi, RecordWifiSignal, RecordsWifi, GlobalAlignment)
 from .utils.misc import add_bool_arg
 from .utils.io import read_image, write_image
 
@@ -96,7 +96,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     sensors = Sensors()
     trajectory = Trajectories()
     images = RecordsCamera()
-    rigs = Rigs() if export_as_rig else None
+    rigs = Rigs() if export_as_rig else None    
 
     if export_as_rig:
         # This code assumes NavVis produces consistent rigs across all frames,
@@ -231,12 +231,34 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
             bluetooth_signals[timestamp_us, sensor_id] = RecordBluetooth()
         bluetooth_signals[timestamp_us, sensor_id][id] = RecordBluetoothSignal(rssi_dbm=rssi_dbm)
 
+    # Read the NavVis origin.json file if present and use proc.GlobalAlignment to save it.
+    navvis_origin = None
+    if nv.load_origin():
+        origin_qvec, origin_tvec, origin_crs = nv.get_origin()
+        navvis_origin = GlobalAlignment()
+        navvis_origin[origin_crs, navvis_origin.no_ref] = (
+            Pose(r=origin_qvec, t=origin_tvec),
+            [],
+        )
+        logger.info("Loaded NavVis origin.json")
+
     session = Session(
         sensors=sensors, rigs=rigs, trajectories=trajectory,
-        images=images, pointclouds=pointclouds, wifi=wifi_signals, bt=bluetooth_signals)
+        images=images, pointclouds=pointclouds, wifi=wifi_signals, bt=bluetooth_signals, 
+        origins=navvis_origin)
     capture.sessions[session_id] = session
     capture.save(capture.path, session_ids=[session_id])
 
+    # Read the NavVis origin.json file if present and use proc.GlobalAlignment to save it.
+    if nv.load_origin():
+        global_alginment_path = capture.data_path(session_id) / "origin.txt"
+        origin_qvec, origin_tvec, origin_crs = nv.get_origin()
+        global_alignment = GlobalAlignment()
+        global_alignment[origin_crs, global_alignment.no_ref] = (
+            Pose(r=origin_qvec, t=origin_tvec), [])   
+        global_alignment.save(global_alginment_path)
+        logger.info('Loaded NavVis origin.json and saved to %s.', global_alginment_path)
+
     logger.info('Generating raw data for session %s.', session_id)
     nv.undistort()
     for ts, cam in tqdm(session.images.key_pairs()):

scantools/scanners/navvis/navvis.py

@@ -10,6 +10,7 @@
 from .camera_tiles import Tiles, TileFormat
 from .ibeacon_parser import parse_navvis_ibeacon_packet, BluetoothMeasurement
 from .iwconfig_parser import parse_iwconfig, WifiMeasurement
+from .origin_parser import parse_navvis_origin_file, get_pose_from_navvis_origin, get_crs_from_navvis_origin
 from . import ocamlib
 from ...utils import transform
 from ...utils.io import read_csv, convert_dng_to_jpg
@@ -29,6 +30,7 @@ def __init__(self, input_path: Path, output_path: Optional[Path] = None,
         self._pointcloud_file_path = None
         self._trace_path = None
         self._imu = None
+        self._origin_file_path = None
 
         self._output_path = None
         self._output_image_path = None
@@ -37,6 +39,7 @@ def __init__(self, input_path: Path, output_path: Optional[Path] = None,
         self.__cameras = {}
         self.__frames = {}
         self.__trace = {}
+        self.__origin_data = {}
 
         # upright fix
         self.__upright = upright
@@ -72,6 +75,9 @@ def _set_dataset_paths(self, input_path: Path, output_path: Optional[Path], tile
         self._input_path = Path(input_path).absolute()
         if not self._input_path.exists():
             raise FileNotFoundError(f'Input path {self._input_path}.')
+
+        # Origin file path
+        self._origin_file_path = self._input_path / "anchors" / "origin.json"
 
         # Images path
         self._input_image_path = self._input_path / "cam"
@@ -677,6 +683,27 @@ def read_wifi(self):
                     wifi_measurements.append(wifi_measurement)
 
         return wifi_measurements
+
+    def get_origin(self):
+        """Returns the NavVis origin transformation vectors and coordinate reference
+        system (CRS) name.
+        Returns
+        -------
+        Tuple: Tuple containing the quaternion, translation vector, and CRS.
+        """
+
+        crs = get_crs_from_navvis_origin(self.__origin_data)
+        qvec, tvec = get_pose_from_navvis_origin(self.__origin_data)    
+        return qvec, tvec, crs
+
+    def load_origin(self):
+        """Tries loading the NavVis origin from file.
+        Returns
+        -------
+        Bool : True if origin was successfully loaded, False otherwise.
+        """
+        self.__origin_data = parse_navvis_origin_file(self._origin_file_path)
+        return self.__origin_data != {}
 
 #
 # auxiliary function for parallel computing

scantools/scanners/navvis/origin_parser.py

@@ -0,0 +1,114 @@
+import json
+from pathlib import Path
+
+UNKNOWN_CRS_NAME = 'UNKNOWN'
+
+def is_navvis_origin_valid(navvis_origin : dict):
+    """
+    Check if the NavVis origin dictionary is valid.
+    CRS is optional. Pose is required.
+    :param navvis_origin: NavVis origin dictionary
+    :return: True if valid, False otherwise
+    :rtype: bool
+    """
+    if  navvis_origin['Pose']['position']['x'] is None or \
+        navvis_origin['Pose']['position']['y'] is None or \
+        navvis_origin['Pose']['position']['z'] is None or \
+        navvis_origin['Pose']['orientation']['w'] is None or \
+        navvis_origin['Pose']['orientation']['x'] is None or \
+        navvis_origin['Pose']['orientation']['y'] is None or \
+        navvis_origin['Pose']['orientation']['z'] is None:
+        return False
+    return True
+
+def parse_navvis_origin_file(file_path : Path):
+    """
+    * The origin.json file is optional and if present it can be found 
+    in the anchors folder.
+
+    * The origin.json file contains two important values: pose and CRS.
+
+    * The pose transforms dataset entities into the origin. The origin 
+        of the dataset can be created in many different ways:
+        0 - The origin is the NavVis 'dataset' origin, where a dataset equals a NavVis session. 
+            The origin then defaults to identity and the origin.json file might not be even present.
+        1 - NavVis software allows relative alignment between dataset via the NavVis IVION Dataset Web Editor 
+            but also via the NavVis local processing software which is soon to be deprecated.
+        2 - The origin is the NavVis Site origin. NavVis organizes datasets in the same physical location
+            via Sites. The origin file contains then the transformation which moves all the entities of a 
+            NavVis dataset into the Site origin. Additionally NavVis IVION allows to register the Site origin
+            to a global coordinate system. Hence, many NavVis sessions can be registered then to the same 
+            global coordinate system. Note that this is achieved via the NavVis IVION Dataset Web Editor.
+        3 - The origin lies in a Coordinate Reference System (CRS) like EPSG:25834 https://epsg.io/25834.
+            The transformation is computed via geo-referenced Control Points which are registered during
+            capture. More information about control points and the origin can be found here:
+            https://knowledge.navvis.com/v1/docs/creating-the-control-point-poses-file 
+            https://knowledge.navvis.com/docs/what-coordinate-system-do-we-use-for-the-control-points-related-tasks
+
+    * The CRS value stands for Coordinate Reference System (CRS) and explains in which coordinate system the origin itself
+        is defined. Example: EPSG:25834 https://epsg.io/25834
+    :param file_path: Path to the file
+    :return: NavVis anchor origin dictionary
+    :rtype: Dict
+    """
+    if not file_path.exists():
+        print(f"Warning: Origin '{file_path}' does not exist.")
+        return {}
+
+    try:
+        with file_path.open() as f:
+            origin = json.load(f)
+            if not is_navvis_origin_valid(origin):
+                print("Invalid origin.json file", json.dumps(origin, indent=4))
+            return origin
+    except Exception as e:
+        print("Warning Failed reading origin.json file.", e)
+    return {}
+
+
+def get_crs_from_navvis_origin(navvis_origin : dict):
+    """
+    Get the label from the NavVis origin
+    :param navvis_origin: NavVis origin dictionary
+    :return: Label
+    :rtype: str
+    """
+
+    return navvis_origin.get('CRS', UNKNOWN_CRS_NAME)
+
+
+def get_pose_from_navvis_origin(navvis_origin : dict):
+    """
+    Extract the pose from the NavVis origin dictionary
+    :param navvis_origin: NavVis origin dictionary
+    :return: Quaternion and translation vector
+    :rtype: qvec, tvec
+    """
+
+    qvec = [1, 0, 0, 0]
+    tvec = [0, 0, 0]
+    if navvis_origin:
+        orientation = navvis_origin['Pose']['orientation']
+        position = navvis_origin['Pose']['position']
+        qvec = [orientation['w'], orientation['x'], orientation['y'], orientation['z']]
+        tvec = [position['x'], position['y'], position['z']]
+    return qvec, tvec
+
+
+def convert_navvis_origin_to_csv(navvis_origin : dict):
+    csv_str = "# CRS, qw, qx, qy, qz, tx, ty, tz\n"
+    if 'CRS' in navvis_origin:
+        crs = navvis_origin['CRS']
+    else:
+        crs = UNKNOWN_CRS_NAME
+    position = navvis_origin['Pose']['position']
+    orientation = navvis_origin['Pose']['orientation']
+    csv_str += (f"{crs},"
+                f"{orientation['w']},"
+                f"{orientation['x']},"
+                f"{orientation['y']},"
+                f"{orientation['z']},"
+                f"{position['x']},"
+                f"{position['y']},"
+                f"{position['z']}\n")
+    return csv_str

scantools/tests/test_navvis.py

@@ -404,3 +404,4 @@ def test_get_image_filename(m6_object, m6_testdata):
 
     res_image_filename = m6_object.get_image_filename(test_frame.id, test_frame.pose.camera_id)
     assert res_image_filename == exp_image_filename
+

scantools/tests/test_navvis_origin_parser.py

@@ -0,0 +1,118 @@
+""" Tests for origin_parse.py """
+import pytest
+import json
+import os
+import numpy as np
+
+from ..capture import Pose, GlobalAlignment
+from ..scanners.navvis import origin_parser
+
+GLOBAL_ALIGNMENT_TABLE_HEADER = "# label, reference_id, qw, qx, qy, qz, tx, ty, tz, [info]+\n"
+
+@pytest.mark.parametrize("navvis_origin, expected_csv_output", [({
+    "CRS": "EPSG:25834",
+    "Pose": {
+        "orientation": {
+            "w": 0.7071068,
+            "x": 0,
+            "y": 0,
+            "z": -0.7071068
+        },
+        "position": {
+            "x": 6.3,
+            "y": 2.4,
+            "z": 99.95
+        }
+    }},
+     GLOBAL_ALIGNMENT_TABLE_HEADER + "EPSG:25834, __absolute__," + 
+     " 0.7071067811865476, 0.0, 0.0, -0.7071067811865476," + 
+     " 6.3, 2.4, 99.95\n"),
+    ({
+        "Pose": {  
+            "orientation": {
+                "w": 1,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    }, GLOBAL_ALIGNMENT_TABLE_HEADER + origin_parser.UNKNOWN_CRS_NAME + ", __absolute__," + 
+    " 1.0, 0.0, 0.0, 0.0," + 
+    " 0.0, 0.0, 0.0\n"),
+])
+def test_parse_navvis_origin(navvis_origin, expected_csv_output, tmp_path):
+    navvis_origin_path = tmp_path / "navvis_origin.json"
+    with open(navvis_origin_path, 'w') as file:
+        json.dump(navvis_origin, file)
+
+    navvis_origin_loaded = origin_parser.parse_navvis_origin_file(navvis_origin_path)
+    assert navvis_origin_loaded == navvis_origin
+    os.remove(navvis_origin_path)
+
+    alignment = GlobalAlignment()
+    crs = origin_parser.get_crs_from_navvis_origin(navvis_origin_loaded)
+    qvec, tvec = origin_parser.get_pose_from_navvis_origin(navvis_origin_loaded)
+    alignment_pose = Pose(qvec, tvec)
+    alignment[crs, alignment.no_ref] = (
+            alignment_pose, [])
+    alignment_path = tmp_path / 'origin.txt'
+    alignment.save(alignment_path)
+
+    with open(alignment_path, 'r') as file:
+        csv_output = file.read()
+        print(csv_output)
+        print(expected_csv_output)
+        assert csv_output == expected_csv_output
+
+    alignment_loaded = GlobalAlignment().load(alignment_path)
+    os.remove(alignment_path)
+
+    alignment_pose_loaded = alignment_loaded.get_abs_pose(crs)
+    assert np.allclose(alignment_pose_loaded.qvec, 
+                       alignment_pose.qvec, 1e-10)
+    assert np.allclose(alignment_pose_loaded.t, 
+                       alignment_pose.t, 1e-10)
+
+
+@pytest.mark.parametrize("bad_json_keys_origin", [{
+    "CRS": "EPSG:25834",
+    "Pose": {
+        "orientation": {
+            "w": 0.8,
+            "x": 0,
+            "y": 0,
+            "z": -0.5
+        },
+        "positon": { # misspelled key
+            "x": 6.3,
+            "y": 2.4,
+            "z": 99.95
+        }
+    }},
+    {
+        "Pose": {  
+            "orentation": { # misspelled key
+                "w": 0.5,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    }
+])
+def test_parse_navvis_origin_bad_input(bad_json_keys_origin, tmp_path):
+    temp_origin_path = tmp_path / "bad_json_keys_origin.json"
+    with open(temp_origin_path, 'w') as file:
+        json.dump(bad_json_keys_origin, file)
+    assert not origin_parser.parse_navvis_origin_file(temp_origin_path)
+    os.remove(temp_origin_path)