Update COLMAP integration for 4.1.0: add GLOMAP mapper and matching type options

.gitignore

@@ -1,2 +1,6 @@
+vocab_tree_flickr100K_words1M.bin
+vocab_tree_flickr100K_words256K.bin
+vocab_tree_flickr100K_words32K.bin
+
 LightGlue/
-__pycache__/
+__pycache__/

colmap_mapper.py

@@ -1,85 +0,0 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - colmap_mapper.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 2.0
-- Created: 2024-07-12
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-Mapping stage of the colmap pipeline (Python port of colmap_mapper.sh): runs COLMAP's incremental
-mapper or GLOMAP's global mapper on the database, picks the sub-model with the most registered
-images and exports it as TXT into the colmap folder. Called by vslamlab_colmap.py.
-"""
-
-import re
-from pathlib import Path
-
-from colmap_utilities import colmap_camera, run
-
-
-def registered_images(model_dir: Path) -> int:
-    out = run(["colmap", "model_analyzer", "--path", str(model_dir)], capture=True)
-    match = re.search(r"Registered images: (\d+)", out)
-    return int(match.group(1)) if match else 0
-
-
-def select_best_model(exp_folder_colmap: Path) -> tuple[Path, int]:
-    """COLMAP may split the scene into several sub-models (<exp_folder_colmap>/0, /1, ...), and the
-    largest one is not necessarily /0. Returns the sub-model with the most registered images and
-    records the choice in <exp_folder_colmap>/best_model for the rest of the pipeline (gui, ...)."""
-    best_model, best_num_images = None, -1
-    for model_dir in sorted(p for p in exp_folder_colmap.iterdir() if p.is_dir() and p.name.isdigit()):
-        if not (model_dir / "images.bin").is_file():
-            continue
-        num_images = registered_images(model_dir)
-        print(f"        sub-model {model_dir.name}: {num_images} registered images")
-        if num_images > best_num_images:
-            best_model, best_num_images = model_dir, num_images
-    if best_model is None or best_num_images == 0:  # glomap writes an empty /0 when its pose graph is empty
-        raise SystemExit(f"    no reconstruction produced (no sub-model with registered images in {exp_folder_colmap})")
-    (exp_folder_colmap / "best_model").write_text(f"{best_model}\n")
-    return best_model, best_num_images
-
-
-def run_mapper(exp_folder_colmap: Path, rgb_path: Path, calibration_yaml: Path, camera_name: str,
-               mapper_type: str, optimize_intrinsics: int) -> Path:
-    """Reconstruct from <exp_folder_colmap>/colmap_database.db; returns the best sub-model folder."""
-    print("Executing colmap_mapper ...")
-
-    calibration_model, _, _ = colmap_camera(calibration_yaml, camera_name)
-    print(f"        camera model : {calibration_model}")
-
-    # optimize_intrinsics (0/1, default 1): whether bundle adjustment refines the camera intrinsics.
-    # With 1, focal length and distortion (extra params) are refined and the principal point stays
-    # fixed, matching COLMAP's own defaults; with 0 the intrinsics from the calibration yaml are kept
-    # as given. An 'unknown' calibration model has no intrinsics to keep (the matcher started COLMAP
-    # from a guess), so it always refines regardless of the flag.
-    if calibration_model == "unknown" and optimize_intrinsics != 1:
-        print(f"        WARNING: optimize_intrinsics={optimize_intrinsics} ignored: camera model is 'unknown' (no intrinsics to keep fixed), refining intrinsics")
-        optimize_intrinsics = 1
-    refine = "1" if optimize_intrinsics == 1 else "0"
-    ba_refine_focal_length, ba_refine_principal_point, ba_refine_extra_params = refine, "0", refine
-    print(f"        optimize_intrinsics: {optimize_intrinsics} (ba_refine_focal_length={ba_refine_focal_length}, "
-          f"ba_refine_principal_point={ba_refine_principal_point}, ba_refine_extra_params={ba_refine_extra_params})")
-
-    database = exp_folder_colmap / "colmap_database.db"
-    if mapper_type == "glomap":
-        print("    global mapper (GLOMAP) ...")
-        command, prefix = "global_mapper", "GlobalMapper"
-    else:
-        print("    colmap mapper (COLMAP) ...")
-        command, prefix = "mapper", "Mapper"
-    run(["colmap", command,
-         "--database_path", str(database),
-         "--image_path", str(rgb_path),
-         "--output_path", str(exp_folder_colmap),
-         f"--{prefix}.ba_refine_focal_length", ba_refine_focal_length,
-         f"--{prefix}.ba_refine_principal_point", ba_refine_principal_point,
-         f"--{prefix}.ba_refine_extra_params", ba_refine_extra_params])
-
-    best_model, best_num_images = select_best_model(exp_folder_colmap)
-
-    print(f"    colmap model_converter (sub-model {best_model.name}, {best_num_images} images) ...")
-    run(["colmap", "model_converter", "--input_path", str(best_model), "--output_path", str(exp_folder_colmap), "--output_type", "TXT"])
-    return best_model

colmap_mapper.sh

@@ -0,0 +1,43 @@
+#!/bin/bash
+echo "Executing colmap_mapper.sh ..."
+
+sequence_path="$1" 
+exp_folder="$2" 
+exp_id="$3" 
+settings_yaml="$4"
+calibration_yaml="$5"
+rgb_csv="$6"
+camera_name="$7"
+
+exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
+rgb_dir="${camera_name}"
+rgb_path="${sequence_path}/${rgb_dir}"
+
+read -r calibration_model more_ <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+echo "        camera model : $calibration_model"
+ba_refine_focal_length="0"
+ba_refine_principal_point="0"
+ba_refine_extra_params="0"
+if [ "${calibration_model}" == "unknown" ]
+then
+  ba_refine_focal_length="1"
+  ba_refine_principal_point="1"
+  ba_refine_extra_params="1"
+fi
+
+echo "    colmap mapper ..."
+database="${exp_folder_colmap}/colmap_database.db"
+
+colmap mapper \
+    --database_path ${database} \
+    --image_path ${rgb_path} \
+    --output_path ${exp_folder_colmap} \
+    --Mapper.ba_refine_focal_length ${ba_refine_focal_length} \
+    --Mapper.ba_refine_principal_point ${ba_refine_principal_point} \
+    --Mapper.ba_refine_extra_params ${ba_refine_extra_params}
+
+echo "    colmap model_converter ..."
+colmap model_converter \
+	--input_path ${exp_folder_colmap}/0 --output_path ${exp_folder_colmap} --output_type TXT
+
+

colmap_matcher.py

@@ -1,111 +0,0 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - colmap_matcher.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 2.0
-- Created: 2024-07-12
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-Feature extraction and matching stage of the colmap pipeline (Python port of colmap_matcher.sh):
-creates the COLMAP database, extracts features for the frames listed in the experiment's rgb csv
-(optionally masked) and matches them exhaustively or sequentially. Called by vslamlab_colmap.py.
-"""
-
-from pathlib import Path
-
-from colmap_utilities import colmap_camera, detect_gpus, run
-from create_colmap_image_list import create_colmap_image_list
-from create_colmap_mask_dir import create_colmap_mask_dir
-
-# matching_type -> (FeatureExtraction.type, FeatureMatching.type)
-MATCHING_TYPES: dict[str, tuple[str, str]] = {
-    'sift_bruteforce': ('SIFT', 'SIFT_BRUTEFORCE'),
-    'sift_lightglue': ('SIFT', 'SIFT_LIGHTGLUE'),
-    'aliked_bruteforce': ('ALIKED_N16ROT', 'ALIKED_BRUTEFORCE'),
-    'aliked_lightglue': ('ALIKED_N16ROT', 'ALIKED_LIGHTGLUE'),
-}
-
-
-def run_matcher(sequence_path: Path, exp_folder_colmap: Path, rgb_path: Path, rgb_csv: Path, calibration_yaml: Path,
-                camera_name: str, matcher_type: str, matching_type: str, use_gpu: int, use_mask: int) -> Path:
-    """Build <exp_folder_colmap>/colmap_database.db with features and matches; returns its path."""
-    print("\nExecuting colmap_matcher ...")
-
-    if matching_type not in MATCHING_TYPES:
-        raise SystemExit(f"Unknown matching_type: {matching_type}")
-    feature_extraction_type, feature_matching_type = MATCHING_TYPES[matching_type]
-
-    gpu_index_list, num_threads = detect_gpus(use_gpu)
-
-    calibration_model, colmap_camera_model, camera_params = colmap_camera(calibration_yaml, camera_name)
-
-    colmap_image_list = exp_folder_colmap / "colmap_image_list.txt"
-    create_colmap_image_list(str(rgb_csv), str(colmap_image_list), camera_name)
-
-    database = exp_folder_colmap / "colmap_database.db"
-    database.unlink(missing_ok=True)
-    run(["colmap", "database_creator", "--database_path", str(database)])
-
-    print(f"    colmap feature_extractor ({feature_extraction_type}) ...")
-    print(f"        gpu_index: {gpu_index_list}")
-    print(f"        camera model : {calibration_model} (colmap: {colmap_camera_model})")
-    camera_params_args: list[str] = []
-    if camera_params:
-        print(f"        camera params: {camera_params}")
-        camera_params_args = ["--ImageReader.camera_params", camera_params]
-
-    # Masks (use_mask=1): the rgb csv's path_mask_<i> column (1 = usable pixel, 0 = masked out - no
-    # features are extracted where the mask is 0). One shared mask (refrax) goes through
-    # --ImageReader.camera_mask_path, per-frame masks (mask2former, datasets shipping masks) through
-    # a directory of <image name>.png symlinks and --ImageReader.mask_path.
-    mask_args: list[str] = []
-    if use_mask == 1:
-        mask_spec = create_colmap_mask_dir(str(rgb_csv), camera_name, str(sequence_path), str(exp_folder_colmap / "colmap_masks"))
-        if mask_spec.startswith("camera_mask:"):
-            mask_args = ["--ImageReader.camera_mask_path", mask_spec[len("camera_mask:"):]]
-            print(f"        mask: shared camera mask {mask_spec[len('camera_mask:'):]}")
-        elif mask_spec.startswith("mask_dir:"):
-            mask_args = ["--ImageReader.mask_path", mask_spec[len("mask_dir:"):]]
-            print(f"        mask: per-frame masks in {mask_spec[len('mask_dir:'):]}")
-        else:
-            print(f"        mask: none available for {camera_name} in {rgb_csv}")
-    else:
-        print(f"        mask: disabled (use_mask={use_mask})")
-
-    run(["colmap", "feature_extractor",
-         "--database_path", str(database),
-         "--image_path", str(rgb_path),
-         "--image_list_path", str(colmap_image_list),
-         "--ImageReader.camera_model", colmap_camera_model,
-         "--ImageReader.single_camera", "1",
-         "--ImageReader.single_camera_per_folder", "1",
-         "--FeatureExtraction.type", feature_extraction_type,
-         "--FeatureExtraction.use_gpu", str(use_gpu),
-         "--FeatureExtraction.gpu_index", gpu_index_list,
-         "--FeatureExtraction.num_threads", str(num_threads),
-         *camera_params_args, *mask_args])
-
-    matching_args = ["--database_path", str(database),
-                     "--FeatureMatching.type", feature_matching_type,
-                     "--FeatureMatching.use_gpu", str(use_gpu),
-                     "--FeatureMatching.gpu_index", gpu_index_list,
-                     "--FeatureMatching.num_threads", str(num_threads)]
-
-    if matcher_type == "exhaustive":
-        print(f"    colmap exhaustive_matcher ({feature_matching_type}) ...")
-        print(f"        gpu_index: {gpu_index_list}")
-        run(["colmap", "exhaustive_matcher", *matching_args])
-    elif matcher_type == "sequential":
-        # Loop detection uses COLMAP's default vocabulary tree for the feature type (a FAISS index
-        # per SIFT / ALIKED / LoMa, auto-downloaded once into ~/.cache/colmap). The legacy
-        # flickr100K FLANN trees are rejected by COLMAP >= 3.12 ("Failed to read faiss index").
-        print(f"    colmap sequential_matcher ({feature_matching_type}) ...")
-        print(f"        Vocabulary Tree: COLMAP default for {feature_extraction_type} (cached in ~/.cache/colmap)")
-        print(f"        gpu_index: {gpu_index_list}")
-        run(["colmap", "sequential_matcher", *matching_args,
-             "--SequentialMatching.loop_detection", "1"])
-    else:
-        raise SystemExit(f"Unknown matcher_type: {matcher_type}")
-
-    return database

colmap_matcher.sh

@@ -0,0 +1,155 @@
+#!/bin/bash
+echo ""
+echo "Executing colmap_matcher.sh ..."
+
+sequence_path="$1"
+exp_folder="$2"
+exp_id="$3"
+settings_yaml="$4"
+calibration_yaml="$5"
+rgb_csv="$6"
+matcher_type="$7"
+use_gpu="$8"
+camera_name="$9"
+
+exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
+rgb_dir=$(awk -F, 'NR==2 { split($2,a,"/"); print a[1]; exit }' "$rgb_csv")
+rgb_path="${sequence_path}/${rgb_dir}"
+
+# Get calibration model
+read -r calibration_model more_ <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+
+# Create colmap image list
+colmap_image_list="${exp_folder_colmap}/colmap_image_list.txt"
+python3 Baselines/colmap/create_colmap_image_list.py "$rgb_csv" "$colmap_image_list" "$camera_name"
+
+# Create Colmap Database
+database="${exp_folder_colmap}/colmap_database.db"
+rm -rf ${database}
+colmap database_creator --database_path ${database}
+
+# Feature extractor
+echo "    colmap feature_extractor ..."
+
+if [ "${calibration_model}" == "unknown" ]
+then
+echo "        camera model : $calibration_model"
+colmap feature_extractor \
+   --database_path ${database} \
+   --image_path ${rgb_path} \
+   --image_list_path ${colmap_image_list} \
+   --ImageReader.camera_model SIMPLE_PINHOLE \
+   --ImageReader.single_camera 1 \
+   --ImageReader.single_camera_per_folder 1 \
+   --FeatureExtraction.use_gpu ${use_gpu}
+fi
+
+if [ "${calibration_model}" == "pinhole" ]
+then
+  read -r calibration_model fx fy cx cy <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+  echo "        camera model : $calibration_model"
+  echo "            fx: $fx , fy: $fy , cx: $cx , cy: $cy"
+  colmap feature_extractor \
+	--database_path ${database} \
+	--image_path ${rgb_path} \
+	--image_list_path ${colmap_image_list} \
+	--ImageReader.camera_model PINHOLE \
+	--ImageReader.single_camera 1 \
+	--ImageReader.single_camera_per_folder 1 \
+	--FeatureExtraction.use_gpu ${use_gpu} \
+	--ImageReader.camera_params "${fx},${fy},${cx},${cy}"
+fi
+
+if [ "${calibration_model}" == "radtan4" ]
+then
+  read -r calibration_model fx fy cx cy k1 k2 p1 p2 <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+  echo "        camera model : $calibration_model"
+  echo "            fx: $fx , fy: $fy , cx: $cx , cy: $cy"
+  echo "            k1: $k1 , k2: $k2 , p1: $p1 , p2: $p2"
+  colmap feature_extractor \
+	--database_path ${database} \
+	--image_path ${rgb_path} \
+	--image_list_path ${colmap_image_list} \
+	--ImageReader.camera_model "OPENCV" \
+	--ImageReader.single_camera 1 \
+  --ImageReader.single_camera_per_folder 0 \
+	--FeatureExtraction.use_gpu ${use_gpu} \
+  --ImageReader.camera_params "${fx},${fy},${cx},${cy},${k1},${k2},${p1},${p2}"
+fi
+
+if [ "${calibration_model}" == "radtan5" ]
+then
+  read -r calibration_model fx fy cx cy k1 k2 p1 p2 k3 <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+  echo "        camera model : $calibration_model"
+  echo "            fx: $fx , fy: $fy , cx: $cx , cy: $cy"
+  echo "            k1: $k1 , k2: $k2 , p1: $p1 , p2: $p2, k3: $k3"
+  colmap feature_extractor \
+	--database_path ${database} \
+	--image_path ${rgb_path} \
+	--image_list_path ${colmap_image_list} \
+	--ImageReader.camera_model "FULL_OPENCV" \
+	--ImageReader.single_camera 1 \
+  --ImageReader.single_camera_per_folder 1 \
+	--FeatureExtraction.use_gpu ${use_gpu} \
+  --ImageReader.camera_params "${fx},${fy},${cx},${cy},${k1},${k2},${p1},${p2},${k3},0,0,0"
+fi
+
+if [ "${calibration_model}" == "equid4" ]
+then
+  read -r calibration_model fx fy cx cy k1 k2 k3 k4 <<< $(python3 Baselines/colmap/get_calibration.py "$calibration_yaml" "$camera_name")
+  echo "        camera model : $calibration_model"
+  echo "            fx: $fx , fy: $fy , cx: $cx , cy: $cy"
+  echo "            k1: $k1 , k2: $k2 , k3: $k3 , k4: $k4"
+	colmap feature_extractor \
+	--database_path ${database} \
+	--image_path ${rgb_path} \
+	--image_list_path ${colmap_image_list} \
+	--ImageReader.camera_model "OPENCV_FISHEYE"\
+	--ImageReader.single_camera 1 \
+	--ImageReader.single_camera_per_folder 1 \
+	--FeatureExtraction.use_gpu ${use_gpu} \
+	--ImageReader.camera_params "${fx},${fy},${cx},${cy},${k1},${k2},${k3},${k4}"
+fi
+
+# Exhaustive Feature Matcher
+if [ "${matcher_type}" == "exhaustive" ]
+then
+	echo "    colmap exhaustive_matcher ..."
+  colmap exhaustive_matcher \
+     --database_path ${database} \
+     --FeatureMatching.use_gpu ${use_gpu}
+fi
+
+# Sequential Feature Matcher
+if [ "${matcher_type}" == "sequential" ]
+then
+  num_rgb=$(( $(wc -l < "$rgb_csv") - 1 ))
+
+  # Pick vocabulary tree based on the number of images
+  vocabulary_tree="Baselines/colmap/vocab_tree_faiss_flickr100K_words32K.bin"
+  if [ "$num_rgb" -gt 1000 ]; then
+    vocabulary_tree="Baselines/colmap/vocab_tree_faiss_flickr100K_words256K.bin"
+  fi
+  if [ "$num_rgb" -gt 10000 ]; then
+    vocabulary_tree="Baselines/colmap/vocab_tree_faiss_flickr100K_words1M.bin"
+  fi
+
+  echo "    colmap sequential_matcher ..."
+  echo "        Vocabulary Tree: $vocabulary_tree"
+  colmap sequential_matcher \
+    --database_path "${database}" \
+    --SequentialMatching.loop_detection 1 \
+    --SequentialMatching.vocab_tree_path ${vocabulary_tree} \
+    --FeatureMatching.use_gpu "${use_gpu}"
+fi
+
+# LightGlue Feature Matcher
+if [ "${matcher_type}" == "custom" ]
+then
+  colmap exhaustive_matcher \
+      --database_path ${database} \
+      --FeatureMatching.use_gpu ${use_gpu}
+
+  pixi run -e lightglue python3 Baselines/colmap/feature_matcher.py --database ${database} --rgb_path ${rgb_path} --rgb_csv ${rgb_csv}
+fi
+

colmap_reconstruction.sh

@@ -0,0 +1,69 @@
+#!/bin/bash
+
+# Default values
+matcher_type="exhaustive"
+use_gpu="1"
+verbose="0"
+settings_yaml=""
+sequence_path=""
+exp_folder=""
+exp_id=""
+calibration_yaml=""
+rgb_csv=""
+camera_name="rgb_0"
+
+# Function to split key-value pairs and assign them to variables
+split_and_assign() {
+  local input=$1
+  local key=$(echo $input | cut -d':' -f1)
+  local value=$(echo $input | cut -d':' -f2-)
+  eval $key=$value
+}
+
+# Read Inputs
+for ((i=1; i<=$#; i++)); do
+    split_and_assign "${!i}"
+done
+
+exp_id=$(printf "%05d" ${exp_id})
+
+echo -e "\n================= Experiment Configuration ================="
+echo "  Sequence Path     : $sequence_path"
+echo "  Experiment Folder : $exp_folder"
+echo "  Experiment ID     : $exp_id"
+echo "  Verbose           : $verbose"
+echo "  Matcher Type      : $matcher_type"
+echo "  Use GPU           : $use_gpu"
+echo "  Settings YAML     : $settings_yaml"
+echo "  Calibration YAML  : $calibration_yaml"
+echo "  RGB CSV           : $rgb_csv"
+echo "  Camera Name       : $camera_name"
+echo "============================================================"
+
+# Create folder to save colmap files
+exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
+rm -rf "$exp_folder_colmap"
+mkdir "$exp_folder_colmap"
+
+# Run COLMAP scripts for matching and mapping
+export QT_QPA_PLATFORM_PLUGIN_PATH="$CONDA_PREFIX/plugins/platforms"
+colmap_args="$sequence_path $exp_folder $exp_id $settings_yaml $calibration_yaml $rgb_csv"
+./Baselines/colmap/colmap_matcher.sh $colmap_args $matcher_type $use_gpu $camera_name
+./Baselines/colmap/colmap_mapper.sh $colmap_args $camera_name
+
+# Convert COLMAP outputs to a format suitable for VSLAM-LAB
+python Baselines/colmap/colmap_to_vslamlab.py $sequence_path $exp_folder $exp_id $verbose $rgb_csv $camera_name
+
+# Visualization with colmap gui
+if [ "$verbose" -eq 1 ]; then
+  exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
+  rgb_dir=$(awk -F, 'NR==2 { split($2,a,"/"); print a[1]; exit }' "$rgb_csv")
+  rgb_path="${sequence_path}/${rgb_dir}"
+  database="${exp_folder_colmap}/colmap_database.db"
+  colmap gui --import_path "${exp_folder_colmap}/0" --database_path ${database} --image_path ${rgb_path}
+fi
+
+# # Remove colmap data
+# rm -rf ${exp_folder_colmap}
+
+

colmap_to_vslamlab.py

@@ -1,89 +1,102 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - colmap_to_vslamlab.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 1.1
-- Created: 2024-07-12
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-Writes <exp_folder>/<exp_id>_KeyFrameTrajectory.csv (VSLAM-LAB's TUM-style csv, camera-to-world
-poses) from the TXT model <exp_folder>/colmap_<exp_id>/images.txt, taking each image's timestamp
-from the experiment's rgb csv (COLMAP image ids are 1-based row indices of that csv, as the image
-list was written in csv order). Importable (vslamlab_colmap.py) and runnable with the positional
-arguments the old shell pipeline used.
-"""
-
-import os
-import sys
-from pathlib import Path
-
 import numpy as np
-import pandas as pd
 from scipy.spatial.transform import Rotation as R
+import sys
+import os
+import pandas as pd
 
-
-def get_colmap_keyframes(images_file: str | Path, number_of_header_lines: int = 4) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
-    """(image ids, t_wc, q_wc xyzw) sorted by image id, quaternion signs made continuous."""
+def get_colmap_keyframes(images_file, number_of_header_lines, verbose=False):
     print(f"get_colmap_keyframes: {images_file}")
+    
+    image_id = []
+    q_wc_xyzw = []
+    t_wc = []
 
-    image_id, q_wc_xyzw, t_wc = [], [], []
-    with open(images_file, 'r') as file:
+    with open(f"{images_file}", 'r') as file:
+        # Skip the header lines
         for _ in range(number_of_header_lines):
             file.readline()
+        
         while True:
             line1 = file.readline()
             if not line1:
                 break
             elements = line1.split()
-            image_id.append(int(elements[0]))
-
-            qw, qx, qy, qz = (float(e) for e in elements[1:5])
-            t_cw_i = np.array([float(e) for e in elements[5:8]])
-            q_wc_i = R.from_quat([qx, qy, qz, qw]).inv()
-            q_wc_xyzw.append(q_wc_i.as_quat())
-            t_wc.append(-q_wc_i.as_matrix() @ t_cw_i)
+            
+            IMAGE_ID = int(elements[0])
+            image_id.append(IMAGE_ID)
+            
+            QW = float(elements[1])
+            QX = float(elements[2])
+            QY = float(elements[3])
+            QZ = float(elements[4])
+            
+            TX = float(elements[5])
+            TY = float(elements[6])
+            TZ = float(elements[7])
+
+            t_cw_i = np.array([TX, TY, TZ])
+            q_wc_i = R.from_quat([QX, QY, QZ, QW]).inv()
+            R_wc_i = q_wc_i.as_matrix()
+            
+            q_wc_xyzw.append([q_wc_i.as_quat()[0], q_wc_i.as_quat()[1], q_wc_i.as_quat()[2], q_wc_i.as_quat()[3]])
+            t_wc.append(-R_wc_i @ t_cw_i)
 
-            file.readline()  # POINTS2D line
+            file.readline()
+    
+    image_id = np.array(image_id)
+    q_wc_xyzw = np.array(q_wc_xyzw)
+    t_wc = np.array(t_wc)
 
-    image_id, q_wc_xyzw, t_wc = np.array(image_id), np.array(q_wc_xyzw), np.array(t_wc)
-    order = image_id.argsort()
-    image_id, q_wc_xyzw, t_wc = image_id[order], q_wc_xyzw[order], t_wc[order]
+    sorted_indices = image_id.argsort()
+    image_id = image_id[sorted_indices]
+    q_wc_xyzw = q_wc_xyzw[sorted_indices]
+    t_wc = t_wc[sorted_indices]
 
-    for i in range(1, len(q_wc_xyzw)):
-        if np.dot(q_wc_xyzw[i - 1], q_wc_xyzw[i]) < 0:
-            q_wc_xyzw[i] = -q_wc_xyzw[i]
-    return image_id, t_wc, q_wc_xyzw
+    q_wc_xyzw_corrected = q_wc_xyzw.copy()
+    for i in range(1, len(q_wc_xyzw_corrected)):
+        dot_product = np.dot(q_wc_xyzw_corrected[i - 1], q_wc_xyzw_corrected[i])
+        if dot_product < 0:
+            q_wc_xyzw_corrected[i] = -q_wc_xyzw_corrected[i]
 
+    return image_id, t_wc, q_wc_xyzw_corrected
 
-def write_trajectory_tum_format(file_name: str | Path, image_ts: np.ndarray, t_wc: np.ndarray, q_wc_xyzw: np.ndarray) -> None:
+def write_trajectory_tum_format(file_name, image_ts, t_wc, q_wc_xyzw):
     print(f"writeTrajectoryTUMformat: {file_name}")
+    
     data = np.hstack((image_ts.reshape(-1, 1), t_wc, q_wc_xyzw))
     data = data[data[:, 0].argsort()]
+
     with open(file_name, 'w', newline='') as file:
         file.write('ts (ns),tx (m),ty (m),tz (m),qx,qy,qz,qw\n')
         for row in data:
             file.write(','.join(f'{x:.15f}' for x in row) + '\n')
 
+def get_timestamps(files_path, rgb_file, camera_name):
+    print(f"getTimestamps: {os.path.join(files_path, rgb_file)}")
+    df = pd.read_csv(rgb_file)       
+    ts = df[f'ts_{camera_name} (ns)'].to_list()
+    return ts
+                
+if __name__ == "__main__":
 
-def get_timestamps(rgb_csv: str | Path, camera_name: str) -> list[float]:
-    print(f"getTimestamps: {rgb_csv}")
-    return pd.read_csv(rgb_csv)[f'ts_{camera_name} (ns)'].to_list()
-
+    sequence_path = sys.argv[1]
+    exp_folder = sys.argv[2]
+    exp_id = sys.argv[3]
+    verbose = bool(int(sys.argv[4]))
+    rgb_file = sys.argv[5]
+    camera_name = sys.argv[6]
 
-def colmap_to_vslamlab(exp_folder: str | Path, exp_id: str, rgb_csv: str | Path, camera_name: str) -> Path:
-    images_file = Path(exp_folder) / f'colmap_{exp_id}' / 'images.txt'
-    image_id, t_wc, q_wc_xyzw = get_colmap_keyframes(images_file)
+    images_file = os.path.join(exp_folder, f'colmap_{exp_id}', 'images.txt')
 
-    image_ts = np.array(get_timestamps(rgb_csv, camera_name))
-    timestamps = np.array([float(image_ts[i - 1]) for i in image_id])
+    number_of_header_lines = 4
+    image_id, t_wc, q_wc_xyzw = get_colmap_keyframes(images_file, number_of_header_lines, verbose)
 
-    trajectory_csv = Path(exp_folder) / f'{exp_id}_KeyFrameTrajectory.csv'
-    write_trajectory_tum_format(trajectory_csv, timestamps, t_wc, q_wc_xyzw)
-    return trajectory_csv
+    image_ts = np.array(get_timestamps(sequence_path, rgb_file, camera_name))
+    timestamps = []
+    for id in image_id:
+        timestamps.append(float(image_ts[id-1]))
 
+    timestamps = np.array(timestamps)
 
-if __name__ == "__main__":
-    # positional: sequence_path exp_folder exp_id verbose rgb_csv camera_name (sequence_path/verbose unused)
-    _, exp_folder_, exp_id_, _, rgb_csv_, camera_name_ = sys.argv[1:7]
-    colmap_to_vslamlab(exp_folder_, exp_id_, rgb_csv_, camera_name_)
+    keyFrameTrajectory_txt = os.path.join(exp_folder, exp_id + '_KeyFrameTrajectory' + '.csv')
+    write_trajectory_tum_format(keyFrameTrajectory_txt, timestamps, t_wc, q_wc_xyzw)

colmap_utilities.py

@@ -1,114 +0,0 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - colmap_utilities.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 1.0
-- Created: 2026-10-03
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-Helpers shared by the colmap pipeline stages (vslamlab_colmap.py, colmap_matcher.py,
-colmap_mapper.py): running colmap commands, GPU / thread detection, and mapping a VSLAM-LAB
-calibration onto a COLMAP camera model.
-"""
-
-import os
-import subprocess
-import sys
-from pathlib import Path
-
-import pandas as pd
-
-from get_calibration import get_camera_intrinsics
-
-COLMAP_DIR = Path(__file__).resolve().parent
-
-
-def run(cmd: list[str], capture: bool = False) -> str:
-    """Run a command, exiting with its return code if it fails. Returns the merged output when capture=True."""
-    if capture:
-        result = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True)
-    else:
-        result = subprocess.run(cmd)
-    if result.returncode != 0:
-        print(f"    ERROR: '{' '.join(cmd[:2])}' failed with return code {result.returncode}")
-        if capture:
-            print(result.stdout)
-        sys.exit(result.returncode)
-    return result.stdout if capture else ""
-
-
-def shell_output(cmd: list[str]) -> str:
-    """stdout+stderr of a command, '' if it cannot run (e.g. no nvidia-smi)."""
-    try:
-        return subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True).stdout
-    except FileNotFoundError as e:
-        return str(e)
-
-
-def indent(text: str, prefix: str) -> str:
-    return "\n".join(prefix + line for line in text.rstrip("\n").split("\n"))
-
-
-def detect_gpus(use_gpu: int) -> tuple[str, int]:
-    """GPU index list for COLMAP's gpu_index options and the CPU thread count.
-
-    Extraction / matching run as a single job across all usable GPUs, since COLMAP spawns one
-    worker per listed GPU index and splits the workload internally. When CUDA_VISIBLE_DEVICES is
-    set (a scheduler restricted this job to a device set, often MIG slice UUIDs on HPC that
-    nvidia-smi does not enumerate per job), CUDA remaps the listed devices to ordinals 0..N-1 inside
-    the process and COLMAP's gpu_index is just cudaSetDevice(ordinal), so the ordinals are used
-    rather than the real indices. The thread count comes from the scheduling affinity (what `nproc`
-    reports) rather than COLMAP's num_threads=-1 auto-detection, which on cgroup-limited nodes often
-    sees the whole node's cores.
-    """
-    print("    detecting GPUs ...")
-    print("        nvidia-smi -L:")
-    print(indent(shell_output(["nvidia-smi", "-L"]), " " * 12))
-    print("        nvidia-smi --query-gpu=index,name,uuid --format=csv:")
-    print(indent(shell_output(["nvidia-smi", "--query-gpu=index,name,uuid", "--format=csv"]), " " * 12))
-    cuda_visible_devices = os.environ.get("CUDA_VISIBLE_DEVICES", "")
-    print(f"        CUDA_VISIBLE_DEVICES: {cuda_visible_devices}")
-
-    if cuda_visible_devices:
-        num_gpus = len([d for d in cuda_visible_devices.split(",") if d.strip()])
-        gpu_ids = list(range(num_gpus))
-    else:
-        out = shell_output(["nvidia-smi", "--query-gpu=index", "--format=csv,noheader"])
-        gpu_ids = [int(tok) for tok in out.split() if tok.isdigit()]
-    if len(gpu_ids) < 1 or use_gpu == 0:
-        gpu_ids = [0]
-    gpu_index_list = ",".join(str(i) for i in gpu_ids)
-    print(f"        use_gpu: {use_gpu}")
-    print(f"        detected gpu_ids: {' '.join(str(i) for i in gpu_ids)}")
-    print(f"        gpu_index_list passed to colmap: {gpu_index_list}")
-
-    num_threads = len(os.sched_getaffinity(0)) if hasattr(os, "sched_getaffinity") else (os.cpu_count() or 1)
-    print(f"        num_threads: {num_threads}")
-    return gpu_index_list, num_threads
-
-
-def rgb_path_from_csv(sequence_path: Path, rgb_csv: Path, camera_name: str) -> Path:
-    """Frame folder of camera_name as the csv names it: the run pipeline may point path_rgb_0 at a
-    generated folder (refrax_0 for 'refraction: refrax') rather than rgb_0."""
-    df = pd.read_csv(rgb_csv, nrows=1)
-    rgb_dir = str(df[f"path_{camera_name}"].iloc[0]).split("/")[0]
-    return sequence_path / rgb_dir
-
-
-def colmap_camera(calibration_yaml: Path, camera_name: str) -> tuple[str, str, str]:
-    """(vslamlab calibration model, COLMAP camera model, comma-separated COLMAP params or '')."""
-    calibration_model, params = get_camera_intrinsics(calibration_yaml, camera_name)
-    params_csv = ",".join(str(p) for p in params)
-    if calibration_model == "unknown":
-        return calibration_model, "OPENCV", ""
-    if calibration_model == "pinhole":
-        return calibration_model, "PINHOLE", params_csv
-    if calibration_model == "radtan4":
-        return calibration_model, "OPENCV", params_csv
-    if calibration_model == "radtan5":
-        return calibration_model, "FULL_OPENCV", params_csv + ",0,0,0"
-    if calibration_model == "equid4":
-        return calibration_model, "OPENCV_FISHEYE", params_csv
-    print(f"Unknown calibration_model: {calibration_model}")
-    sys.exit(1)

create_colmap_mask_dir.py

@@ -1,61 +0,0 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - create_colmap_mask_dir.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5)
-- Version: 1.0
-- Created: 2026-08-28
-- Updated: 2026-08-28
-- License: GPLv3 License
-
-Turns the path_mask_<i> column of an rgb_exp.csv (written by the run pipeline for
-'segmentation: mask2former', 'refraction: refrax', or datasets that ship masks; 1 = usable pixel,
-0 = masked out, which is COLMAP's own convention: no features where the mask is 0) into what
-colmap feature_extractor accepts, and prints one line the calling shell script evals:
-  camera_mask:<png>   every frame shares one mask (refrax's mask.png) -> --ImageReader.camera_mask_path
-  mask_dir:<dir>      per-frame masks -> --ImageReader.mask_path: <dir>/<image name>.png symlinks,
-                      one per frame (COLMAP looks masks up by image name + '.png')
-  none                the csv has no mask column for this camera (or a mask file is missing)
-"""
-
-import argparse
-import os
-from pathlib import Path
-
-import pandas as pd
-
-
-def create_colmap_mask_dir(rgb_csv: str, camera_name: str, sequence_path: str, mask_dir: str) -> str:
-    df = pd.read_csv(rgb_csv)
-    cam_idx = camera_name.rsplit("_", 1)[-1]
-    mask_col, path_col = f"path_mask_{cam_idx}", f"path_{camera_name}"
-    if mask_col not in df.columns:
-        print(f"    no '{mask_col}' column in {os.path.basename(rgb_csv)}; extracting features without masks")
-        return "none"
-
-    masks = [Path(sequence_path) / p for p in df[mask_col]]
-    missing = [m for m in masks if not m.exists()]
-    if missing:
-        print(f"    {len(missing)}/{len(masks)} mask files missing (e.g. {missing[0]}); extracting features without masks")
-        return "none"
-
-    if len(set(masks)) == 1:
-        return f"camera_mask:{masks[0].resolve()}"
-
-    mask_dir = Path(mask_dir)
-    mask_dir.mkdir(parents=True, exist_ok=True)
-    for image, mask in zip(df[path_col], masks):
-        link = mask_dir / f"{os.path.basename(image)}.png"
-        if link.is_symlink() or link.exists():
-            link.unlink()
-        os.symlink(mask.resolve(), link)
-    return f"mask_dir:{mask_dir.resolve()}"
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("rgb_csv", help="Path to the experiment's rgb csv")
-    parser.add_argument("camera_name", help="camera_name (e.g. rgb_0)")
-    parser.add_argument("sequence_path", help="Sequence folder the csv paths are relative to")
-    parser.add_argument("mask_dir", help="Where to build the per-frame mask directory if needed")
-    args = parser.parse_args()
-    print(create_colmap_mask_dir(args.rgb_csv, args.camera_name, args.sequence_path, args.mask_dir))

get_calibration.py

@@ -1,45 +1,33 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - get_calibration.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 1.1
-- Created: 2024-07-12
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-Reads one camera of a VSLAM-LAB calibration yaml as (model, [fx, fy, cx, cy, *distortion]).
-The model is the distortion_type when the camera has distortion fields, else its cam_model
-(so 'pinhole' and 'unknown' come through unchanged). Importable (colmap_utilities.colmap_camera)
-and runnable: `python get_calibration.py <calibration_yaml> <camera_name>` prints the same
-fields space-separated on one line.
-"""
-
-import argparse
-from pathlib import Path
-
 import yaml
+import sys
+import argparse
+import numpy as np
 
-
-def get_camera_intrinsics(calibration_yaml: str | Path, cam_name: str) -> tuple[str, list[float]]:
+def get_camera_intrinsics(calibration_yaml, cam_name):
     with open(calibration_yaml, 'r') as file:
         data = yaml.safe_load(file)
-    cam = next(c for c in data.get('cameras', []) if c['cam_name'] == cam_name)
-
-    fx, fy = cam['focal_length'][0], cam['focal_length'][1]
-    cx, cy = cam['principal_point'][0], cam['principal_point'][1]
-    params = [float(fx), float(fy), float(cx), float(cy)]
-
+    cameras = data.get('cameras', [])
+    for cam_ in cameras:
+        if cam_['cam_name'] == cam_name:
+            cam = cam_;
+            break;
+  
     has_dist = ('distortion_type' in cam) and ('distortion_coefficients' in cam)
+    K = np.array([[cam['focal_length'][0], 0,  cam['principal_point'][0]],
+                  [0,  cam['focal_length'][1], cam['principal_point'][1]],
+                  [0,  0,   1]], dtype=np.float32)
+    
     if has_dist:
-        return cam['distortion_type'], params + [float(d) for d in cam['distortion_coefficients']]
-    return cam['cam_model'], params
-
-
+        dist= " ".join(map(str, cam['distortion_coefficients']))
+        print(f"{cam['distortion_type']} {K[0,0]} {K[1,1]} {K[0,2]} {K[1,2]} {dist}")
+    else:
+        print(f"{cam['cam_model']} {K[0,0]} {K[1,1]} {K[0,2]} {K[1,2]}")
+        
+    
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     parser.add_argument("calibration_yaml", help="Path to the calibration YAML")
     parser.add_argument("camera_name", help="camera_name")
     args = parser.parse_args()
-
-    model, params = get_camera_intrinsics(args.calibration_yaml, args.camera_name)
-    print(model, *params)
+    
+    get_camera_intrinsics(args.calibration_yaml, args.camera_name)

vslamlab_colmap.py

@@ -1,111 +0,0 @@
-"""
-Module: VSLAM-LAB - Baselines - colmap - vslamlab_colmap.py
-- Author: Alejandro Fontan Villacampa
-- Assisted by: Claude (Fable 5.1)
-- Version: 2.0
-- Created: 2024-07-12
-- Updated: 2026-10-03
-- License: GPLv3 License
-
-VSLAM-LAB entry point for the colmap baseline (Python port of colmap_reconstruction.sh), run by
-the pixi task execute-mono with the arguments BaselineVSLAMLAB.build_execute_command passes for
-command_style 'python' (--key value). Stages, each in its own module:
-  colmap_matcher.run_matcher      database, feature extraction (masks optional), exhaustive / sequential matching
-  colmap_mapper.run_mapper        COLMAP incremental or GLOMAP global mapping, best sub-model, TXT export
-  colmap_to_vslamlab              <exp_folder>/<exp_id>_KeyFrameTrajectory.csv from images.txt
-With verbose=1 the best sub-model is opened in the colmap gui afterwards.
-"""
-
-import argparse
-import os
-import shutil
-import sys
-from pathlib import Path
-
-sys.path.insert(0, str(Path(__file__).resolve().parent))
-
-from colmap_matcher import run_matcher  # noqa: E402
-from colmap_mapper import run_mapper  # noqa: E402
-from colmap_to_vslamlab import colmap_to_vslamlab  # noqa: E402
-from colmap_utilities import rgb_path_from_csv, run  # noqa: E402
-
-
-def parse_args() -> argparse.Namespace:
-    parser = argparse.ArgumentParser(description="VSLAM-LAB colmap baseline")
-    # Fixed per-run arguments (BaselineVSLAMLAB.build_execute_command)
-    parser.add_argument("--sequence_path", type=Path, required=True)
-    parser.add_argument("--calibration_yaml", type=Path, required=True)
-    parser.add_argument("--rgb_csv", type=Path, required=True)
-    parser.add_argument("--exp_folder", type=Path, required=True)
-    parser.add_argument("--exp_it", type=int, required=True)
-    parser.add_argument("--settings_yaml", type=Path, default=None)
-    # Baseline parameters (baseline_colmap.py default_parameters, overridable per experiment)
-    parser.add_argument("--verbose", type=int, default=0)
-    parser.add_argument("--mode", type=str, default="mono")
-    parser.add_argument("--matcher_type", type=str, default="exhaustive", choices=["exhaustive", "sequential"])
-    parser.add_argument("--matching_type", type=str, default="sift_bruteforce")
-    parser.add_argument("--mapper_type", type=str, default="colmap", choices=["colmap", "glomap"])
-    parser.add_argument("--rgb_max", type=int, default=None, help="consumed by the run pipeline; accepted here because it is forwarded")
-    parser.add_argument("--use_mask", type=int, default=0)
-    parser.add_argument("--optimize_intrinsics", type=int, default=1)
-    parser.add_argument("--dense", type=int, default=0)
-    parser.add_argument("--dense_max_image_size", type=int, default=1600)
-    parser.add_argument("--mesher", type=str, default="none")
-    # Not exposed as baseline parameters
-    parser.add_argument("--use_gpu", type=int, default=1)
-    parser.add_argument("--camera_name", type=str, default="rgb_0")
-    return parser.parse_args()
-
-
-def main() -> None:
-    args = parse_args()
-    exp_id = f"{args.exp_it:05d}"
-
-    print("\n================= Experiment Configuration =================")
-    print(f"  Sequence Path     : {args.sequence_path}")
-    print(f"  Experiment Folder : {args.exp_folder}")
-    print(f"  Experiment ID     : {exp_id}")
-    print(f"  Verbose           : {args.verbose}")
-    print(f"  Matcher Type      : {args.matcher_type}")
-    print(f"  Matching Type     : {args.matching_type}")
-    print(f"  Mapper Type       : {args.mapper_type}")
-    print(f"  Use GPU           : {args.use_gpu}")
-    print(f"  Use Mask          : {args.use_mask}")
-    print(f"  Optimize Intrins. : {args.optimize_intrinsics}")
-    print(f"  Dense             : {args.dense}")
-    print(f"  Dense Max Img Size: {args.dense_max_image_size}")
-    print(f"  Mesher            : {args.mesher}")
-    print(f"  Settings YAML     : {args.settings_yaml}")
-    print(f"  Calibration YAML  : {args.calibration_yaml}")
-    print(f"  RGB CSV           : {args.rgb_csv}")
-    print(f"  Camera Name       : {args.camera_name}")
-    print("============================================================")
-
-    # Folder for the colmap files of this run
-    exp_folder_colmap = args.exp_folder / f"colmap_{exp_id}"
-    shutil.rmtree(exp_folder_colmap, ignore_errors=True)
-    exp_folder_colmap.mkdir(parents=True)
-
-    conda_prefix = os.environ.get("CONDA_PREFIX")
-    if conda_prefix:
-        os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = f"{conda_prefix}/plugins/platforms"
-
-    rgb_path = rgb_path_from_csv(args.sequence_path, args.rgb_csv, args.camera_name)
-
-    database = run_matcher(args.sequence_path, exp_folder_colmap, rgb_path, args.rgb_csv, args.calibration_yaml, args.camera_name,
-                           args.matcher_type, args.matching_type, args.use_gpu, args.use_mask)
-    best_model = run_mapper(exp_folder_colmap, rgb_path, args.calibration_yaml, args.camera_name, args.mapper_type, args.optimize_intrinsics)
-
-    # Convert COLMAP outputs to a format suitable for VSLAM-LAB
-    colmap_to_vslamlab(args.exp_folder, exp_id, args.rgb_csv, args.camera_name)
-
-    if args.dense == 1:
-        print(f"\n    WARNING: dense={args.dense} (mesher={args.mesher}) requested, but the dense stage is not wired yet; skipping")
-
-    # Visualization with colmap gui
-    if args.verbose == 1:
-        run(["colmap", "gui", "--import_path", str(best_model), "--database_path", str(database), "--image_path", str(rgb_path)])
-
-
-if __name__ == "__main__":
-    main()