diff --git a/CAPTURE.md b/CAPTURE.md index b243bb1..d639ad4 100644 --- a/CAPTURE.md +++ b/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 (label and alignment pose) │   │   ├── raw_data/ # root path of images, point clouds, etc. │   │   │   ├── images_undistorted/ │   │   │   ├── render/ # root path for the rgb and depth maps renderings diff --git a/pipelines/pipeline_navvis_rig.py b/pipelines/pipeline_navvis_rig.py index 7cfab70..c70e99f 100644 --- a/pipelines/pipeline_navvis_rig.py +++ b/pipelines/pipeline_navvis_rig.py @@ -68,6 +68,7 @@ ├── rigs.txt ├── sensors.txt ├── trajectories.txt + ├── origins.txt └── wifi.txt 1. **Mesh Generation** diff --git a/scantools/capture/__init__.py b/scantools/capture/__init__.py index fd21f28..9e77294 100644 --- a/scantools/capture/__init__.py +++ b/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 diff --git a/scantools/capture/records.py b/scantools/capture/records.py index e5a7a8e..facc64f 100644 --- a/scantools/capture/records.py +++ b/scantools/capture/records.py @@ -276,3 +276,6 @@ class RecordsBluetooth(RecordsArray[RecordBluetooth]): records[timestamp, sensor_id] = """ record_type = RecordBluetooth + + + diff --git a/scantools/capture/session.py b/scantools/capture/session.py index 541d2f5..864f163 100644 --- a/scantools/capture/session.py +++ b/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' diff --git a/scantools/run_navvis_to_capture.py b/scantools/run_navvis_to_capture.py index 1422958..c7bb5d4 100644 --- a/scantools/run_navvis_to_capture.py +++ b/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 @@ -231,9 +231,21 @@ 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]) diff --git a/scantools/scanners/navvis/navvis.py b/scantools/scanners/navvis/navvis.py index 920f0da..cfbf269 100644 --- a/scantools/scanners/navvis/navvis.py +++ b/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 diff --git a/scantools/scanners/navvis/origin_parser.py b/scantools/scanners/navvis/origin_parser.py new file mode 100644 index 0000000..770c4f7 --- /dev/null +++ b/scantools/scanners/navvis/origin_parser.py @@ -0,0 +1,119 @@ +import json +import logging +from pathlib import Path + +logger = logging.getLogger(__name__) + +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 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 + + 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 + + :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: + logger.warning( + "Failed reading origin.json file. %s", 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 diff --git a/scantools/tests/test_navvis.py b/scantools/tests/test_navvis.py index 06e11ce..66726a8 100644 --- a/scantools/tests/test_navvis.py +++ b/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 + diff --git a/scantools/tests/test_navvis_origin_parser.py b/scantools/tests/test_navvis_origin_parser.py new file mode 100644 index 0000000..5d8ec40 --- /dev/null +++ b/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)