From c31b7681884a0bfa899ded996a12b1e1a18187ce Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 15 Jul 2024 17:36:58 -0700 Subject: [PATCH] bundle_adjust: Modularize --- src/asp/Camera/BundleAdjustCostFuns.cc | 63 +++++++++++++++++++++++++ src/asp/Camera/BundleAdjustCostFuns.h | 65 ++++++++++++++++---------- src/asp/Tools/bundle_adjust.cc | 48 ++++--------------- 3 files changed, 113 insertions(+), 63 deletions(-) diff --git a/src/asp/Camera/BundleAdjustCostFuns.cc b/src/asp/Camera/BundleAdjustCostFuns.cc index 746a4ad09..9636879a8 100644 --- a/src/asp/Camera/BundleAdjustCostFuns.cc +++ b/src/asp/Camera/BundleAdjustCostFuns.cc @@ -895,6 +895,69 @@ void addPixelReprojCostFun(asp::BaOptions const& opt, return; } +// Add a soft constraint that ties triangulated points close to their initial positions. +// This is adjusted for GSD. +void addTriConstraint(asp::BaOptions const& opt, + vw::ba::ControlNetwork const& cnet, + asp::CRNJ const& crn, + std::vector const& image_files, + std::vector const& orig_cams, + double tri_weight, + std::string cost_function_str, + double tri_robust_threshold, + // Outputs + asp::BAParams & param_storage, + ceres::Problem & problem, + int & num_tri_residuals) { + + // Initialize the output + num_tri_residuals = 0; + + // Tri weight must be positive + if (tri_weight <= 0) + vw::vw_throw(vw::ArgumentErr() << "The triangulation weight must be positive.\n"); + + int num_points = param_storage.num_points(); + if ((int)cnet.size() != num_points) + vw_throw(ArgumentErr() << "Book-keeping error, the size of the control network " + "must be the same as the number of triangulated points.\n"); + + // Add triangulation weight to make each triangulated point not move too far + std::vector gsds; + asp::estimateGsdPerTriPoint(image_files, orig_cams, crn, param_storage, gsds); + for (int ipt = 0; ipt < num_points; ipt++) { + if (cnet[ipt].type() == vw::ba::ControlPoint::GroundControlPoint || + cnet[ipt].type() == vw::ba::ControlPoint::PointFromDem) + continue; // Skip GCPs and height-from-dem points which have their own constraint + + if (param_storage.get_point_outlier(ipt)) + continue; // skip outliers + + // Use as constraint the triangulated point optimized at the previous + // iteration. That is on purpose, to ensure that the triangulated point is + // more accurate than it was at the start of the optimization. For the + // first iteration, that will be what is read from the cnet. Note how the + // weight is normalized by the GSD, to make it in pixel coordinates, as + // the rest of the residuals. + double * point = param_storage.get_point_ptr(ipt); + Vector3 observation(point[0], point[1], point[2]); + double gsd = gsds[ipt]; + if (gsd <= 0 || std::isnan(gsd)) + continue; // GSD calculation failed. Do not use a constraint. + + double s = gsd/tri_weight; + Vector3 xyz_sigma(s, s, s); + + ceres::CostFunction* cost_function = XYZError::Create(observation, xyz_sigma); + ceres::LossFunction* loss_function + = get_loss_function(cost_function_str, tri_robust_threshold); + problem.AddResidualBlock(cost_function, loss_function, point); + + num_tri_residuals++; + } // End loop through xyz + +} // end adding a triangulation constraint + // Add a cost function meant to tie up to known disparity form left to right // image and known ground truth reference terrain (option --reference-terrain). // This was only tested for pinhole cameras. Disparity must be created with diff --git a/src/asp/Camera/BundleAdjustCostFuns.h b/src/asp/Camera/BundleAdjustCostFuns.h index 85bf79704..8e3edf818 100644 --- a/src/asp/Camera/BundleAdjustCostFuns.h +++ b/src/asp/Camera/BundleAdjustCostFuns.h @@ -490,31 +490,6 @@ void add_disparity_residual_block(vw::Vector3 const& reference_xyz, asp::BaOptions const& opt, ceres::Problem & problem); -// Add a cost function meant to tie up to known disparity form left to right -// image and known ground truth reference terrain (option --reference-terrain). -// This was only tested for pinhole cameras. Disparity must be created with -// stereo with the option --unalign-disparity. If there are n images, there must -// be n-1 disparities, from each image to the next. -void addReferenceTerrainCostFunction( - asp::BaOptions & opt, - asp::BAParams & param_storage, - ceres::Problem & problem, - std::vector & reference_vec, - std::vector> & interp_disp); - -// Add a soft constraint to keep the cameras near the original position. -// Add a combined constraint for all reprojection errors in given camera. -void addCamPosCostFun(asp::BaOptions const& opt, - asp::BAParams const& orig_parameters, - std::vector> const& pixels_per_cam, - std::vector> const& tri_points_per_cam, - std::vector> const& pixel_sigmas, - std::vector const& orig_cams, - // Outputs - asp::BAParams & param_storage, - ceres::Problem & problem, - int & num_cam_pos_residuals); - // Pixel reprojection error. Note: cam_residual_counts and num_pixels_per_cam // serve different purposes. void addPixelReprojCostFun(asp::BaOptions const& opt, @@ -535,6 +510,21 @@ void addPixelReprojCostFun(asp::BaOptions const& opt, std::vector> & tri_points_per_cam, std::vector> & pixel_sigmas); +// Add a soft constraint that ties triangulated points close to their initial positions. +// This is adjusted for GSD. +void addTriConstraint(asp::BaOptions const& opt, + vw::ba::ControlNetwork const& cnet, + asp::CRNJ const& crn, + std::vector const& image_files, + std::vector const& orig_cams, + double tri_weight, + std::string cost_function_str, + double tri_robust_threshold, + // Outputs + asp::BAParams & param_storage, + ceres::Problem & problem, + int & num_tri_residuals); + // Add a ground constraint (GCP or height from DEM) void addGcpOrDemConstraint(asp::BaBaseOptions const& opt, std::string const& cost_function_str, @@ -547,6 +537,31 @@ void addGcpOrDemConstraint(asp::BaBaseOptions const& opt, asp::BAParams & param_storage, ceres::Problem & problem); +// Add a cost function meant to tie up to known disparity form left to right +// image and known ground truth reference terrain (option --reference-terrain). +// This was only tested for pinhole cameras. Disparity must be created with +// stereo with the option --unalign-disparity. If there are n images, there must +// be n-1 disparities, from each image to the next. +void addReferenceTerrainCostFunction( + asp::BaOptions & opt, + asp::BAParams & param_storage, + ceres::Problem & problem, + std::vector & reference_vec, + std::vector> & interp_disp); + +// Add a soft constraint to keep the cameras near the original position. +// Add a combined constraint for all reprojection errors in given camera. +void addCamPosCostFun(asp::BaOptions const& opt, + asp::BAParams const& orig_parameters, + std::vector> const& pixels_per_cam, + std::vector> const& tri_points_per_cam, + std::vector> const& pixel_sigmas, + std::vector const& orig_cams, + // Outputs + asp::BAParams & param_storage, + ceres::Problem & problem, + int & num_cam_pos_residuals); + } // end namespace asp #endif // __ASP_CAMERA_BUNDLE_ADJUST_COST_FUNCTIONS_H__ diff --git a/src/asp/Tools/bundle_adjust.cc b/src/asp/Tools/bundle_adjust.cc index 93ca85489..550dcd882 100644 --- a/src/asp/Tools/bundle_adjust.cc +++ b/src/asp/Tools/bundle_adjust.cc @@ -93,7 +93,7 @@ class BaCallback: public ceres::IterationCallback { // Start outlier functions /// Add to the outliers based on the large residuals -int add_to_outliers(ControlNetwork & cnet, +int add_to_outliers(vw::ba::ControlNetwork & cnet, asp::CRNJ const& crn, asp::BAParams & param_storage, asp::BaOptions const& opt, @@ -461,42 +461,13 @@ int do_ba_ceres_one_pass(asp::BaOptions & opt, asp::addReferenceTerrainCostFunction(opt, param_storage, problem, reference_vec, interp_disp); - // TODO(oalexan1): Put this in a separate function + // Add a ground constraints to keep points close to their initial positions int num_tri_residuals = 0; - if (opt.tri_weight > 0) { - // Add triangulation weight to make each triangulated point not move too far - std::vector gsds; - asp::estimateGsdPerTriPoint(opt.image_files, orig_cams, crn, param_storage, gsds); - for (int ipt = 0; ipt < num_points; ipt++) { - if (cnet[ipt].type() == vw::ba::ControlPoint::GroundControlPoint || - cnet[ipt].type() == vw::ba::ControlPoint::PointFromDem) - continue; // Skip GCPs and height-from-dem points which have their own constraint - - if (param_storage.get_point_outlier(ipt)) - continue; // skip outliers - - // Use as constraint the triangulated point optimized at the previous - // iteration. That is on purpose, to ensure that the triangulated point is - // more accurate than it was at the start of the optimization. For the - // first iteration, that will be what is read from the cnet. Note how the - // weight is normalized by the GSD, to make it in pixel coordinates, as - // the rest of the residuals. - double * point = param_storage.get_point_ptr(ipt); - Vector3 observation(point[0], point[1], point[2]); - double gsd = gsds[ipt]; - if (gsd <= 0) - continue; // GSD calculation failed. Do not use a constraint. - double s = gsd/opt.tri_weight; - Vector3 xyz_sigma(s, s, s); - - ceres::CostFunction* cost_function = XYZError::Create(observation, xyz_sigma); - ceres::LossFunction* loss_function - = get_loss_function(opt.cost_function, opt.tri_robust_threshold); - problem.AddResidualBlock(cost_function, loss_function, point); - - num_tri_residuals++; - } // End loop through xyz - } // end adding a triangulation constraint + if (opt.tri_weight > 0) + asp::addTriConstraint(opt, cnet, crn, opt.image_files, orig_cams, + opt.tri_weight, opt.cost_function, opt.tri_robust_threshold, + // Outputs + param_storage, problem, num_tri_residuals); const size_t MIN_KML_POINTS = 50; size_t kmlPointSkip = 30; @@ -534,9 +505,9 @@ int do_ba_ceres_one_pass(asp::BaOptions & opt, options.gradient_tolerance = 1e-16; options.function_tolerance = 1e-16; options.parameter_tolerance = opt.parameter_tolerance; // default is 1e-8 - options.max_num_iterations = opt.num_iterations; + options.max_num_iterations = opt.num_iterations; options.max_num_consecutive_invalid_steps = std::max(5, opt.num_iterations/5); // try hard - options.minimizer_progress_to_stdout = true; + options.minimizer_progress_to_stdout = true; if (opt.single_threaded_cameras) options.num_threads = 1; @@ -2496,6 +2467,7 @@ void computeStats(asp::BaOptions const& opt, std::vector const& map return; } +// TODO(oalexan1): Move to BundleAjustIp.cc void findPairwiseMatches(asp::BaOptions & opt, // will change std::vector const& map_files, std::string const& mapproj_dem,