#!/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"

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"

# Reading settings from yaml file
mapper_Mapper_min_num_matches=$(yq '.mapper.Mapper_min_num_matches // 15' $settings_yaml)
mapper_Mapper_ignore_watermarks=$(yq '.mapper.Mapper_ignore_watermarks // 0' $settings_yaml)
mapper_Mapper_multiple_models=$(yq '.mapper.Mapper_multiple_models // 1' $settings_yaml)
mapper_Mapper_max_num_models=$(yq '.mapper.Mapper_max_num_models // 50' $settings_yaml)
mapper_Mapper_max_model_overlap=$(yq '.mapper.Mapper_max_model_overlap // 20' $settings_yaml)
mapper_Mapper_min_model_size=$(yq '.mapper.Mapper_min_model_size // 10' $settings_yaml)
mapper_Mapper_init_image_id1=$(yq '.mapper.Mapper_init_image_id1 // -1' $settings_yaml)
mapper_Mapper_init_image_id2=$(yq '.mapper.Mapper_init_image_id2 // -1' $settings_yaml)
mapper_Mapper_init_num_trials=$(yq '.mapper.Mapper_init_num_trials // 200' $settings_yaml)
mapper_Mapper_extract_colors=$(yq '.mapper.Mapper_extract_colors // 1' $settings_yaml)
mapper_Mapper_num_threads=$(yq '.mapper.Mapper_num_threads // -1' $settings_yaml)
mapper_Mapper_min_focal_length_ratio=$(yq '.mapper.Mapper_min_focal_length_ratio // 0.1' $settings_yaml)
mapper_Mapper_max_focal_length_ratio=$(yq '.mapper.Mapper_max_focal_length_ratio // 10' $settings_yaml)
mapper_Mapper_max_extra_param=$(yq '.mapper.Mapper_max_extra_param // 1' $settings_yaml)
mapper_Mapper_ba_refine_focal_length=$(yq '.mapper.Mapper_ba_refine_focal_length // 1' $settings_yaml)
mapper_Mapper_ba_refine_principal_point=$(yq '.mapper.Mapper_ba_refine_principal_point // 0' $settings_yaml)
mapper_Mapper_ba_refine_extra_params=$(yq '.mapper.Mapper_ba_refine_extra_params // 1' $settings_yaml)
mapper_Mapper_ba_local_num_images=$(yq '.mapper.Mapper_ba_local_num_images // 6' $settings_yaml)
mapper_Mapper_ba_local_function_tolerance=$(yq '.mapper.Mapper_ba_local_function_tolerance // 0' $settings_yaml)
mapper_Mapper_ba_local_max_num_iterations=$(yq '.mapper.Mapper_ba_local_max_num_iterations // 25' $settings_yaml)
mapper_Mapper_ba_global_images_ratio=$(yq '.mapper.Mapper_ba_global_images_ratio // 1.1' $settings_yaml)
mapper_Mapper_ba_global_points_ratio=$(yq '.mapper.Mapper_ba_global_points_ratio // 1.1' $settings_yaml)
mapper_Mapper_ba_global_images_freq=$(yq '.mapper.Mapper_ba_global_images_freq // 500' $settings_yaml)
mapper_Mapper_ba_global_points_freq=$(yq '.mapper.Mapper_ba_global_points_freq // 250000' $settings_yaml)
mapper_Mapper_ba_global_function_tolerance=$(yq '.mapper.Mapper_ba_global_function_tolerance // 0' $settings_yaml)
mapper_Mapper_ba_global_max_num_iterations=$(yq '.mapper.Mapper_ba_global_max_num_iterations // 50' $settings_yaml)
mapper_Mapper_ba_global_max_refinements=$(yq '.mapper.Mapper_ba_global_max_refinements // 5' $settings_yaml)
mapper_Mapper_ba_global_max_refinement_change=$(yq '.mapper.Mapper_ba_global_max_refinement_change // 0.0005' $settings_yaml)
mapper_Mapper_ba_local_max_refinements=$(yq '.mapper.Mapper_ba_local_max_refinements // 2' $settings_yaml)
mapper_Mapper_ba_local_max_refinement_change=$(yq '.mapper.Mapper_ba_local_max_refinement_change // 0.001' $settings_yaml)
mapper_Mapper_ba_use_gpu=$(yq '.mapper.Mapper_ba_use_gpu // 0' $settings_yaml)
mapper_Mapper_ba_gpu_index=$(yq '.mapper.Mapper_ba_gpu_index // -1' $settings_yaml)
mapper_Mapper_ba_min_num_residuals_for_cpu_multi_threading=$(yq '.mapper.Mapper_ba_min_num_residuals_for_cpu_multi_threading // 50000' $settings_yaml)
mapper_Mapper_snapshot_images_freq=$(yq '.mapper.Mapper_snapshot_images_freq // 0' $settings_yaml)
mapper_Mapper_fix_existing_images=$(yq '.mapper.Mapper_fix_existing_images // 0' $settings_yaml)
mapper_Mapper_init_min_num_inliers=$(yq '.mapper.Mapper_init_min_num_inliers // 100' $settings_yaml)
mapper_Mapper_init_max_error=$(yq '.mapper.Mapper_init_max_error // 4' $settings_yaml)
mapper_Mapper_init_max_forward_motion=$(yq '.mapper.Mapper_init_max_forward_motion // 0.95' $settings_yaml)
mapper_Mapper_init_min_tri_angle=$(yq '.mapper.Mapper_init_min_tri_angle // 16' $settings_yaml)
mapper_Mapper_init_max_reg_trials=$(yq '.mapper.Mapper_init_max_reg_trials // 2' $settings_yaml)
mapper_Mapper_abs_pose_max_error=$(yq '.mapper.Mapper_abs_pose_max_error // 12' $settings_yaml)
mapper_Mapper_abs_pose_min_num_inliers=$(yq '.mapper.Mapper_abs_pose_min_num_inliers // 30' $settings_yaml)
mapper_Mapper_abs_pose_min_inlier_ratio=$(yq '.mapper.Mapper_abs_pose_min_inlier_ratio // 0.25' $settings_yaml)
mapper_Mapper_filter_max_reproj_error=$(yq '.mapper.Mapper_filter_max_reproj_error // 4' $settings_yaml)
mapper_Mapper_filter_min_tri_angle=$(yq '.mapper.Mapper_filter_min_tri_angle // 1.5' $settings_yaml)
mapper_Mapper_max_reg_trials=$(yq '.mapper.Mapper_max_reg_trials // 3' $settings_yaml)
mapper_Mapper_local_ba_min_tri_angle=$(yq '.mapper.Mapper_local_ba_min_tri_angle // 6' $settings_yaml)
mapper_Mapper_tri_max_transitivity=$(yq '.mapper.Mapper_tri_max_transitivity // 1' $settings_yaml)
mapper_Mapper_tri_create_max_angle_error=$(yq '.mapper.Mapper_tri_create_max_angle_error // 2' $settings_yaml)
mapper_Mapper_tri_continue_max_angle_error=$(yq '.mapper.Mapper_tri_continue_max_angle_error // 2' $settings_yaml)
mapper_Mapper_tri_merge_max_reproj_error=$(yq '.mapper.Mapper_tri_merge_max_reproj_error // 4' $settings_yaml)
mapper_Mapper_tri_complete_max_reproj_error=$(yq '.mapper.Mapper_tri_complete_max_reproj_error // 4' $settings_yaml)
mapper_Mapper_tri_complete_max_transitivity=$(yq '.mapper.Mapper_tri_complete_max_transitivity // 5' $settings_yaml)
mapper_Mapper_tri_re_max_angle_error=$(yq '.mapper.Mapper_tri_re_max_angle_error // 5' $settings_yaml)
mapper_Mapper_tri_re_min_ratio=$(yq '.mapper.Mapper_tri_re_min_ratio // 0.2' $settings_yaml)
mapper_Mapper_tri_re_max_trials=$(yq '.mapper.Mapper_tri_re_max_trials // 1' $settings_yaml)
mapper_Mapper_tri_min_angle=$(yq '.mapper.Mapper_tri_min_angle // 1.5' $settings_yaml)
mapper_Mapper_tri_ignore_two_view_tracks=$(yq '.mapper.Mapper_tri_ignore_two_view_tracks // 1' $settings_yaml)

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.min_num_matches ${mapper_Mapper_min_num_matches} \
    # --Mapper.ignore_watermarks ${mapper_Mapper_ignore_watermarks} \
    # --Mapper.multiple_models ${mapper_Mapper_multiple_models} \
    # --Mapper.max_num_models ${mapper_Mapper_max_num_models} \
    # --Mapper.max_model_overlap ${mapper_Mapper_max_model_overlap} \
    # --Mapper.min_model_size ${mapper_Mapper_min_model_size} \
    # --Mapper.init_image_id1 ${mapper_Mapper_init_image_id1} \
    # --Mapper.init_image_id2 ${mapper_Mapper_init_image_id2} \
    # --Mapper.init_num_trials ${mapper_Mapper_init_num_trials} \
    # --Mapper.extract_colors ${mapper_Mapper_extract_colors} \
    # --Mapper.num_threads ${mapper_Mapper_num_threads} \
    # --Mapper.min_focal_length_ratio ${mapper_Mapper_min_focal_length_ratio} \
    # --Mapper.max_focal_length_ratio ${mapper_Mapper_max_focal_length_ratio} \
    # --Mapper.max_extra_param ${mapper_Mapper_max_extra_param} \
    # --Mapper.ba_refine_focal_length ${mapper_Mapper_ba_refine_focal_length} \
    # --Mapper.ba_refine_principal_point ${mapper_Mapper_ba_refine_principal_point} \
    # --Mapper.ba_refine_extra_params ${mapper_Mapper_ba_refine_extra_params} \
    # --Mapper.ba_local_num_images ${mapper_Mapper_ba_local_num_images} \
    # --Mapper.ba_local_function_tolerance ${mapper_Mapper_ba_local_function_tolerance} \
    # --Mapper.ba_local_max_num_iterations ${mapper_Mapper_ba_local_max_num_iterations} \
    # --Mapper.ba_global_images_ratio ${mapper_Mapper_ba_global_images_ratio} \
    # --Mapper.ba_global_points_ratio ${mapper_Mapper_ba_global_points_ratio} \
    # --Mapper.ba_global_images_freq ${mapper_Mapper_ba_global_images_freq} \
    # --Mapper.ba_global_points_freq ${mapper_Mapper_ba_global_points_freq} \
    # --Mapper.ba_global_function_tolerance ${mapper_Mapper_ba_global_function_tolerance} \
    # --Mapper.ba_global_max_num_iterations ${mapper_Mapper_ba_global_max_num_iterations} \
    # --Mapper.ba_global_max_refinements ${mapper_Mapper_ba_global_max_refinements} \
    # --Mapper.ba_global_max_refinement_change ${mapper_Mapper_ba_global_max_refinement_change} \
    # --Mapper.ba_local_max_refinements ${mapper_Mapper_ba_local_max_refinements} \
    # --Mapper.ba_local_max_refinement_change ${mapper_Mapper_ba_local_max_refinement_change} \
    # --Mapper.ba_use_gpu ${mapper_Mapper_ba_use_gpu} \
    # --Mapper.ba_gpu_index ${mapper_Mapper_ba_gpu_index} \
    # --Mapper.ba_min_num_residuals_for_cpu_multi_threading ${mapper_Mapper_ba_min_num_residuals_for_cpu_multi_threading} \
    # --Mapper.snapshot_images_freq ${mapper_Mapper_snapshot_images_freq} \
    # --Mapper.fix_existing_images ${mapper_Mapper_fix_existing_images} \
    # --Mapper.init_min_num_inliers ${mapper_Mapper_init_min_num_inliers} \
    # --Mapper.init_max_error ${mapper_Mapper_init_max_error} \
    # --Mapper.init_max_forward_motion ${mapper_Mapper_init_max_forward_motion} \
    # --Mapper.init_min_tri_angle ${mapper_Mapper_init_min_tri_angle} \
    # --Mapper.init_max_reg_trials ${mapper_Mapper_init_max_reg_trials} \
    # --Mapper.abs_pose_max_error ${mapper_Mapper_abs_pose_max_error} \
    # --Mapper.abs_pose_min_num_inliers ${mapper_Mapper_abs_pose_min_num_inliers} \
    # --Mapper.abs_pose_min_inlier_ratio ${mapper_Mapper_abs_pose_min_inlier_ratio} \
    # --Mapper.filter_max_reproj_error ${mapper_Mapper_filter_max_reproj_error} \
    # --Mapper.filter_min_tri_angle ${mapper_Mapper_filter_min_tri_angle} \
    # --Mapper.max_reg_trials ${mapper_Mapper_max_reg_trials} \
    # --Mapper.local_ba_min_tri_angle ${mapper_Mapper_local_ba_min_tri_angle} \
    # --Mapper.tri_max_transitivity ${mapper_Mapper_tri_max_transitivity} \
    # --Mapper.tri_create_max_angle_error ${mapper_Mapper_tri_create_max_angle_error} \
    # --Mapper.tri_continue_max_angle_error ${mapper_Mapper_tri_continue_max_angle_error} \
    # --Mapper.tri_merge_max_reproj_error ${mapper_Mapper_tri_merge_max_reproj_error} \
    # --Mapper.tri_complete_max_reproj_error ${mapper_Mapper_tri_complete_max_reproj_error} \
    # --Mapper.tri_complete_max_transitivity ${mapper_Mapper_tri_complete_max_transitivity} \
    # --Mapper.tri_re_max_angle_error ${mapper_Mapper_tri_re_max_angle_error} \
    # --Mapper.tri_re_min_ratio ${mapper_Mapper_tri_re_min_ratio} \
    # --Mapper.tri_re_max_trials ${mapper_Mapper_tri_re_max_trials} \
    # --Mapper.tri_min_angle ${mapper_Mapper_tri_min_angle} \
    # --Mapper.tri_ignore_two_view_tracks ${mapper_Mapper_tri_ignore_two_view_tracks}


echo "    colmap model_converter ..."
colmap model_converter \
	--input_path ${exp_folder_colmap}/0 --output_path ${exp_folder_colmap} --output_type TXT