Skip to content

Commit

Permalink
bundle_adjust: Modularize
Browse files Browse the repository at this point in the history
  • Loading branch information
@oleg-alexandrov
oleg-alexandrov committed Jul 16, 2024
1 parent 73553f0 commit c31b768
Show file tree
Hide file tree
Showing 3 changed files with 113 additions and 63 deletions.
63 changes: 63 additions & 0 deletions src/asp/Camera/BundleAdjustCostFuns.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> const& image_files,
std::vector<vw::CamPtr> 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<double> 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
Expand Down
65 changes: 40 additions & 25 deletions src/asp/Camera/BundleAdjustCostFuns.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<vw::Vector3> & reference_vec,
std::vector<vw::ImageViewRef<DispPixelT>> & 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<std::vector<vw::Vector2>> const& pixels_per_cam,
std::vector<std::vector<vw::Vector3>> const& tri_points_per_cam,
std::vector<std::map<int, vw::Vector2>> const& pixel_sigmas,
std::vector<vw::CamPtr> 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,
Expand All @@ -535,6 +510,21 @@ void addPixelReprojCostFun(asp::BaOptions const& opt,
std::vector<std::vector<vw::Vector3>> & tri_points_per_cam,
std::vector<std::map<int, vw::Vector2>> & 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<std::string> const& image_files,
std::vector<vw::CamPtr> 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,
Expand All @@ -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<vw::Vector3> & reference_vec,
std::vector<vw::ImageViewRef<DispPixelT>> & 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<std::vector<vw::Vector2>> const& pixels_per_cam,
std::vector<std::vector<vw::Vector3>> const& tri_points_per_cam,
std::vector<std::map<int, vw::Vector2>> const& pixel_sigmas,
std::vector<vw::CamPtr> 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__
Expand Down
48 changes: 10 additions & 38 deletions src/asp/Tools/bundle_adjust.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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<double> 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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -2496,6 +2467,7 @@ void computeStats(asp::BaOptions const& opt, std::vector<std::string> const& map
return;
}

// TODO(oalexan1): Move to BundleAjustIp.cc
void findPairwiseMatches(asp::BaOptions & opt, // will change
std::vector<std::string> const& map_files,
std::string const& mapproj_dem,
Expand Down

0 comments on commit c31b768

Please sign in to comment.