update: first auxiliary files

.gitignore

@@ -0,0 +1,3 @@
+vocab_tree_flickr100K_words1M.bin
+vocab_tree_flickr100K_words256K.bin
+vocab_tree_flickr100K_words32K.bin

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.sh

@@ -0,0 +1,144 @@
+#!/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 1 \
+	--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_flickr100K_words32K.bin"
+  if [ "$num_rgb" -gt 1000 ]; then
+    vocabulary_tree="Baselines/colmap/vocab_tree_flickr100K_words256K.bin"
+  fi
+  if [ "$num_rgb" -gt 10000 ]; then
+    vocabulary_tree="Baselines/colmap/vocab_tree_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

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

@@ -0,0 +1,102 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+import sys
+import os
+import pandas as pd
+
+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 = []
+
+    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 = 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()
+    
+    image_id = np.array(image_id)
+    q_wc_xyzw = np.array(q_wc_xyzw)
+    t_wc = np.array(t_wc)
+
+    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]
+
+    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, 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__":
+
+    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]
+
+    images_file = os.path.join(exp_folder, f'colmap_{exp_id}', 'images.txt')
+
+    number_of_header_lines = 4
+    image_id, t_wc, q_wc_xyzw = get_colmap_keyframes(images_file, number_of_header_lines, verbose)
+
+    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)
+
+    keyFrameTrajectory_txt = os.path.join(exp_folder, exp_id + '_KeyFrameTrajectory' + '.csv')
+    write_trajectory_tum_format(keyFrameTrajectory_txt, timestamps, t_wc, q_wc_xyzw)

create_colmap_image_list.py

@@ -0,0 +1,21 @@
+import argparse
+import pandas as pd
+import os
+def create_colmap_image_list(rgb_csv, colmap_image_list_txt, cam_name):
+
+    df = pd.read_csv(rgb_csv)       
+    image_list = df[f'path_{cam_name}'].to_list()
+
+    with open(colmap_image_list_txt, 'w') as f:
+        for name in image_list:
+            file_name = os.path.basename(name)
+            f.write(f"{file_name}\n")
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("rgb_csv", help="Path to the rgb_csv")
+    parser.add_argument("colmap_image_list", help="Path to the colmap_image_list")
+    parser.add_argument("camera_name", help="camera_name")
+
+    args = parser.parse_args()
+    create_colmap_image_list(args.rgb_csv, args.colmap_image_list, args.camera_name)

get_calibration.py

@@ -0,0 +1,33 @@
+import yaml
+import sys
+import argparse
+import numpy as np
+
+def get_camera_intrinsics(calibration_yaml, cam_name):
+    with open(calibration_yaml, 'r') as file:
+        data = yaml.safe_load(file)
+    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:
+        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()
+    
+    get_camera_intrinsics(args.calibration_yaml, args.camera_name)

vslamlab_colmap_settings.yaml

@@ -0,0 +1,79 @@
+feature_extractor:
+  SiftExtraction_num_octaves: 4
+  SiftExtraction_octave_resolution: 3
+  SiftExtraction_peak_threshold: 0.0066666666666666671
+  SiftExtraction_edge_threshold: 10.0
+  SiftExtraction_dsp_min_scale: 0.1666666666666666
+  SiftExtraction_dsp_max_scale: 3.0
+  SiftExtraction_dsp_num_scales: 10
+
+matcher:
+  SiftMatching_max_ratio: 0.80000000000000004
+  SiftMatching_max_distance: 0.69999999999999996
+  TwoViewGeometry_min_num_inliers: 15
+  TwoViewGeometry_max_error: 4.0
+  TwoViewGeometry_confidence: 0.999
+  TwoViewGeometry_min_inlier_ratio: 0.25
+  SequentialMatching_overlap: 10
+  SequentialMatching_quadratic_overlap: 1 
+  ExhaustiveMatching_block_size: 50
+
+mapper:
+  Mapper_min_num_matches: 15
+  Mapper_ignore_watermarks: 0
+  Mapper_multiple_models: 1
+  Mapper_max_num_models: 50
+  Mapper_max_model_overlap: 20
+  Mapper_min_model_size: 10
+  Mapper_init_image_id1: -1
+  Mapper_init_image_id2: -1
+  Mapper_init_num_trials: 200
+  Mapper_extract_colors: 1
+  Mapper_num_threads: -1
+  Mapper_min_focal_length_ratio: 0.10000000000000001
+  Mapper_max_focal_length_ratio: 10
+  Mapper_max_extra_param: 1
+  Mapper_ba_refine_focal_length: 1
+  Mapper_ba_refine_principal_point: 0
+  Mapper_ba_refine_extra_params: 1
+  Mapper_ba_local_num_images: 6
+  Mapper_ba_local_function_tolerance: 0
+  Mapper_ba_local_max_num_iterations: 25
+  Mapper_ba_global_images_ratio: 1.1000000000000001
+  Mapper_ba_global_points_ratio: 1.1000000000000001
+  Mapper_ba_global_images_freq: 500
+  Mapper_ba_global_points_freq: 250000
+  Mapper_ba_global_function_tolerance: 0
+  Mapper_ba_global_max_num_iterations: 50
+  Mapper_ba_global_max_refinements: 5
+  Mapper_ba_global_max_refinement_change: 0.00050000000000000001
+  Mapper_ba_local_max_refinements: 2
+  Mapper_ba_local_max_refinement_change: 0.001
+  Mapper_ba_use_gpu: 0
+  Mapper_ba_gpu_index: -1
+  Mapper_ba_min_num_residuals_for_cpu_multi_threading: 50000
+  Mapper_snapshot_images_freq: 0
+  Mapper_fix_existing_images: 0
+  Mapper_init_min_num_inliers: 100
+  Mapper_init_max_error: 4
+  Mapper_init_max_forward_motion: 0.94999999999999996
+  Mapper_init_min_tri_angle: 16
+  Mapper_init_max_reg_trials: 2
+  Mapper_abs_pose_max_error: 12
+  Mapper_abs_pose_min_num_inliers: 30
+  Mapper_abs_pose_min_inlier_ratio: 0.25
+  Mapper_filter_max_reproj_error: 4
+  Mapper_filter_min_tri_angle: 1.5
+  Mapper_max_reg_trials: 3
+  Mapper_local_ba_min_tri_angle: 6
+  Mapper_tri_max_transitivity: 1
+  Mapper_tri_create_max_angle_error: 2
+  Mapper_tri_continue_max_angle_error: 2
+  Mapper_tri_merge_max_reproj_error: 4
+  Mapper_tri_complete_max_reproj_error: 4
+  Mapper_tri_complete_max_transitivity: 5
+  Mapper_tri_re_max_angle_error: 5
+  Mapper_tri_re_min_ratio: 0.20000000000000001
+  Mapper_tri_re_max_trials: 1
+  Mapper_tri_min_angle: 1.5
+  Mapper_tri_ignore_two_view_tracks: 1