in source/calibration/GeometricCalibration.cpp [998:1207]
double refine(
std::vector<Camera>& cameras,
const std::vector<Camera>& groundTruth,
FeatureMap featureMap,
std::vector<Overlap> overlaps,
const int pass) {
boost::timer::cpu_timer timer;
// remove outlier matches
LOG(INFO) << "Removing outlier matches...";
removeOutliersFromCameras(overlaps, featureMap, {}, cameras, FLAGS_outlier_factor);
// assemble and remove outlier traces
LOG(INFO) << "Assembling traces and removing outlier traces...";
std::vector<Trace> traces = assembleTraces(featureMap, overlaps);
triangulateTraces(traces, featureMap, cameras);
removeOutliersFromCameras(overlaps, featureMap, traces, cameras, FLAGS_outlier_factor);
// final triangulation
LOG(INFO) << "Reassembling traces with outliers removed and removing invalid traces...";
traces = assembleTraces(featureMap, overlaps);
if (!FLAGS_keep_invalid_traces) {
removeInvalidTraces(traces, featureMap);
}
triangulateTraces(traces, featureMap, cameras);
std::vector<int> weights = calculateCameraWeights(cameras, traces);
// visualization for debugging
showMatches(cameras, featureMap, overlaps, traces, pass);
// read camera parameters from cameras
std::vector<Camera::Vector3> positions;
std::vector<Camera::Vector3> rotations;
std::vector<Camera::Vector2> principals;
std::vector<Camera::Real> focals;
std::vector<Camera::Distortion> distortions;
for (Camera& camera : cameras) {
positions.push_back(camera.position);
rotations.push_back(camera.getRotation());
principals.push_back(camera.principal);
focals.push_back(camera.getScalarFocal());
distortions.push_back(camera.getDistortion());
}
int referenceCameraIdx = -1;
int relativeCameraIdx = -1;
Camera::Real theta;
Camera::Real phi;
Camera::Real radius = 0.0; // initialized to silence compiler but this will never get used
// If positions are unlocked, define a locked reference camera and lock the baseline between
// the reference camera and relative camera
if (positionsUnlocked(pass)) {
if (FLAGS_reference_camera.empty()) {
referenceCameraIdx = 0;
} else {
CHECK(cameraIdToIndex.count(FLAGS_reference_camera))
<< "bad reference_camera: " << FLAGS_reference_camera;
referenceCameraIdx = cameraIdToIndex[FLAGS_reference_camera];
}
relativeCameraIdx = (referenceCameraIdx + 1) % ssize(cameras);
Camera::Vector3 relativePosition = positions[relativeCameraIdx] - positions[referenceCameraIdx];
cartesianToSpherical(radius, theta, phi, relativePosition);
}
// create the problem: add a residual for each observation
ceres::Problem problem;
std::vector<int> counts(cameras.size());
for (const Trace& trace : traces) {
if (FLAGS_cap_traces && !randomSample(FLAGS_cap_traces, traces.size())) {
continue;
}
for (const auto& ref : trace.references) {
const ImageId& image = ref.first;
const Feature& feature = featureMap[image][ref.second];
const int camera = getCameraIndex(image);
++counts[camera];
const int group = cameraGroupToIndex[cameras[camera].group];
if (camera == relativeCameraIdx) {
SphericalReprojectionFunctor::addResidual(
problem,
theta,
phi,
rotations[camera],
principals[FLAGS_shared_principal_and_focal ? group : camera],
focals[FLAGS_shared_principal_and_focal ? group : camera],
distortions[FLAGS_shared_distortion ? group : camera],
traces[feature.trace].position,
radius,
cameras[referenceCameraIdx].position,
cameras[camera],
feature.position,
FLAGS_robust,
weights[camera]);
} else {
ReprojectionFunctor::addResidual(
problem,
positions[camera],
rotations[camera],
principals[FLAGS_shared_principal_and_focal ? group : camera],
focals[FLAGS_shared_principal_and_focal ? group : camera],
distortions[FLAGS_shared_distortion ? group : camera],
traces[feature.trace].position,
cameras[camera],
feature.position,
FLAGS_robust,
weights[camera]);
}
}
}
validateMatchCount(cameras, counts);
// lock focal and distortion
if (pass == 0 || FLAGS_lock_focal) {
if (FLAGS_shared_principal_and_focal) {
for (const auto& mapping : cameraGroupToIndex) {
lockParameter(problem, focals[mapping.second]);
}
} else {
lockParameters(problem, focals);
}
}
if (pass == 0 || FLAGS_lock_distortion) {
if (FLAGS_shared_distortion) {
for (const auto& mapping : cameraGroupToIndex) {
lockParameter(problem, distortions[mapping.second]);
}
} else {
lockParameters(problem, distortions);
}
}
if (FLAGS_lock_principals) {
lockParameters(problem, principals);
}
// lock position
LOG(INFO) << folly::sformat("Pass: {}", pass);
// If positions are unlocked, only lock the position and rotation of the reference camera
if (positionsUnlocked(pass)) {
problem.SetParameterBlockConstant(positions[referenceCameraIdx].data());
problem.SetParameterBlockConstant(rotations[referenceCameraIdx].data());
} else {
lockParameters(problem, positions);
}
if (FLAGS_lock_rotations) {
lockParameters(problem, rotations);
}
if (FLAGS_robust) {
std::vector<calibration::ReprojectionErrorOutlier> errorsIgnored =
getReprojectionErrorOutliers(problem);
LOG(INFO) << folly::sformat("Number of down-weighted outliers: {}", errorsIgnored.size());
std::sort(errorsIgnored.begin(), errorsIgnored.end(), math_util::sortdescPair<double, double>);
LOG(INFO) << folly::sformat(
"Highest 3 (true/weighted): {}/{}, {}/{}, {}/{}",
errorsIgnored[2].first,
errorsIgnored[2].second,
errorsIgnored[1].first,
errorsIgnored[1].second,
errorsIgnored[0].first,
errorsIgnored[0].second);
}
reportReprojectionErrors(overlaps, featureMap, traces, cameras);
solve(problem);
if (positionsUnlocked(pass)) {
positions[relativeCameraIdx] = sphericalToCartesian(radius, theta, phi);
positions[relativeCameraIdx] += positions[referenceCameraIdx];
}
std::vector<double> norms =
getReprojectionErrorNorms(problem, nullptr, FLAGS_weighted_statistics);
double median = calcPercentile(norms, 0.5);
if (pass == FLAGS_pass_count - 1 && median > FLAGS_max_error) {
LOG(INFO) << folly::sformat("Warning: Final pass median error too high: {}", median);
}
// write optimized camera parameters back into cameras
for (int i = 0; i < int(cameras.size()); ++i) {
const int group = cameraGroupToIndex[cameras[i].group];
cameras[i] = makeCamera(
cameras[i],
positions[i],
rotations[i],
principals[FLAGS_shared_principal_and_focal ? group : i],
focals[FLAGS_shared_principal_and_focal ? group : i],
distortions[FLAGS_shared_distortion ? group : i]);
}
reportReprojectionErrors(overlaps, featureMap, traces, cameras);
if (FLAGS_points_file != "" && pass == FLAGS_pass_count - 1) {
savePointsFile(featureMap, traces);
}
if (FLAGS_points_file_json != "" && pass == FLAGS_pass_count - 1) {
savePointsFileJson(featureMap, traces);
}
// visualization for debugging
if (FLAGS_debug_error_scale && pass == FLAGS_pass_count - 1) {
showReprojections(cameras, featureMap, traces, FLAGS_debug_error_scale);
}
if (FLAGS_enable_timing) {
LOG(INFO) << folly::sformat("Pass {} timing :{}", pass, timer.format());
}
return median;
}