vslamlab integration

colmap_mapper.sh

@@ -6,12 +6,13 @@ exp_folder="$2"
 exp_id="$3" 
 settings_yaml="$4"
 calibration_yaml="$5"
-rgb_txt="$6"
+rgb_csv="$6"
 
 exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
-rgb_path="${sequence_path}/$(awk '{print $2}' "${rgb_txt}" | awk -F'/' 'NR==1 {print $1}')"
+rgb_dir=$(awk -F, 'NR==2 { split($2,a,"/"); print a[1]; exit }' "$rgb_csv")
+rgb_path="${sequence_path}/${rgb_dir}"
 
-calibration_model=$(grep -oP '(?<=Camera\.model:\s)[\w]+' "$calibration_yaml")
+calibration_model=$(grep -oP '(?<=Camera0\.model:\s)[\w]+' "$calibration_yaml")
 echo "        camera model : $calibration_model"
 
 # Reading settings from yaml file

colmap_matcher.sh

@@ -7,28 +7,20 @@ exp_folder="$2"
 exp_id="$3" 
 settings_yaml="$4"
 calibration_yaml="$5"
-rgb_txt="$6"
+rgb_csv="$6"
 matcher_type="$7"
 use_gpu="$8"
 
 exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
-rgb_path="${sequence_path}/$(awk '{print $2}' "${rgb_txt}" | awk -F'/' 'NR==1 {print $1}')"
+rgb_dir=$(awk -F, 'NR==2 { split($2,a,"/"); print a[1]; exit }' "$rgb_csv")
+rgb_path="${sequence_path}/${rgb_dir}"
 
-calibration_model=$(grep -oP '(?<=Camera\.model:\s)[\w]+' "$calibration_yaml")
+calibration_model=$(grep -oP '(?<=Camera0\.model:\s)[\w]+' "$calibration_yaml")
 
-fx=$(grep -oP '(?<=Camera\.fx:\s)-?\d+\.\d+' "$calibration_yaml")
-fy=$(grep -oP '(?<=Camera\.fy:\s)-?\d+\.\d+' "$calibration_yaml")
-cx=$(grep -oP '(?<=Camera\.cx:\s)-?\d+\.\d+' "$calibration_yaml")
-cy=$(grep -oP '(?<=Camera\.cy:\s)-?\d+\.\d+' "$calibration_yaml")
-
-k1=$(grep -oP '(?<=Camera\.k1:\s)-?\d+\.\d+' "$calibration_yaml")
-k2=$(grep -oP '(?<=Camera\.k2:\s)-?\d+\.\d+' "$calibration_yaml")
-p1=$(grep -oP '(?<=Camera\.p1:\s)-?\d+\.\d+' "$calibration_yaml")
-p2=$(grep -oP '(?<=Camera\.p2:\s)-?\d+\.\d+' "$calibration_yaml")
-k3=$(grep -oP '(?<=Camera\.k3:\s)-?\d+\.\d+' "$calibration_yaml")
-k4=0.0
-k5=0.0
-k6=0.0
+fx=$(grep -oP '(?<=Camera0\.fx:\s)-?\d+\.\d+' "$calibration_yaml")
+fy=$(grep -oP '(?<=Camera0\.fy:\s)-?\d+\.\d+' "$calibration_yaml")
+cx=$(grep -oP '(?<=Camera0\.cx:\s)-?\d+\.\d+' "$calibration_yaml")
+cy=$(grep -oP '(?<=Camera0\.cy:\s)-?\d+\.\d+' "$calibration_yaml")
 
 # Reading settings from yaml file
 feature_extractor_SiftExtraction_num_octaves=$(yq '.feature_extractor.SiftExtraction_num_octaves // 4.0' $settings_yaml)
@@ -51,7 +43,7 @@ matcher_ExhaustiveMatching_block_size=$(yq '.matcher.ExhaustiveMatching_block_si
 
 # Create colmap image list
 colmap_image_list="${exp_folder_colmap}/colmap_image_list.txt"
-awk '{split($2, arr, "/"); print arr[2]}' "$rgb_txt" > "$colmap_image_list"
+awk -F, 'NR>1 { split($2,a,"/"); print a[2] }' "$rgb_csv" > "$colmap_image_list"
 
 # Create Colmap Database
 database="${exp_folder_colmap}/colmap_database.db"
@@ -81,14 +73,14 @@ colmap feature_extractor \
    --SiftExtraction.dsp_num_scales ${feature_extractor_SiftExtraction_dsp_num_scales}
 fi
 
-if [ "${calibration_model}" == "PINHOLE" ]
+if [ "${calibration_model}" == "Pinhole" ]
 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 ${calibration_model} \
+	--ImageReader.camera_model PINHOLE \
 	--ImageReader.single_camera 1 \
 	--ImageReader.single_camera_per_folder 1 \
 	--SiftExtraction.use_gpu ${use_gpu} \
@@ -104,6 +96,13 @@ fi
 
 if [ "${calibration_model}" == "OPENCV" ]
 then
+
+  k1=$(grep -oP '(?<=Camera0\.k1:\s)-?\d+\.\d+' "$calibration_yaml")
+  k2=$(grep -oP '(?<=Camera0\.k2:\s)-?\d+\.\d+' "$calibration_yaml")
+  p1=$(grep -oP '(?<=Camera0\.p1:\s)-?\d+\.\d+' "$calibration_yaml")
+  p2=$(grep -oP '(?<=Camera0\.p2:\s)-?\d+\.\d+' "$calibration_yaml")
+  k3=$(grep -oP '(?<=Camera0\.k3:\s)-?\d+\.\d+' "$calibration_yaml")
+
   echo "        camera model : $calibration_model"
   colmap feature_extractor \
 	--database_path ${database} \
@@ -125,6 +124,12 @@ fi
 
 if [ "${calibration_model}" == "OPENCV_FISHEYE" ] 
 then
+
+  k1=$(grep -oP '(?<=Camera0\.k1:\s)-?\d+\.\d+' "$calibration_yaml")
+  k2=$(grep -oP '(?<=Camera0\.k2:\s)-?\d+\.\d+' "$calibration_yaml")
+  k3=$(grep -oP '(?<=Camera0\.k3:\s)-?\d+\.\d+' "$calibration_yaml")
+  k4=$(grep -oP '(?<=Camera0\.k4:\s)-?\d+\.\d+' "$calibration_yaml")
+
   echo "        camera model : $calibration_model"
 	colmap feature_extractor \
 	--database_path ${database} \
@@ -164,7 +169,7 @@ fi
 # Sequential Feature Matcher
 if [ "${matcher_type}" == "sequential" ]
 then
-  num_rgb=$(wc -l < ${rgb_txt})
+  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"

colmap_reconstruction.sh

@@ -9,7 +9,7 @@ sequence_path=""
 exp_folder=""
 exp_id=""
 calibration_yaml=""
-rgb_txt=""
+rgb_csv=""
 
 # Function to split key-value pairs and assign them to variables
 split_and_assign() {
@@ -35,7 +35,7 @@ echo "  Matcher Type      : $matcher_type"
 echo "  Use GPU           : $use_gpu"
 echo "  Settings YAML     : $settings_yaml"
 echo "  Calibration YAML  : $calibration_yaml"
-echo "  RGB TXT           : $rgb_txt"
+echo "  RGB CSV           : $rgb_csv"
 echo "============================================================"
 
 # Create folder to save colmap files
@@ -45,17 +45,18 @@ 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_txt"
+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
 ./Baselines/colmap/colmap_mapper.sh $colmap_args 
 
 # 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_txt
+python Baselines/colmap/colmap_to_vslamlab.py $sequence_path $exp_folder $exp_id $verbose $rgb_csv
 
 # Visualization with colmap gui
 if [ "$verbose" -eq 1 ]; then
   exp_folder_colmap="${exp_folder}/colmap_${exp_id}"
-  rgb_path="${sequence_path}/$(awk '{print $2}' "${rgb_txt}" | awk -F'/' 'NR==1 {print $1}')"
+  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

colmap_to_vslamlab.py

@@ -2,6 +2,7 @@ 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}")
@@ -65,21 +66,15 @@ def write_trajectory_tum_format(file_name, image_ts, t_wc, q_wc_xyzw):
     data = np.hstack((image_ts.reshape(-1, 1), t_wc, q_wc_xyzw))
     data = data[data[:, 0].argsort()]
 
-    with open(file_name, 'w') as file:
+    with open(file_name, 'w', newline='') as file:
+        file.write('timestamp,tx,ty,tz,qx,qy,qz,qw\n')
         for row in data:
-            # Format the row with the appropriate precision
-            file.write(' '.join(f'{x:.15f}' for x in row) + '\n')
+            file.write(','.join(f'{x:.15f}' for x in row) + '\n')
 
 def get_timestamps(files_path, rgb_file):
     print(f"getTimestamps: {os.path.join(files_path, rgb_file)}")
-    
-    ts = []
-    
-    with open(os.path.join(files_path, rgb_file), 'r') as file:
-        for line in file:
-            parts = line.split()
-            ts.append(float(parts[0]))  # Assuming the first part is the timestamp
-    
+    df = pd.read_csv(rgb_file)       
+    ts = df['ts_rgb0 (s)'].to_list()
     return ts
                 
 if __name__ == "__main__":
@@ -102,5 +97,5 @@ if __name__ == "__main__":
 
     timestamps = np.array(timestamps)
 
-    keyFrameTrajectory_txt = os.path.join(exp_folder, exp_id + '_KeyFrameTrajectory' + '.txt')
+    keyFrameTrajectory_txt = os.path.join(exp_folder, exp_id + '_KeyFrameTrajectory' + '.csv')
     write_trajectory_tum_format(keyFrameTrajectory_txt, timestamps, t_wc, q_wc_xyzw)