Code release

.gitignore

@@ -0,0 +1,2 @@
+**/__pycache__/
+.DS_Store

README.md

@@ -1 +1,6 @@
-# lamaria
+# Benchmarking Egocentric Visual-Inertial SLAM at City Scale
+
+### Downloading the dataset
+
+
+### Additional tools

defaults.yaml

@@ -0,0 +1,53 @@
+# Default configuration for run_pipeline.py
+
+sensor:
+  camera_model: RAD_TAN_THIN_PRISM_FISHEYE
+  has_slam_drops: false
+
+keyframing:
+  max_rotation: 20.0
+  max_distance: 1.0
+  max_elapsed: 1e9 # 1 second in ns
+
+triangulation:
+  feature_conf: aliked-n16
+  matcher_conf: aliked+lightglue
+  retrieval_conf: netvlad
+  num_retrieval_matches: 5
+
+  # relaxed triangulation
+  # https://github.com/colmap/colmap/blob/dbdb3cd5a9a90a01b7e1096905ba2ffc34a68b82/src/colmap/sfm/incremental_triangulator.h
+  merge_max_reproj_error: 15.0
+  complete_max_reproj_error: 15.0
+  min_angle: 1.0
+
+  # https://github.com/colmap/colmap/blob/dbdb3cd5a9a90a01b7e1096905ba2ffc34a68b82/src/colmap/sfm/incremental_mapper.h
+  filter_max_reproj_error: 15.0
+  filter_min_tri_angle: 1.0
+
+optimization:
+  general:
+    use_callback: true
+    max_num_iterations: 10
+    normalize_reconstruction: false
+    minimizer_progress_to_stdout: true
+    update_state_every_iteration: true
+
+  cam:
+    feature_std: 1.0
+    optimize_focal_length: false
+    optimize_principal_point: false
+    optimize_extra_params: false
+    optimize_cam_from_rig: false
+
+  imu:
+    optimize_scale: false
+    optimize_gravity: false
+    optimize_imu_from_rig: false
+    optimize_bias: true
+    gyro_infl: 1.0
+    acc_infl: 1.0
+    integration_noise_density: 0.05
+
+mps:
+  use_online_calibration: false # relevant only when mps folder is used

lamaria/__init__.py

@@ -0,0 +1,13 @@
+import logging
+
+formatter = logging.Formatter(
+    fmt="[%(asctime)s %(name)s %(levelname)s] %(message)s",
+    datefmt="%Y/%m/%d %H:%M:%S",
+)
+handler = logging.StreamHandler()
+handler.setFormatter(formatter)
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+logger.addHandler(handler)
+logger.propagate = False

lamaria/config/options.py

@@ -0,0 +1,173 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field, replace
+
+from omegaconf import OmegaConf, open_dict
+
+
+def _structured_merge_to_obj(cls, section) -> object:
+    """
+    Merge a YAML section onto a structured
+    config made from the dataclass `cls`,
+    then return a dataclass instance.
+    """
+    base = OmegaConf.structured(cls)
+    merged = OmegaConf.merge(base, section or {})
+    return OmegaConf.to_object(merged)
+
+
+# General options
+@dataclass(slots=True)
+class MPSOptions:
+    use_online_calibration: bool = (
+        False  # set to True if using MPS online calibration result
+    )
+
+    @classmethod
+    def load(cls, cfg: OmegaConf | None = None) -> MPSOptions:
+        if cfg is None:
+            return cls()
+
+        return _structured_merge_to_obj(cls, cfg)
+
+
+@dataclass(slots=True)
+class SensorOptions:
+    left_cam_stream_id: str = "1201-1"
+    right_cam_stream_id: str = "1201-2"
+    right_imu_stream_id: str = "1202-1"
+    camera_model: str = "RAD_TAN_THIN_PRISM_FISHEYE"
+    has_slam_drops: bool = (
+        False  # check vrs json metadata file for each sequence
+    )
+
+    @classmethod
+    def load(cls, cfg: OmegaConf | None = None) -> SensorOptions:
+        if cfg is None:
+            return cls()
+
+        obj: SensorOptions = _structured_merge_to_obj(cls, cfg)
+        return obj
+
+
+# Estimate to COLMAP options
+@dataclass(slots=True)
+class EstimateToLamariaOptions:
+    mps: MPSOptions = field(default_factory=MPSOptions)
+    sensor: SensorOptions = field(default_factory=SensorOptions)
+
+    @classmethod
+    def load(
+        cls,
+        cfg_mps: OmegaConf | None = None,
+        cfg_sensor: OmegaConf | None = None,
+    ) -> EstimateToLamariaOptions:
+        if cfg_mps is None or cfg_sensor is None:
+            return cls()
+
+        base = cls()
+        return replace(
+            base,
+            mps=MPSOptions.load(cfg_mps),
+            sensor=SensorOptions.load(cfg_sensor),
+        )
+
+
+# Keyframing options
+@dataclass(slots=True)
+class KeyframeSelectorOptions:
+    max_rotation: float = 20.0  # degrees
+    max_distance: float = 1.0  # meters
+    max_elapsed: int = int(1e9)  # 1 second in ns
+
+    @classmethod
+    def load(cls, cfg: OmegaConf | None = None) -> KeyframeSelectorOptions:
+        if cfg is None:
+            return cls()
+
+        cfg = OmegaConf.create(cfg)
+        with open_dict(cfg):
+            if "max_elapsed" in cfg and isinstance(cfg.max_elapsed, float):
+                cfg.max_elapsed = int(cfg.max_elapsed)
+
+        obj: KeyframeSelectorOptions = _structured_merge_to_obj(cls, cfg)
+        return obj
+
+
+# Triangulation options
+@dataclass(slots=True)
+class TriangulatorOptions:
+    feature_conf: str = "aliked-n16"
+    matcher_conf: str = "aliked+lightglue"
+    retrieval_conf: str = "netvlad"
+    num_retrieval_matches: int = 5
+
+    # colmap defaults
+    merge_max_reproj_error: float = 4.0
+    complete_max_reproj_error: float = 4.0
+    min_angle: float = 1.5
+
+    filter_max_reproj_error: float = 4.0
+    filter_min_tri_angle: float = 1.5
+
+    @classmethod
+    def load(cls, cfg: OmegaConf | None = None) -> TriangulatorOptions:
+        if cfg is None:
+            return cls()
+
+        return _structured_merge_to_obj(cls, cfg)
+
+
+# Optimization options
+@dataclass(slots=True)
+class OptCamOptions:
+    feature_std: float = 1.0  # in pixels
+    optimize_focal_length: bool = False
+    optimize_principal_point: bool = False
+    optimize_extra_params: bool = False
+    optimize_cam_from_rig: bool = False
+
+
+@dataclass(slots=True)
+class OptIMUOptions:
+    gyro_infl: float = 1.0
+    acc_infl: float = 1.0
+    integration_noise_density: float = 0.05
+
+    optimize_scale: bool = False
+    optimize_gravity: bool = False
+    optimize_imu_from_rig: bool = False
+    optimize_bias: bool = False
+
+
+@dataclass(slots=True)
+class OptOptions:
+    use_callback: bool = True
+    max_num_iterations: int = 10
+    normalize_reconstruction: bool = False
+    minimizer_progress_to_stdout: bool = True
+    update_state_every_iteration: bool = True
+
+
+@dataclass(slots=True)
+class VIOptimizerOptions:
+    cam: OptCamOptions = field(default_factory=OptCamOptions)
+    imu: OptIMUOptions = field(default_factory=OptIMUOptions)
+    optim: OptOptions = field(default_factory=OptOptions)
+
+    @classmethod
+    def load(cls, cfg: OmegaConf | None = None) -> VIOptimizerOptions:
+        if cfg is None:
+            return cls()
+
+        base = cls()
+        cam = _structured_merge_to_obj(OptCamOptions, cfg.cam)
+        imu = _structured_merge_to_obj(OptIMUOptions, cfg.imu)
+        optim = _structured_merge_to_obj(OptOptions, cfg.general)
+
+        return replace(
+            base,
+            cam=cam,
+            imu=imu,
+            optim=optim,
+        )

lamaria/config/pipeline.py

@@ -0,0 +1,114 @@
+from collections.abc import Sequence
+from pathlib import Path
+
+from omegaconf import DictConfig, OmegaConf
+
+from .options import (
+    EstimateToLamariaOptions,
+    KeyframeSelectorOptions,
+    TriangulatorOptions,
+    VIOptimizerOptions,
+)
+
+
+class PipelineOptions:
+    def __init__(self) -> None:
+        self._output_path: Path = Path("output/")
+        self._estimate_to_colmap_options: EstimateToLamariaOptions = (
+            EstimateToLamariaOptions()
+        )
+        self._keyframing_options: KeyframeSelectorOptions = (
+            KeyframeSelectorOptions()
+        )
+        self._triangulator_options: TriangulatorOptions = TriangulatorOptions()
+        self._vi_optimizer_options: VIOptimizerOptions = VIOptimizerOptions()
+
+    def load(
+        self,
+        yaml: Path | str,
+        cli_overrides: Sequence[str] | None = None,
+    ) -> None:
+        """Load configuration from a YAML file and apply any overrides."""
+        cfg = OmegaConf.load(str(yaml))
+        if cli_overrides:
+            cfg = OmegaConf.merge(
+                cfg, OmegaConf.from_dotlist(list(cli_overrides))
+            )
+        OmegaConf.resolve(cfg)
+
+        self._update_from_cfg(cfg)
+
+    def _update_from_cfg(self, cfg: DictConfig) -> None:
+        """Update object attributes from a config."""
+        self._estimate_to_colmap_options = EstimateToLamariaOptions.load(
+            cfg.mps,
+            cfg.sensor,
+        )
+        self._keyframing_options = KeyframeSelectorOptions.load(cfg.keyframing)
+        self._triangulator_options = TriangulatorOptions.load(cfg.triangulation)
+        self._vi_optimizer_options = VIOptimizerOptions.load(cfg.optimization)
+
+        out = cfg.get("output_path", None)
+        self._output_path = Path(out) if out is not None else Path("/output/")
+
+    @property
+    def output_path(self) -> Path:
+        """Get the parent output path."""
+        return self._output_path
+
+    @output_path.setter
+    def output_path(self, path: Path) -> None:
+        """Set the parent output path."""
+        self._output_path = path
+
+    # Properties for estimate to COLMAP
+    @property
+    def estimate_to_colmap_options(self) -> EstimateToLamariaOptions:
+        return self._estimate_to_colmap_options
+
+    @property
+    def images_path(self) -> Path:
+        return self._output_path / "images"
+
+    @property
+    def colmap_model_path(self) -> Path:
+        return self._output_path / "initial_recon"
+
+    # Properties for keyframing
+    @property
+    def keyframing_options(self) -> KeyframeSelectorOptions:
+        return self._keyframing_options
+
+    @property
+    def keyframes_path(self) -> Path:
+        return self._output_path / "keyframes"
+
+    @property
+    def kf_model_path(self) -> Path:
+        return self._output_path / "keyframed_recon"
+
+    # Properties for triangulation
+    @property
+    def triangulator_options(self) -> TriangulatorOptions:
+        return self._triangulator_options
+
+    @property
+    def hloc_path(self) -> Path:
+        return self._output_path / "hloc"
+
+    @property
+    def pairs_file(self) -> Path:
+        return self._output_path / "hloc" / "pairs.txt"
+
+    @property
+    def tri_model_path(self) -> Path:
+        return self._output_path / "triangulated_recon"
+
+    # Properties for visual-inertial optimization
+    @property
+    def vi_optimizer_options(self) -> VIOptimizerOptions:
+        return self._vi_optimizer_options
+
+    @property
+    def optim_model_path(self) -> Path:
+        return self._output_path / "optim_recon"