in opensfm/src/sfm/src/ba_helpers.cc [115:309]
py::tuple BAHelpers::BundleLocal(
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 map::ShotId& central_shot_id, const py::dict& config) {
py::dict report;
const auto start = std::chrono::high_resolution_clock::now();
auto neighborhood = ShotNeighborhood(
map, central_shot_id, config["local_bundle_radius"].cast<size_t>(),
config["local_bundle_min_common_points"].cast<size_t>(),
config["local_bundle_max_shots"].cast<size_t>());
auto& interior = neighborhood.first;
auto& boundary = neighborhood.second;
// set up BA
auto ba = bundle::BundleAdjuster();
ba.SetUseAnalyticDerivatives(
config["bundle_analytic_derivatives"].cast<bool>());
for (const auto& cam_pair : map.GetCameras()) {
const auto& cam = cam_pair.second;
const auto& cam_prior = camera_priors.at(cam.id);
constexpr bool fix_cameras{true};
ba.AddCamera(cam.id, cam, cam_prior, fix_cameras);
}
// combine the sets
std::unordered_set<map::Shot*> int_and_bound(interior.cbegin(),
interior.cend());
int_and_bound.insert(boundary.cbegin(), boundary.cend());
std::unordered_set<map::Landmark*> points;
py::list pt_ids;
constexpr bool point_constant{false};
constexpr bool rig_camera_constant{true};
// gather required rig data to setup
std::unordered_set<map::RigCameraId> rig_cameras_ids;
std::unordered_set<map::RigInstanceId> rig_instances_ids;
for (auto* shot : int_and_bound) {
rig_cameras_ids.insert(shot->GetRigCameraId());
rig_instances_ids.insert(shot->GetRigInstanceId());
}
// rig cameras are going to be fixed
for (const auto& rig_camera_id : rig_cameras_ids) {
const auto& rig_camera = map.GetRigCamera(rig_camera_id);
ba.AddRigCamera(rig_camera_id, rig_camera.pose,
rig_camera_priors.at(rig_camera_id).pose,
rig_camera_constant);
}
// add rig instances shots
const std::string gps_scale_group = "dummy"; // unused for now
for (const auto& rig_instance_id : rig_instances_ids) {
auto& instance = map.GetRigInstance(rig_instance_id);
std::unordered_map<std::string, std::string> shot_cameras, shot_rig_cameras;
// we're going to assign GPS constraint to the instance itself
// by averaging its shot's GPS values (and std dev.)
Vec3d average_position = Vec3d::Zero();
double average_std = 0.;
int gps_count = 0;
// if any instance's shot is in boundary
// then the entire instance will be fixed
bool fix_instance = false;
for (const auto& shot_n_rig_camera : instance.GetRigCameras()) {
const auto shot_id = shot_n_rig_camera.first;
auto& shot = map.GetShot(shot_id);
shot_cameras[shot_id] = shot.GetCamera()->id;
shot_rig_cameras[shot_id] = shot_n_rig_camera.second->id;
const auto is_boundary = boundary.find(&shot) != boundary.end();
const auto is_interior = !is_boundary;
if (is_interior) {
const auto& measurements = shot.GetShotMeasurements();
if (config["bundle_use_gps"].cast<bool>() &&
measurements.gps_position_.HasValue()) {
average_position += measurements.gps_position_.Value();
average_std += measurements.gps_accuracy_.Value();
++gps_count;
}
} else {
fix_instance = true;
}
}
ba.AddRigInstance(rig_instance_id, instance.GetPose(), shot_cameras,
shot_rig_cameras, fix_instance);
// only add averaged rig position constraints to moving instances
if (!fix_instance && gps_count > 0) {
average_position /= gps_count;
average_std /= gps_count;
ba.AddRigInstancePositionPrior(rig_instance_id, average_position,
Vec3d::Constant(average_std),
gps_scale_group);
}
}
for (auto* shot : interior) {
// Add all points of the shots that are in the interior
for (const auto& lm_obs : shot->GetLandmarkObservations()) {
auto* lm = lm_obs.first;
if (points.count(lm) == 0) {
points.insert(lm);
pt_ids.append(lm->id_);
ba.AddPoint(lm->id_, lm->GetGlobalPos(), point_constant);
}
const auto& obs = lm_obs.second;
ba.AddPointProjectionObservation(shot->id_, lm_obs.first->id_, obs.point,
obs.scale);
}
}
for (auto* shot : boundary) {
for (const auto& lm_obs : shot->GetLandmarkObservations()) {
auto* lm = lm_obs.first;
if (points.count(lm) > 0) {
const auto& obs = lm_obs.second;
ba.AddPointProjectionObservation(shot->id_, lm_obs.first->id_,
obs.point, obs.scale);
}
}
}
if (config["bundle_use_gcp"].cast<bool>() && !gcp.empty()) {
const auto& reference = map.GetTopocentricConverter();
AddGCPToBundle(ba, reference, gcp, map.GetShots(),
config["gcp_horizontal_sd"].cast<double>(),
config["gcp_vertical_sd"].cast<double>());
}
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(10);
ba.SetLinearSolverType("DENSE_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();
for (const auto& rig_instance_id : rig_instances_ids) {
auto& instance = map.GetRigInstance(rig_instance_id);
auto i = ba.GetRigInstance(rig_instance_id);
instance.SetPose(i.GetValue());
}
for (auto* point : points) {
const auto& pt = ba.GetPoint(point->id_);
point->SetGlobalPos(pt.GetValue());
point->SetReprojectionErrors(pt.reprojection_errors);
}
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;
report["num_interior_images"] = interior.size();
report["num_boundary_images"] = boundary.size();
report["num_other_images"] =
map.NumberOfShots() - interior.size() - boundary.size();
return py::make_tuple(pt_ids, report);
}