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

scantools/capture/__init__.py

@@ -10,3 +10,4 @@
 from .pose import Pose
 from .proc import Proc
 from .misc import KeyType
+from .namedposes import NamedPoses

scantools/capture/namedposes.py

@@ -0,0 +1,24 @@
+from pathlib import Path
+from .pose import Pose
+from ..utils.io import read_csv, write_csv
+
+
+class NamedPoses(dict):
+    def __setitem__(self, name: str, pose: Pose):
+        if not isinstance(name, str):
+            raise TypeError('expect str type as name')
+        if not isinstance(pose, Pose):
+            raise TypeError('expect Pose type as pose')
+        super().__setitem__(name, pose)
+
+    def load(self, path: Path) -> 'NamedPoses':
+        table = read_csv(path)
+        for name, *qt in table:
+            self[name] = Pose.from_list(qt)
+
+    def save(self, path: Path):
+        columns = ['name', 'qw', 'qx', 'qy', 'qz', 'tx', 'ty', 'tz']
+        table = []
+        for name, pose in self.items():
+            table.append([name] + pose.to_list())
+        write_csv(path, table, columns=columns)

scantools/capture/records.py

@@ -8,6 +8,7 @@
 from typing import Dict, List, Set, TypeVar, Union
 import numpy as np
 
+from .pose import Pose
 from ..utils.io import read_csv, write_csv
 from .misc import KeyType
 
@@ -276,3 +277,6 @@ class RecordsBluetooth(RecordsArray[RecordBluetooth]):
             records[timestamp, sensor_id] = <RecordBluetooth>
     """
     record_type = RecordBluetooth
+
+
+

scantools/capture/session.py

@@ -7,7 +7,7 @@
 from .sensors import Sensors, Camera
 from .rigs import Rigs
 from .trajectories import Trajectories
-from .records import RecordsBluetooth, RecordsCamera, RecordsDepth, RecordsLidar, RecordsWifi
+from .records import RecordsBluetooth, RecordsCamera, RecordsDepth, RecordsLidar, RecordsWifi, NamedPoses
 from .proc import Proc
 from .pose import Pose
 
@@ -42,6 +42,7 @@ class Session:
     bt: Optional[RecordsBluetooth] = None
     proc: Optional[Proc] = None
     id: Optional[str] = None
+    namedposes: Optional[NamedPoses] = 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, NamedPoses)
 from .utils.misc import add_bool_arg
 from .utils.io import read_image, write_image
 
@@ -98,6 +98,15 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
     images = RecordsCamera()
     rigs = Rigs() if export_as_rig else None
 
+    # Read the NavVis origin.json file if present and convert to Capture format.
+    if nv.load_origin():
+        origin_qvec, origin_tvec, origin_crs = nv.get_origin()
+        origin_pose = Pose(r=origin_qvec, t=origin_tvec)
+        origin_poses = NamedPoses()
+        origin_poses['origin'] = origin_pose    
+
+
+
     if export_as_rig:
         # This code assumes NavVis produces consistent rigs across all frames,
         # using `cam_id=0` as the rig base.
@@ -226,7 +235,7 @@ def run(input_path: Path, capture: Capture, tiles_format: str, session_id: Optio
 
     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, namedposes=origin_poses)
     capture.sessions[session_id] = session
     capture.save(capture.path, session_ids=[session_id])
 

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 UNKNOWN_CRS_NAME, parse_navvis_origin_file
 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 / "artifacts" / "origin.json"
 
         # Images path
         self._input_image_path = self._input_path / "cam"
@@ -677,6 +683,54 @@ def read_wifi(self):
                     wifi_measurements.append(wifi_measurement)
 
         return wifi_measurements
+
+    def get_origin(self):
+        crs = UNKNOWN_CRS_NAME
+        qvec = [1, 0, 0, 0]
+        tvec = [0, 0, 0]        
+        if self.__origin_data:
+            orientation = self.__origin_data['orientation']
+            position = self.__origin_data['position']
+            qvec = [orientation['w'], orientation['x'], orientation['y'], orientation['z']]
+            tvec = [position['x'], position['y'], position['z']]
+        return crs, qvec, tvec
+
+    def load_origin(self):
+        """Tries loading the NavVis origin from file.
+
+        * The origin.json file is optional and if present it can be found 
+          in the artifacts 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
+
+
+        Returns
+        -------
+        Bool : True if origin was successfully loaded, False otherwise.
+        """
+
+        self.__origin_data = parse_navvis_origin_file(Path(self._input_path) / "origin.json")
+        return self.__origin_data 
 
 #
 # auxiliary function for parallel computing

scantools/scanners/navvis/origin_parser.py

@@ -0,0 +1,69 @@
+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
+    :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):
+    """
+    Read NavVis Origin File Format Version 1.0
+    :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 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_capture_namedposes.py

@@ -0,0 +1,35 @@
+import pytest
+import numpy as np
+import os
+from scipy.spatial.transform import Rotation
+
+from ..capture import Pose
+from ..capture import NamedPoses
+
+def Rz(theta):
+    return Rotation.from_euler('z', theta, degrees=True)
+
+
+@pytest.mark.parametrize(
+    "rot,tvec",
+    [
+        (['1', '0', '0', '0'], ['0', '1', '1']),
+        (Rz(30), ['1', '1', '0'])
+    ])
+def test_pose_valid(rot, tvec, tmp_path):
+    temp_file_path = tmp_path / 'named_poses.csv'
+    pose = Pose(rot, tvec)
+    poses = NamedPoses()
+    poses['pose1'] = pose
+    poses['pose2'] = pose
+    poses.save(temp_file_path)
+    loaded_poses = NamedPoses()
+    loaded_poses.load(temp_file_path)
+    assert len(poses) == len(loaded_poses)
+    assert poses.keys() == loaded_poses.keys()
+    for k in poses.keys():
+        assert poses[k].to_list() == loaded_poses[k].to_list()
+    os.remove(temp_file_path)
+
+
+

scantools/tests/test_origin_parser.py

@@ -0,0 +1,91 @@
+""" Tests for origin_parse.py """
+import pytest
+import json
+import os
+
+from ..scanners.navvis import origin_parser
+
+@pytest.mark.parametrize("nominal_origin, expected_output_csv", [({
+    "CRS": "EPSG:25834",
+    "Pose": {
+        "orientation": {
+            "w": 0.8,
+            "x": 0,
+            "y": 0,
+            "z": -0.5
+        },
+        "position": {
+            "x": 6.3,
+            "y": 2.4,
+            "z": 99.95
+        }
+    }},
+     "# CRS, qw, qx, qy, qz, \
+        tx, ty, tz\n\
+        EPSG:25834,0.8,0,0,-0.5,\
+        6.3,2.4,99.95\n"),
+    ({
+        "Pose": {  
+            "orientation": {
+                "w": 0.5,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    }, "# CRS, qw, qx, qy, qz, tx, ty, tz\n" + origin_parser.UNKNOWN_CRS_NAME + ",0.5,0,0,0,0,0,0\n"),
+])
+def test_parse_navvis_origin(nominal_origin, expected_output_csv, tmp_path):
+    temp_origin_path = tmp_path / "input_data.json"
+    with open(temp_origin_path, 'w') as file:
+        json.dump(nominal_origin, file)
+    origin = origin_parser.parse_navvis_origin_file(temp_origin_path)
+    assert origin == nominal_origin
+    assert expected_output_csv.replace(" ","") == origin_parser.convert_navvis_origin_to_csv(origin).replace(" ","")
+    os.remove(temp_origin_path)    
+
+@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)
+    with pytest.raises(KeyError):
+        origin_parser.convert_navvis_origin_to_csv(bad_json_keys_origin)
+    os.remove(temp_origin_path)
+