#!/bin/bash
echo "Executing glomapMapper.sh ..."

sequence_path="$1" 
exp_folder="$2" 
exp_id="$3" 
settings_yaml="$4"
calibration_yaml="$5"
rgb_csv="$6"


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}"

calibration_model=$(grep -oP '(?<=Camera0\.model:\s)[\w]+' "$calibration_yaml")
echo "        camera model : $calibration_model"
optimize_intrinsics="0"
skip_view_graph_calibration="1"
if [ "${calibration_model}" == "UNKNOWN" ]
then
  optimize_intrinsics="1"
  skip_view_graph_calibration="0"
fi

# Reading settings from yaml file
ba_iteration_num=$(yq '.ba.iteration_num // 3' $settings_yaml)
retriangulation_iteration_num=$(yq '.retriangulation.iteration_num // 1' $settings_yaml)

skip_preprocessing=$(yq '.skip.preprocessing // 0' $settings_yaml)
skip_view_graph_calibration=$(yq '.skip.view_graph_calibration // 0' $settings_yaml)
skip_relative_pose_estimation=$(yq '.skip.relative_pose_estimation // 0' $settings_yaml)
skip_rotation_averaging=$(yq '.skip.rotation_averaging // 0' $settings_yaml)
skip_global_positioning=$(yq '.skip.global_positioning // 0' $settings_yaml)
skip_bundle_adjustment=$(yq '.skip.bundle_adjustment // 0' $settings_yaml)
skip_retriangulation_stage=$(yq '.skip.retriangulation_stage // 0' $settings_yaml)
skip_pruning=$(yq '.skip.pruning // 0' $settings_yaml)

ViewGraphCalib_thres_lower_ratio=$(yq '.ViewGraphCalib.thres_lower_ratio // 0.1' $settings_yaml)
ViewGraphCalib_thres_higher_ratio=$(yq '.ViewGraphCalib.thres_higher_ratio // 10.0' $settings_yaml)
ViewGraphCalib_thres_two_view_error=$(yq '.ViewGraphCalib.thres_two_view_error // 2.0' $settings_yaml)

RelPoseEstimation_max_epipolar_error=$(yq '.RelPoseEstimation.max_epipolar_error // 1.0' $settings_yaml)

TrackEstablishment_min_num_tracks_per_view=$(yq '.TrackEstablishment.min_num_tracks_per_view // -1' $settings_yaml)
TrackEstablishment_min_num_view_per_track=$(yq '.TrackEstablishment.min_num_view_per_track // 3' $settings_yaml)
TrackEstablishment_max_num_view_per_track=$(yq '.TrackEstablishment.max_num_view_per_track // 100' $settings_yaml)
TrackEstablishment_max_num_tracks=$(yq '.TrackEstablishment.max_num_tracks // 10000000' $settings_yaml)

GlobalPositioning_optimize_positions=$(yq '.GlobalPositioning.optimize_positions // 1' $settings_yaml)
GlobalPositioning_optimize_points=$(yq '.GlobalPositioning.optimize_points // 1' $settings_yaml)
GlobalPositioning_optimize_scales=$(yq '.GlobalPositioning.optimize_scales // 1' $settings_yaml)
GlobalPositioning_thres_loss_function=$(yq '.GlobalPositioning.thres_loss_function // 0.1' $settings_yaml)
GlobalPositioning_max_num_iterations=$(yq '.GlobalPositioning.max_num_iterations // 100' $settings_yaml)

BundleAdjustment_optimize_rotations=$(yq '.BundleAdjustment.optimize_rotations // 1' $settings_yaml)
BundleAdjustment_optimize_translation=$(yq '.BundleAdjustment.optimize_translation // 1' $settings_yaml)
BundleAdjustment_optimize_intrinsics=$(yq '.BundleAdjustment.optimize_intrinsics // 1' $settings_yaml)
BundleAdjustment_optimize_points=$(yq '.BundleAdjustment.optimize_points // 1' $settings_yaml)
BundleAdjustment_thres_loss_function=$(yq '.BundleAdjustment.thres_loss_function // 1.0' $settings_yaml)
BundleAdjustment_max_num_iterations=$(yq '.BundleAdjustment.max_num_iterations // 200' $settings_yaml)

Triangulation_complete_max_reproj_error=$(yq '.Triangulation.complete_max_reproj_error // 15.0' $settings_yaml)
Triangulation_merge_max_reproj_error=$(yq '.Triangulation.merge_max_reproj_error // 15.0' $settings_yaml)
Triangulation_min_angle=$(yq '.Triangulation.min_angle // 1.0' $settings_yaml)
Triangulation_min_num_matches=$(yq '.Triangulation.min_num_matches // 15' $settings_yaml)

Thresholds_max_epipolar_error_E=$(yq '.Thresholds.max_epipolar_error_E // 1.0' $settings_yaml)
Thresholds_max_epipolar_error_F=$(yq '.Thresholds.max_epipolar_error_F // 4.0' $settings_yaml)
Thresholds_max_epipolar_error_H=$(yq '.Thresholds.max_epipolar_error_H // 4.0' $settings_yaml)
Thresholds_min_inlier_num=$(yq '.Thresholds.min_inlier_num // 30.0' $settings_yaml)
Thresholds_min_inlier_ratio=$(yq '.Thresholds.min_inlier_ratio // 0.25' $settings_yaml)
Thresholds_max_rotation_error=$(yq '.Thresholds.max_rotation_error // 10.0' $settings_yaml)

echo "    glomap mapper ..."
database="${exp_folder_colmap}/colmap_database.db"

glomap mapper \
    --database_path "${database}" \
    --image_path "${rgb_path}" \
    --output_path "${exp_folder_colmap}" #\
    # --ba_iteration_num "${ba_iteration_num}" \
    # --retriangulation_iteration_num "${retriangulation_iteration_num}" \
    # --skip_preprocessing "${skip_preprocessing}" \
    # --skip_view_graph_calibration "${skip_view_graph_calibration}" \
    # --skip_relative_pose_estimation "${skip_relative_pose_estimation}" \
    # --skip_rotation_averaging "${skip_rotation_averaging}" \
    # --skip_global_positioning "${skip_global_positioning}" \
    # --skip_bundle_adjustment "${skip_bundle_adjustment}" \
    # --skip_retriangulation "${skip_retriangulation_stage}" \
    # --skip_pruning "${skip_pruning}" \
    # --ViewGraphCalib.thres_lower_ratio "${ViewGraphCalib_thres_lower_ratio}" \
    # --ViewGraphCalib.thres_higher_ratio "${ViewGraphCalib_thres_higher_ratio}" \
    # --ViewGraphCalib.thres_two_view_error "${ViewGraphCalib_thres_two_view_error}" \
    # --RelPoseEstimation.max_epipolar_error "${RelPoseEstimation_max_epipolar_error}" \
    # --TrackEstablishment.min_num_tracks_per_view "${TrackEstablishment_min_num_tracks_per_view}" \
    # --TrackEstablishment.min_num_view_per_track "${TrackEstablishment_min_num_view_per_track}" \
    # --TrackEstablishment.max_num_view_per_track "${TrackEstablishment_max_num_view_per_track}" \
    # --TrackEstablishment.max_num_tracks "${TrackEstablishment_max_num_tracks}" \
    # --GlobalPositioning.optimize_positions "${GlobalPositioning_optimize_positions}" \
    # --GlobalPositioning.optimize_points "${GlobalPositioning_optimize_points}" \
    # --GlobalPositioning.optimize_scales "${GlobalPositioning_optimize_scales}" \
    # --GlobalPositioning.thres_loss_function "${GlobalPositioning_thres_loss_function}" \
    # --GlobalPositioning.max_num_iterations "${GlobalPositioning_max_num_iterations}" \
    # --BundleAdjustment.optimize_rotations "${BundleAdjustment_optimize_rotations}" \
    # --BundleAdjustment.optimize_translation "${BundleAdjustment_optimize_translation}" \
    # --BundleAdjustment.optimize_intrinsics "${BundleAdjustment_optimize_intrinsics}" \
    # --BundleAdjustment.optimize_points "${BundleAdjustment_optimize_points}" \
    # --BundleAdjustment.thres_loss_function "${BundleAdjustment_thres_loss_function}" \
    # --BundleAdjustment.max_num_iterations "${BundleAdjustment_max_num_iterations}" \
    # --Triangulation.complete_max_reproj_error "${Triangulation_complete_max_reproj_error}" \
    # --Triangulation.merge_max_reproj_error "${Triangulation_merge_max_reproj_error}" \
    # --Triangulation.min_angle "${Triangulation_min_angle}" \
    # --Triangulation.min_num_matches "${Triangulation_min_num_matches}" \
    # --Thresholds.max_epipolar_error_E "${Thresholds_max_epipolar_error_E}" \
    # --Thresholds.max_epipolar_error_F "${Thresholds_max_epipolar_error_F}" \
    # --Thresholds.max_epipolar_error_H "${Thresholds_max_epipolar_error_H}" \
    # --Thresholds.min_inlier_num "${Thresholds_min_inlier_num}" \
    # --Thresholds.min_inlier_ratio "${Thresholds_min_inlier_ratio}" \
    # --Thresholds.max_rotation_error "${Thresholds_max_rotation_error}"

# colmap image_undistorter \
#   --image_path "${rgb_path}" \
#   --input_path "${exp_folder_colmap}/0" \
#   --output_path "${exp_folder_colmap}" \
#   --output_type COLMAP \
#   --max_image_size 2000

# colmap patch_match_stereo \
#   --workspace_path "${exp_folder_colmap}" \
#   --workspace_format COLMAP \
#   --PatchMatchStereo.cache_size 64 \
#   --PatchMatchStereo.filter true \
#   --PatchMatchStereo.window_step 2 \
#   --PatchMatchStereo.geom_consistency true
    
echo "    colmap model_converter ..."
colmap model_converter \
	--input_path ${exp_folder_colmap}/0 --output_path ${exp_folder_colmap} --output_type TXT