in opensfm/src/sfm/src/ba_helpers.cc [558:733]
py::dict BAHelpers::Bundle(
map::Map& map,
const std::unordered_map<map::CameraId, geometry::Camera>& camera_priors,
const std::unordered_map<map::RigCameraId, map::RigCamera>&
rig_camera_priors,
const AlignedVector<map::GroundControlPoint>& gcp, const py::dict& config) {
py::dict report;
auto ba = bundle::BundleAdjuster();
const bool fix_cameras = !config["optimize_camera_parameters"].cast<bool>();
ba.SetUseAnalyticDerivatives(
config["bundle_analytic_derivatives"].cast<bool>());
const auto start = std::chrono::high_resolution_clock::now();
const auto& all_cameras = map.GetCameras();
for (const auto& cam_pair : all_cameras) {
const auto& cam = cam_pair.second;
const auto& cam_prior = camera_priors.at(cam.id);
ba.AddCamera(cam.id, cam, cam_prior, fix_cameras);
}
for (const auto& pt_pair : map.GetLandmarks()) {
const auto& pt = pt_pair.second;
ba.AddPoint(pt.id_, pt.GetGlobalPos(), false);
}
auto align_method = config["align_method"].cast<std::string>();
if (align_method.compare("auto") == 0) {
align_method = DetectAlignmentConstraints(map, config, gcp);
}
bool do_add_align_vector = false;
Vec3d up_vector = Vec3d::Zero();
if (align_method.compare("orientation_prior") == 0) {
const std::string align_orientation_prior =
config["align_orientation_prior"].cast<std::string>();
if (align_orientation_prior.compare("vertical") == 0) {
do_add_align_vector = true;
up_vector = Vec3d(0, 0, -1);
} else if (align_orientation_prior.compare("horizontal") == 0) {
do_add_align_vector = true;
up_vector = Vec3d(0, -1, 0);
}
}
// setup rig cameras
constexpr size_t kMinRigInstanceForAdjust{10};
const auto lock_rig_camera =
map.GetRigInstances().size() <= kMinRigInstanceForAdjust;
for (const auto& camera_pair : map.GetRigCameras()) {
// could be set to false (not locked) the day we expose leverarm adjustment
const bool is_leverarm =
all_cameras.find(camera_pair.first) != all_cameras.end();
ba.AddRigCamera(camera_pair.first, camera_pair.second.pose,
rig_camera_priors.at(camera_pair.first).pose,
is_leverarm | lock_rig_camera);
}
// setup rig instances
const std::string gps_scale_group = "dummy"; // unused for now
for (auto instance_pair : map.GetRigInstances()) {
auto& instance = instance_pair.second;
Vec3d average_position = Vec3d::Zero();
double average_std = 0.;
int gps_count = 0;
// average GPS and assign GPS constraint to the instance
std::unordered_map<std::string, std::string> shot_cameras, shot_rig_cameras;
for (const auto& shot_n_rig_camera : instance.GetRigCameras()) {
const auto shot_id = shot_n_rig_camera.first;
const auto& shot = map.GetShot(shot_id);
shot_cameras[shot_id] = shot.GetCamera()->id;
shot_rig_cameras[shot_id] = shot_n_rig_camera.second->id;
if (config["bundle_use_gps"].cast<bool>()) {
const auto pos = shot.GetShotMeasurements().gps_position_;
const auto acc = shot.GetShotMeasurements().gps_accuracy_;
if (pos.HasValue() && acc.HasValue()) {
average_position += pos.Value();
average_std += acc.Value();
++gps_count;
}
}
}
ba.AddRigInstance(instance_pair.first, instance.GetPose(), shot_cameras,
shot_rig_cameras, false);
if (config["bundle_use_gps"].cast<bool>() && gps_count > 0) {
average_position /= gps_count;
average_std /= gps_count;
ba.AddRigInstancePositionPrior(instance_pair.first, average_position,
Vec3d::Constant(average_std),
gps_scale_group);
}
}
for (const auto& shot_pair : map.GetShots()) {
const auto& shot = shot_pair.second;
// that one doesn't have it's rig counterpart
if (do_add_align_vector) {
constexpr double std_dev = 1e-3;
ba.AddAbsoluteUpVector(shot.id_, up_vector, std_dev);
}
// setup observations for any shot type
for (const auto& lm_obs : shot.GetLandmarkObservations()) {
const auto& obs = lm_obs.second;
ba.AddPointProjectionObservation(shot.id_, lm_obs.first->id_, obs.point,
obs.scale);
}
}
const auto& reference = map.GetTopocentricConverter();
if (config["bundle_use_gcp"].cast<bool>() && !gcp.empty()) {
AddGCPToBundle(ba, reference, gcp, map.GetShots(),
config["gcp_horizontal_sd"].cast<double>(),
config["gcp_vertical_sd"].cast<double>());
}
if (config["bundle_compensate_gps_bias"].cast<bool>()) {
const auto& biases = map.GetBiases();
for (const auto& camera : map.GetCameras()) {
ba.SetCameraBias(camera.first, biases.at(camera.first));
}
}
ba.SetPointProjectionLossFunction(
config["loss_function"].cast<std::string>(),
config["loss_function_threshold"].cast<double>());
ba.SetInternalParametersPriorSD(
config["exif_focal_sd"].cast<double>(),
config["principal_point_sd"].cast<double>(),
config["radial_distortion_k1_sd"].cast<double>(),
config["radial_distortion_k2_sd"].cast<double>(),
config["tangential_distortion_p1_sd"].cast<double>(),
config["tangential_distortion_p2_sd"].cast<double>(),
config["radial_distortion_k3_sd"].cast<double>(),
config["radial_distortion_k4_sd"].cast<double>());
ba.SetRigParametersPriorSD(config["rig_translation_sd"].cast<double>(),
config["rig_rotation_sd"].cast<double>());
ba.SetNumThreads(config["processes"].cast<int>());
ba.SetMaxNumIterations(config["bundle_max_iterations"].cast<int>());
ba.SetLinearSolverType("SPARSE_SCHUR");
const auto timer_setup = std::chrono::high_resolution_clock::now();
{
py::gil_scoped_release release;
ba.Run();
}
const auto timer_run = std::chrono::high_resolution_clock::now();
BundleToMap(ba, map, !fix_cameras);
const auto timer_teardown = std::chrono::high_resolution_clock::now();
report["brief_report"] = ba.BriefReport();
report["wall_times"] = py::dict();
report["wall_times"]["setup"] =
std::chrono::duration_cast<std::chrono::microseconds>(timer_setup - start)
.count() /
1000000.0;
report["wall_times"]["run"] =
std::chrono::duration_cast<std::chrono::microseconds>(timer_run -
timer_setup)
.count() /
1000000.0;
report["wall_times"]["teardown"] =
std::chrono::duration_cast<std::chrono::microseconds>(timer_teardown -
timer_run)
.count() /
1000000.0;
return report;
}