1449 lines
65 KiB
C++
1449 lines
65 KiB
C++
#include <lardon3d/sparse_sfm_bundle_adjustment.h>
|
|
|
|
#include "incremental_reconstruction_internal.h"
|
|
|
|
#include <ceres/ceres.h>
|
|
#include <ceres/rotation.h>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <limits>
|
|
#include <memory>
|
|
#include <new>
|
|
#include <numeric>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
namespace lardon3d::sparse_bundle_adjustment {
|
|
|
|
constexpr size_t maximum_images = 4096;
|
|
constexpr size_t maximum_landmarks = 250000;
|
|
constexpr size_t maximum_observations = 1000000;
|
|
constexpr double depth_epsilon = 1e-9;
|
|
constexpr double gauge_epsilon = 1e-9;
|
|
|
|
enum class PreparationStatus {
|
|
prepared,
|
|
invalid_argument,
|
|
out_of_memory,
|
|
internal_error
|
|
};
|
|
enum class PrivateTermination { converged, no_convergence, failure };
|
|
|
|
struct ResolvedObservation {
|
|
Lardon3DSparseIncrementalObservation source;
|
|
size_t image_index;
|
|
};
|
|
|
|
struct Preparation {
|
|
std::vector<Lardon3DSparseIncrementalImage> images;
|
|
std::vector<Lardon3DSparseIncrementalComponent> components;
|
|
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
|
|
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
|
|
std::vector<ResolvedObservation> resolved_observations;
|
|
std::vector<Lardon3DSparseBundleAdjustmentComponentDiagnostic> diagnostics;
|
|
};
|
|
|
|
struct ImageIndex {
|
|
uint64_t image_id;
|
|
size_t index;
|
|
};
|
|
|
|
struct ObservationIndex {
|
|
uint64_t feature_set_id;
|
|
uint32_t feature_index;
|
|
size_t index;
|
|
};
|
|
|
|
struct ComponentView {
|
|
std::vector<size_t> cameras;
|
|
std::vector<size_t> landmarks;
|
|
std::vector<size_t> observations;
|
|
};
|
|
|
|
struct SolverCamera {
|
|
size_t source_index;
|
|
size_t image_index;
|
|
double quaternion[4];
|
|
double center[3];
|
|
double initial_center[3];
|
|
};
|
|
|
|
struct SolverLandmark {
|
|
size_t source_index;
|
|
double point[3];
|
|
};
|
|
|
|
struct SolverObservation {
|
|
size_t camera_index;
|
|
size_t landmark_index;
|
|
size_t resolved_index;
|
|
};
|
|
|
|
struct UnderconstraintResult {
|
|
bool valid;
|
|
uint32_t mask;
|
|
};
|
|
enum class UnderconstraintAction { proceed, reject, internal_error };
|
|
|
|
struct CandidateDecision {
|
|
bool accepted;
|
|
bool has_metrics;
|
|
Lardon3DSparseBundleAdjustmentRejectionReason rejection_reason;
|
|
};
|
|
|
|
enum class ParameterKind : int { landmark = 0, camera_quaternion = 1, camera_center = 2 };
|
|
struct ParameterRecord {
|
|
ParameterKind kind;
|
|
uint64_t identity;
|
|
int group;
|
|
bool constant;
|
|
int subset_axis;
|
|
};
|
|
|
|
struct DisjointSet {
|
|
std::vector<size_t> parent;
|
|
|
|
explicit DisjointSet(size_t count) : parent(count) {
|
|
std::iota(parent.begin(), parent.end(), 0);
|
|
}
|
|
|
|
size_t find(size_t value) {
|
|
while (parent[value] != value) {
|
|
parent[value] = parent[parent[value]];
|
|
value = parent[value];
|
|
}
|
|
return value;
|
|
}
|
|
|
|
void join(size_t a, size_t b) {
|
|
a = find(a);
|
|
b = find(b);
|
|
if (a != b) parent[std::max(a, b)] = std::min(a, b);
|
|
}
|
|
};
|
|
|
|
bool finite_calibration(const Lardon3DSparseGeometryCalibration &value) {
|
|
return value.width > 0 && value.height > 0 && std::isfinite(value.fx) &&
|
|
std::isfinite(value.fy) && value.fx > 0.0 && value.fy > 0.0 &&
|
|
std::isfinite(value.cx) && std::isfinite(value.cy) && value.cx >= 0.0 &&
|
|
value.cy >= 0.0 && value.cx < value.width && value.cy < value.height &&
|
|
std::isfinite(value.k1) && std::isfinite(value.k2) &&
|
|
std::isfinite(value.p1) && std::isfinite(value.p2);
|
|
}
|
|
|
|
bool finite_pose(const Lardon3DSparseGeometryPose &value) {
|
|
for (double item : value.rotation_cw)
|
|
if (!std::isfinite(item)) return false;
|
|
for (double item : value.translation_cw)
|
|
if (!std::isfinite(item)) return false;
|
|
return true;
|
|
}
|
|
|
|
bool valid_rotation(const Lardon3DSparseGeometryPose &value) {
|
|
const double *r = value.rotation_cw;
|
|
for (size_t row = 0; row < 3; ++row) {
|
|
for (size_t column = 0; column < 3; ++column) {
|
|
double dot = 0.0;
|
|
for (size_t item = 0; item < 3; ++item)
|
|
dot += r[row * 3 + item] * r[column * 3 + item];
|
|
if (!std::isfinite(dot) ||
|
|
std::abs(dot - (row == column ? 1.0 : 0.0)) >= 1e-6)
|
|
return false;
|
|
}
|
|
}
|
|
const double determinant =
|
|
r[0] * (r[4] * r[8] - r[5] * r[7]) -
|
|
r[1] * (r[3] * r[8] - r[5] * r[6]) +
|
|
r[2] * (r[3] * r[7] - r[4] * r[6]);
|
|
return std::isfinite(determinant) && std::abs(determinant - 1.0) < 1e-6;
|
|
}
|
|
|
|
bool finite_point(const Lardon3DSparseGeometryPoint3 &value) {
|
|
return std::isfinite(value.x) && std::isfinite(value.y) && std::isfinite(value.z);
|
|
}
|
|
|
|
bool camera_center(const Lardon3DSparseGeometryPose &pose, double center[3]) {
|
|
if (!finite_pose(pose) || !valid_rotation(pose)) return false;
|
|
for (size_t column = 0; column < 3; ++column) {
|
|
center[column] = -(pose.rotation_cw[column] * pose.translation_cw[0] +
|
|
pose.rotation_cw[3 + column] * pose.translation_cw[1] +
|
|
pose.rotation_cw[6 + column] * pose.translation_cw[2]);
|
|
if (!std::isfinite(center[column])) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool rotation_to_quaternion(const double rotation[9], double quaternion[4]) {
|
|
const double trace = rotation[0] + rotation[4] + rotation[8];
|
|
if (trace > 0.0) {
|
|
const double scale = 2.0 * std::sqrt(trace + 1.0);
|
|
if (!std::isfinite(scale) || scale == 0.0) return false;
|
|
quaternion[0] = 0.25 * scale;
|
|
quaternion[1] = (rotation[7] - rotation[5]) / scale;
|
|
quaternion[2] = (rotation[2] - rotation[6]) / scale;
|
|
quaternion[3] = (rotation[3] - rotation[1]) / scale;
|
|
} else if (rotation[0] > rotation[4] && rotation[0] > rotation[8]) {
|
|
const double scale = 2.0 * std::sqrt(1.0 + rotation[0] - rotation[4] - rotation[8]);
|
|
if (!std::isfinite(scale) || scale == 0.0) return false;
|
|
quaternion[0] = (rotation[7] - rotation[5]) / scale;
|
|
quaternion[1] = 0.25 * scale;
|
|
quaternion[2] = (rotation[1] + rotation[3]) / scale;
|
|
quaternion[3] = (rotation[2] + rotation[6]) / scale;
|
|
} else if (rotation[4] > rotation[8]) {
|
|
const double scale = 2.0 * std::sqrt(1.0 + rotation[4] - rotation[0] - rotation[8]);
|
|
if (!std::isfinite(scale) || scale == 0.0) return false;
|
|
quaternion[0] = (rotation[2] - rotation[6]) / scale;
|
|
quaternion[1] = (rotation[1] + rotation[3]) / scale;
|
|
quaternion[2] = 0.25 * scale;
|
|
quaternion[3] = (rotation[5] + rotation[7]) / scale;
|
|
} else {
|
|
const double scale = 2.0 * std::sqrt(1.0 + rotation[8] - rotation[0] - rotation[4]);
|
|
if (!std::isfinite(scale) || scale == 0.0) return false;
|
|
quaternion[0] = (rotation[3] - rotation[1]) / scale;
|
|
quaternion[1] = (rotation[2] + rotation[6]) / scale;
|
|
quaternion[2] = (rotation[5] + rotation[7]) / scale;
|
|
quaternion[3] = 0.25 * scale;
|
|
}
|
|
double norm = 0.0;
|
|
for (size_t index = 0; index < 4; ++index)
|
|
norm += quaternion[index] * quaternion[index];
|
|
norm = std::sqrt(norm);
|
|
if (!std::isfinite(norm) || norm == 0.0) return false;
|
|
for (size_t index = 0; index < 4; ++index) quaternion[index] /= norm;
|
|
if (quaternion[0] < 0.0 ||
|
|
(quaternion[0] == 0.0 &&
|
|
(quaternion[1] < 0.0 ||
|
|
(quaternion[1] == 0.0 &&
|
|
(quaternion[2] < 0.0 ||
|
|
(quaternion[2] == 0.0 && quaternion[3] < 0.0))))))
|
|
for (size_t index = 0; index < 4; ++index) quaternion[index] = -quaternion[index];
|
|
return true;
|
|
}
|
|
|
|
bool quaternion_to_pose(const double quaternion_source[4], const double center[3],
|
|
Lardon3DSparseGeometryPose *pose) {
|
|
if (!pose) return false;
|
|
double quaternion[4] = {quaternion_source[0], quaternion_source[1],
|
|
quaternion_source[2], quaternion_source[3]};
|
|
double norm = 0.0;
|
|
for (double value : quaternion) norm += value * value;
|
|
norm = std::sqrt(norm);
|
|
if (!std::isfinite(norm) || norm == 0.0) return false;
|
|
for (double &value : quaternion) value /= norm;
|
|
if (quaternion[0] < 0.0) for (double &value : quaternion) value = -value;
|
|
const double w = quaternion[0];
|
|
const double x = quaternion[1];
|
|
const double y = quaternion[2];
|
|
const double z = quaternion[3];
|
|
double *r = pose->rotation_cw;
|
|
r[0] = 1.0 - 2.0 * (y * y + z * z);
|
|
r[1] = 2.0 * (x * y - z * w);
|
|
r[2] = 2.0 * (x * z + y * w);
|
|
r[3] = 2.0 * (x * y + z * w);
|
|
r[4] = 1.0 - 2.0 * (x * x + z * z);
|
|
r[5] = 2.0 * (y * z - x * w);
|
|
r[6] = 2.0 * (x * z - y * w);
|
|
r[7] = 2.0 * (y * z + x * w);
|
|
r[8] = 1.0 - 2.0 * (x * x + y * y);
|
|
for (size_t row = 0; row < 3; ++row) {
|
|
pose->translation_cw[row] =
|
|
-(r[row * 3] * center[0] + r[row * 3 + 1] * center[1] +
|
|
r[row * 3 + 2] * center[2]);
|
|
}
|
|
return finite_pose(*pose) && valid_rotation(*pose);
|
|
}
|
|
|
|
struct ReprojectionResidual {
|
|
Lardon3DSparseGeometryCalibration calibration;
|
|
double observed_x;
|
|
double observed_y;
|
|
|
|
template <typename T>
|
|
bool operator()(const T *quaternion, const T *center, const T *point,
|
|
T *residual) const {
|
|
const T relative[3] = {point[0] - center[0], point[1] - center[1],
|
|
point[2] - center[2]};
|
|
T camera[3];
|
|
ceres::QuaternionRotatePoint(quaternion, relative, camera);
|
|
if (camera[2] <= T(depth_epsilon)) return false;
|
|
const T xn = camera[0] / camera[2];
|
|
const T yn = camera[1] / camera[2];
|
|
const T r2 = xn * xn + yn * yn;
|
|
const T radial = T(1.0) + T(calibration.k1) * r2 +
|
|
T(calibration.k2) * r2 * r2;
|
|
const T xd = xn * radial + T(2.0 * calibration.p1) * xn * yn +
|
|
T(calibration.p2) * (r2 + T(2.0) * xn * xn);
|
|
const T yd = yn * radial + T(calibration.p1) * (r2 + T(2.0) * yn * yn) +
|
|
T(2.0 * calibration.p2) * xn * yn;
|
|
residual[0] = T(calibration.fx) * xd + T(calibration.cx - observed_x);
|
|
residual[1] = T(calibration.fy) * yd + T(calibration.cy - observed_y);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
bool select_anchors(const std::vector<Lardon3DSparseIncrementalCamera> &cameras,
|
|
uint64_t component_key,
|
|
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic) {
|
|
const Lardon3DSparseIncrementalCamera *pose_anchor = nullptr;
|
|
for (const auto &camera : cameras) {
|
|
if (camera.component_key == component_key &&
|
|
(!pose_anchor || camera.image_id < pose_anchor->image_id))
|
|
pose_anchor = &camera;
|
|
}
|
|
if (!pose_anchor) return false;
|
|
double anchor_center[3];
|
|
if (!camera_center(pose_anchor->pose_cw, anchor_center)) return false;
|
|
const Lardon3DSparseIncrementalCamera *scale_anchor = nullptr;
|
|
double best_distance = -1.0;
|
|
double best_delta[3] = {};
|
|
for (const auto &camera : cameras) {
|
|
if (camera.component_key != component_key || camera.image_id == pose_anchor->image_id)
|
|
continue;
|
|
double center[3];
|
|
if (!camera_center(camera.pose_cw, center)) return false;
|
|
double delta[3] = {center[0] - anchor_center[0], center[1] - anchor_center[1],
|
|
center[2] - anchor_center[2]};
|
|
const double distance = std::hypot(delta[0], delta[1], delta[2]);
|
|
if (!std::isfinite(distance)) return false;
|
|
if (!scale_anchor || distance > best_distance ||
|
|
(distance == best_distance && camera.image_id < scale_anchor->image_id)) {
|
|
scale_anchor = &camera;
|
|
best_distance = distance;
|
|
std::copy(delta, delta + 3, best_delta);
|
|
}
|
|
}
|
|
if (!scale_anchor) return false;
|
|
size_t axis = 0;
|
|
if (std::abs(best_delta[1]) > std::abs(best_delta[axis])) axis = 1;
|
|
if (std::abs(best_delta[2]) > std::abs(best_delta[axis])) axis = 2;
|
|
diagnostic->pose_anchor_image_id = pose_anchor->image_id;
|
|
diagnostic->scale_anchor_image_id = scale_anchor->image_id;
|
|
diagnostic->scale_axis =
|
|
static_cast<Lardon3DSparseBundleAdjustmentScaleAxis>(axis + 1);
|
|
diagnostic->has_anchors = std::abs(best_delta[axis]) > gauge_epsilon;
|
|
return diagnostic->has_anchors;
|
|
}
|
|
|
|
bool termination_accepted(PrivateTermination value) {
|
|
return value == PrivateTermination::converged;
|
|
}
|
|
|
|
bool project(const Lardon3DSparseGeometryCalibration &calibration,
|
|
const Lardon3DSparseGeometryPose &pose,
|
|
const Lardon3DSparseGeometryPoint3 &point,
|
|
Lardon3DSparseGeometryPoint2 *pixel) {
|
|
if (!pixel || !finite_calibration(calibration) || !finite_pose(pose) ||
|
|
!valid_rotation(pose) || !finite_point(point))
|
|
return false;
|
|
const double *r = pose.rotation_cw;
|
|
const double *t = pose.translation_cw;
|
|
const double x = r[0] * point.x + r[1] * point.y + r[2] * point.z + t[0];
|
|
const double y = r[3] * point.x + r[4] * point.y + r[5] * point.z + t[1];
|
|
const double z = r[6] * point.x + r[7] * point.y + r[8] * point.z + t[2];
|
|
if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) ||
|
|
z <= depth_epsilon)
|
|
return false;
|
|
const double xn = x / z;
|
|
const double yn = y / z;
|
|
const double r2 = xn * xn + yn * yn;
|
|
const double radial = 1.0 + calibration.k1 * r2 + calibration.k2 * r2 * r2;
|
|
const double xd = xn * radial + 2.0 * calibration.p1 * xn * yn +
|
|
calibration.p2 * (r2 + 2.0 * xn * xn);
|
|
const double yd = yn * radial + calibration.p1 * (r2 + 2.0 * yn * yn) +
|
|
2.0 * calibration.p2 * xn * yn;
|
|
pixel->x = calibration.fx * xd + calibration.cx;
|
|
pixel->y = calibration.fy * yd + calibration.cy;
|
|
return std::isfinite(pixel->x) && std::isfinite(pixel->y);
|
|
}
|
|
|
|
bool residual_metrics(const double *residuals, size_t count, double *rmse,
|
|
double *huber_cost) {
|
|
if (!residuals || count == 0 || !rmse || !huber_cost || count > SIZE_MAX / 2)
|
|
return false;
|
|
double squared_sum = 0.0;
|
|
double rho_sum = 0.0;
|
|
for (size_t index = 0; index < count; ++index) {
|
|
const double dx = residuals[index * 2];
|
|
const double dy = residuals[index * 2 + 1];
|
|
const double squared = dx * dx + dy * dy;
|
|
if (!std::isfinite(dx) || !std::isfinite(dy) || !std::isfinite(squared))
|
|
return false;
|
|
const double rho = squared <= 4.0 ? squared : 4.0 * std::sqrt(squared) - 4.0;
|
|
squared_sum += squared;
|
|
rho_sum += rho;
|
|
if (!std::isfinite(rho) || !std::isfinite(squared_sum) || !std::isfinite(rho_sum))
|
|
return false;
|
|
}
|
|
*rmse = std::sqrt(squared_sum / static_cast<double>(count));
|
|
*huber_cost = 0.5 * rho_sum;
|
|
return std::isfinite(*rmse) && std::isfinite(*huber_cost);
|
|
}
|
|
|
|
bool cost_acceptable(double initial_cost, double final_cost) {
|
|
if (!std::isfinite(initial_cost) || !std::isfinite(final_cost)) return false;
|
|
const double tolerance = 1e-12 * std::max(1.0, std::abs(initial_cost));
|
|
return std::isfinite(tolerance) && final_cost <= initial_cost + tolerance;
|
|
}
|
|
|
|
bool build_component_views(const Preparation &preparation,
|
|
std::vector<ComponentView> *views) {
|
|
if (!views) return false;
|
|
views->clear();
|
|
views->resize(preparation.components.size());
|
|
for (size_t index = 0; index < preparation.components.size(); ++index) {
|
|
(*views)[index].cameras.reserve(
|
|
static_cast<size_t>(preparation.components[index].registered_image_count));
|
|
(*views)[index].landmarks.reserve(
|
|
static_cast<size_t>(preparation.components[index].landmark_count));
|
|
(*views)[index].observations.reserve(
|
|
static_cast<size_t>(preparation.diagnostics[index].observation_count));
|
|
}
|
|
for (size_t index = 0; index < preparation.cameras.size(); ++index) {
|
|
auto component = std::lower_bound(
|
|
preparation.components.begin(), preparation.components.end(),
|
|
preparation.cameras[index].component_key,
|
|
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
|
if (component == preparation.components.end() ||
|
|
component->component_key != preparation.cameras[index].component_key)
|
|
return false;
|
|
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
|
.cameras.push_back(index);
|
|
}
|
|
for (size_t index = 0; index < preparation.landmarks.size(); ++index) {
|
|
auto component = std::lower_bound(
|
|
preparation.components.begin(), preparation.components.end(),
|
|
preparation.landmarks[index].component_key,
|
|
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
|
if (component == preparation.components.end() ||
|
|
component->component_key != preparation.landmarks[index].component_key)
|
|
return false;
|
|
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
|
.landmarks.push_back(index);
|
|
}
|
|
for (size_t index = 0; index < preparation.observations.size(); ++index) {
|
|
auto landmark = std::lower_bound(
|
|
preparation.landmarks.begin(), preparation.landmarks.end(),
|
|
preparation.observations[index].track_id,
|
|
[](const auto &value, uint64_t key) { return value.track_id < key; });
|
|
if (landmark == preparation.landmarks.end() ||
|
|
landmark->track_id != preparation.observations[index].track_id)
|
|
return false;
|
|
auto component = std::lower_bound(
|
|
preparation.components.begin(), preparation.components.end(),
|
|
landmark->component_key,
|
|
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
|
if (component == preparation.components.end() ||
|
|
component->component_key != landmark->component_key)
|
|
return false;
|
|
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
|
.observations.push_back(index);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
uint32_t structural_underconstraint_mask(
|
|
size_t camera_count, size_t landmark_count, size_t observation_count,
|
|
size_t pose_anchor, std::vector<std::pair<size_t, size_t>> edges) {
|
|
constexpr uint32_t uc1 = 1U << 0;
|
|
constexpr uint32_t uc2 = 1U << 1;
|
|
constexpr uint32_t uc3 = 1U << 2;
|
|
constexpr uint32_t uc4 = 1U << 3;
|
|
uint32_t mask = 0;
|
|
const uint64_t cameras = camera_count;
|
|
const uint64_t landmarks = landmark_count;
|
|
const uint64_t observations = observation_count;
|
|
if (cameras > (UINT64_MAX - 7) / 6 || landmarks > UINT64_MAX / 3 ||
|
|
observations > UINT64_MAX / 2) {
|
|
mask |= uc1;
|
|
} else {
|
|
const uint64_t camera_dof = 6 * cameras;
|
|
const uint64_t landmark_dof = 3 * landmarks;
|
|
if (camera_dof > UINT64_MAX - landmark_dof ||
|
|
camera_dof + landmark_dof < 7 ||
|
|
2 * observations < camera_dof + landmark_dof - 7)
|
|
mask |= uc1;
|
|
}
|
|
std::sort(edges.begin(), edges.end());
|
|
edges.erase(std::unique(edges.begin(), edges.end()), edges.end());
|
|
std::vector<size_t> camera_support(camera_count, 0);
|
|
std::vector<size_t> landmark_support(landmark_count, 0);
|
|
DisjointSet graph(camera_count + landmark_count);
|
|
for (const auto &[camera, landmark] : edges) {
|
|
if (camera >= camera_count || landmark >= landmark_count)
|
|
return uc1 | uc2 | uc3 | uc4;
|
|
++camera_support[camera];
|
|
++landmark_support[landmark];
|
|
graph.join(camera, camera_count + landmark);
|
|
}
|
|
for (size_t support : landmark_support)
|
|
if (support < 2) mask |= uc2;
|
|
if (pose_anchor >= camera_count) return uc1 | uc2 | uc3 | uc4;
|
|
for (size_t index = 0; index < camera_support.size(); ++index)
|
|
if (index != pose_anchor && camera_support[index] < 3) mask |= uc3;
|
|
const size_t root = graph.find(pose_anchor);
|
|
for (size_t index = 0; index < graph.parent.size(); ++index)
|
|
if (graph.find(index) != root) mask |= uc4;
|
|
return mask;
|
|
}
|
|
|
|
UnderconstraintResult underconstraint_result(
|
|
const Preparation &preparation, const ComponentView &view,
|
|
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic) {
|
|
std::vector<std::pair<size_t, size_t>> edges;
|
|
edges.reserve(view.observations.size());
|
|
for (size_t observation_index : view.observations) {
|
|
const auto &observation = preparation.observations[observation_index];
|
|
auto camera = std::lower_bound(
|
|
view.cameras.begin(), view.cameras.end(), observation.image_id,
|
|
[&](size_t index, uint64_t key) {
|
|
return preparation.cameras[index].image_id < key;
|
|
});
|
|
auto landmark = std::lower_bound(
|
|
view.landmarks.begin(), view.landmarks.end(), observation.track_id,
|
|
[&](size_t index, uint64_t key) {
|
|
return preparation.landmarks[index].track_id < key;
|
|
});
|
|
if (camera == view.cameras.end() || landmark == view.landmarks.end() ||
|
|
preparation.cameras[*camera].image_id != observation.image_id ||
|
|
preparation.landmarks[*landmark].track_id != observation.track_id)
|
|
return {false, 0};
|
|
edges.emplace_back(static_cast<size_t>(camera - view.cameras.begin()),
|
|
static_cast<size_t>(landmark - view.landmarks.begin()));
|
|
}
|
|
size_t pose_anchor = view.cameras.size();
|
|
for (size_t index = 0; index < view.cameras.size(); ++index)
|
|
if (preparation.cameras[view.cameras[index]].image_id ==
|
|
diagnostic.pose_anchor_image_id)
|
|
pose_anchor = index;
|
|
if (pose_anchor == view.cameras.size()) return {false, 0};
|
|
return {true, structural_underconstraint_mask(
|
|
view.cameras.size(), view.landmarks.size(),
|
|
view.observations.size(), pose_anchor, std::move(edges))};
|
|
}
|
|
|
|
UnderconstraintAction classify_underconstraint(const UnderconstraintResult &result) {
|
|
if (!result.valid) return UnderconstraintAction::internal_error;
|
|
return result.mask == 0 ? UnderconstraintAction::proceed
|
|
: UnderconstraintAction::reject;
|
|
}
|
|
|
|
bool public_counts_valid(size_t image_count, size_t camera_count,
|
|
size_t landmark_count, size_t result_observation_count,
|
|
size_t input_observation_count) {
|
|
if (image_count > maximum_images || camera_count > maximum_images ||
|
|
landmark_count > maximum_landmarks ||
|
|
result_observation_count > maximum_observations ||
|
|
input_observation_count > maximum_observations)
|
|
return false;
|
|
return result_observation_count <= SIZE_MAX / 2 &&
|
|
result_observation_count <= SIZE_MAX / sizeof(double) / 2;
|
|
}
|
|
|
|
template <typename T>
|
|
void copy_view(std::vector<T> *destination, const T *source, size_t count) {
|
|
destination->clear();
|
|
if (count != 0) destination->assign(source, source + count);
|
|
}
|
|
|
|
PreparationStatus prepare(
|
|
const Lardon3DSparseBundleAdjustmentInput &input, Preparation *preparation,
|
|
const Lardon3DSparseBundleAdjustmentAnchor *anchors = nullptr,
|
|
size_t anchor_count = 0) {
|
|
if (!preparation || !input.incremental_result)
|
|
return PreparationStatus::invalid_argument;
|
|
const auto &result = *input.incremental_result;
|
|
if (result.status < LARDON3D_SPARSE_INCREMENTAL_COMPLETE ||
|
|
result.status > LARDON3D_SPARSE_INCREMENTAL_FAILED ||
|
|
!public_counts_valid(input.image_count, result.camera_count,
|
|
result.landmark_count, result.observation_count,
|
|
input.observation_count) ||
|
|
(input.image_count && !input.images) ||
|
|
(input.observation_count && !input.observations) ||
|
|
(result.component_count && !result.components) ||
|
|
(result.camera_count && !result.cameras) ||
|
|
(result.landmark_count && !result.landmarks) ||
|
|
(result.observation_count && !result.observations))
|
|
return PreparationStatus::invalid_argument;
|
|
try {
|
|
Preparation candidate;
|
|
copy_view(&candidate.images, input.images, input.image_count);
|
|
copy_view(&candidate.components, result.components, result.component_count);
|
|
copy_view(&candidate.cameras, result.cameras, result.camera_count);
|
|
copy_view(&candidate.landmarks, result.landmarks, result.landmark_count);
|
|
copy_view(&candidate.observations, result.observations, result.observation_count);
|
|
std::sort(candidate.images.begin(), candidate.images.end(),
|
|
[](const auto &a, const auto &b) { return a.image_id < b.image_id; });
|
|
|
|
std::vector<ImageIndex> images;
|
|
images.reserve(input.image_count);
|
|
for (size_t index = 0; index < input.image_count; ++index) {
|
|
const auto &image = candidate.images[index];
|
|
if (image.image_id == 0 || !finite_calibration(image.calibration) ||
|
|
(index && candidate.images[index - 1].image_id == image.image_id))
|
|
return PreparationStatus::invalid_argument;
|
|
images.push_back({image.image_id, index});
|
|
}
|
|
|
|
std::vector<ObservationIndex> observation_index;
|
|
observation_index.reserve(input.observation_count);
|
|
for (size_t index = 0; index < input.observation_count; ++index) {
|
|
const auto &observation = input.observations[index];
|
|
if (observation.track_id == 0 || observation.image_id == 0 ||
|
|
observation.feature_set_id == 0 || observation.feature_count == 0 ||
|
|
observation.feature_index >= observation.feature_count ||
|
|
!std::isfinite(observation.x) || !std::isfinite(observation.y))
|
|
return PreparationStatus::invalid_argument;
|
|
auto image = std::lower_bound(
|
|
images.begin(), images.end(), observation.image_id,
|
|
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
|
if (image == images.end() || image->image_id != observation.image_id)
|
|
return PreparationStatus::invalid_argument;
|
|
observation_index.push_back(
|
|
{observation.feature_set_id, observation.feature_index, index});
|
|
}
|
|
std::sort(observation_index.begin(), observation_index.end(),
|
|
[](const auto &a, const auto &b) {
|
|
return std::pair{a.feature_set_id, a.feature_index} <
|
|
std::pair{b.feature_set_id, b.feature_index};
|
|
});
|
|
for (size_t index = 1; index < observation_index.size(); ++index) {
|
|
if (observation_index[index - 1].feature_set_id ==
|
|
observation_index[index].feature_set_id &&
|
|
observation_index[index - 1].feature_index ==
|
|
observation_index[index].feature_index)
|
|
return PreparationStatus::invalid_argument;
|
|
}
|
|
|
|
std::vector<uint64_t> component_camera_counts(result.component_count, 0);
|
|
std::vector<uint64_t> component_landmark_counts(result.component_count, 0);
|
|
for (size_t index = 0; index < result.component_count; ++index) {
|
|
const auto &component = candidate.components[index];
|
|
if (component.component_key == 0 ||
|
|
(index && candidate.components[index - 1].component_key >=
|
|
component.component_key) ||
|
|
component.registered_image_count > component.image_count)
|
|
return PreparationStatus::invalid_argument;
|
|
}
|
|
for (size_t index = 0; index < result.camera_count; ++index) {
|
|
const auto &camera = candidate.cameras[index];
|
|
if (camera.image_id == 0 ||
|
|
(index && candidate.cameras[index - 1].image_id >= camera.image_id) ||
|
|
!finite_pose(camera.pose_cw) || !valid_rotation(camera.pose_cw))
|
|
return PreparationStatus::invalid_argument;
|
|
auto component = std::lower_bound(
|
|
candidate.components.begin(), candidate.components.end(), camera.component_key,
|
|
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
|
auto image = std::lower_bound(
|
|
images.begin(), images.end(), camera.image_id,
|
|
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
|
if (component == candidate.components.end() ||
|
|
component->component_key != camera.component_key || image == images.end() ||
|
|
image->image_id != camera.image_id)
|
|
return PreparationStatus::invalid_argument;
|
|
++component_camera_counts[static_cast<size_t>(component - candidate.components.begin())];
|
|
}
|
|
for (size_t index = 0; index < result.landmark_count; ++index) {
|
|
const auto &landmark = candidate.landmarks[index];
|
|
if (landmark.landmark_id == 0 || landmark.track_id == 0 ||
|
|
landmark.observation_count < 2 || !finite_point(landmark.point) ||
|
|
(index && candidate.landmarks[index - 1].track_id >= landmark.track_id))
|
|
return PreparationStatus::invalid_argument;
|
|
auto component = std::lower_bound(
|
|
candidate.components.begin(), candidate.components.end(), landmark.component_key,
|
|
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
|
if (component == candidate.components.end() ||
|
|
component->component_key != landmark.component_key)
|
|
return PreparationStatus::invalid_argument;
|
|
++component_landmark_counts[static_cast<size_t>(component - candidate.components.begin())];
|
|
}
|
|
for (size_t index = 0; index < result.component_count; ++index) {
|
|
if (candidate.components[index].registered_image_count !=
|
|
component_camera_counts[index] ||
|
|
candidate.components[index].landmark_count != component_landmark_counts[index])
|
|
return PreparationStatus::invalid_argument;
|
|
}
|
|
|
|
candidate.diagnostics.resize(result.component_count);
|
|
std::vector<uint64_t> landmark_observation_counts(result.landmark_count, 0);
|
|
std::vector<uint8_t> published_observations(input.observation_count, 0);
|
|
candidate.resolved_observations.reserve(result.observation_count);
|
|
uint64_t previous_landmark = 0;
|
|
uint32_t previous_position = 0;
|
|
for (size_t index = 0; index < result.observation_count; ++index) {
|
|
const auto &published = candidate.observations[index];
|
|
const bool ordered = index == 0 || published.landmark_id > previous_landmark ||
|
|
(published.landmark_id == previous_landmark &&
|
|
published.position_in_track > previous_position);
|
|
auto landmark = std::lower_bound(
|
|
candidate.landmarks.begin(), candidate.landmarks.end(), published.track_id,
|
|
[](const auto &item, uint64_t key) { return item.track_id < key; });
|
|
auto observation = std::lower_bound(
|
|
observation_index.begin(), observation_index.end(),
|
|
std::pair{published.feature_set_id, published.feature_index},
|
|
[](const auto &item, const auto &key) {
|
|
return std::pair{item.feature_set_id, item.feature_index} < key;
|
|
});
|
|
auto image = std::lower_bound(
|
|
images.begin(), images.end(), published.image_id,
|
|
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
|
if (!ordered || landmark == candidate.landmarks.end() ||
|
|
landmark->landmark_id != published.landmark_id ||
|
|
landmark->track_id != published.track_id || observation == observation_index.end() ||
|
|
observation->feature_set_id != published.feature_set_id ||
|
|
observation->feature_index != published.feature_index || image == images.end() ||
|
|
image->image_id != published.image_id)
|
|
return PreparationStatus::invalid_argument;
|
|
const auto &source = input.observations[observation->index];
|
|
if (published_observations[observation->index])
|
|
return PreparationStatus::invalid_argument;
|
|
published_observations[observation->index] = 1;
|
|
auto camera = std::lower_bound(
|
|
candidate.cameras.begin(), candidate.cameras.end(), published.image_id,
|
|
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
|
if (source.track_id != published.track_id || source.image_id != published.image_id ||
|
|
camera == candidate.cameras.end() || camera->image_id != published.image_id ||
|
|
camera->component_key != landmark->component_key)
|
|
return PreparationStatus::invalid_argument;
|
|
const size_t landmark_index = static_cast<size_t>(landmark - candidate.landmarks.begin());
|
|
auto component = std::lower_bound(
|
|
candidate.components.begin(), candidate.components.end(), landmark->component_key,
|
|
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
|
const size_t component_index =
|
|
static_cast<size_t>(component - candidate.components.begin());
|
|
++landmark_observation_counts[landmark_index];
|
|
++candidate.diagnostics[component_index].observation_count;
|
|
candidate.resolved_observations.push_back({source, image->index});
|
|
previous_landmark = published.landmark_id;
|
|
previous_position = published.position_in_track;
|
|
}
|
|
for (size_t index = 0; index < candidate.landmarks.size(); ++index) {
|
|
if (candidate.landmarks[index].observation_count !=
|
|
landmark_observation_counts[index])
|
|
return PreparationStatus::invalid_argument;
|
|
}
|
|
for (size_t index = 0; index < candidate.components.size(); ++index) {
|
|
auto &diagnostic = candidate.diagnostics[index];
|
|
const auto &component = candidate.components[index];
|
|
diagnostic.component_key = component.component_key;
|
|
diagnostic.camera_count = component.registered_image_count;
|
|
diagnostic.landmark_count = component.landmark_count;
|
|
diagnostic.termination = LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NOT_RUN;
|
|
diagnostic.rejection_reason =
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE;
|
|
const Lardon3DSparseBundleAdjustmentAnchor *override = nullptr;
|
|
for (size_t anchor_index = 0; anchor_index < anchor_count; ++anchor_index) {
|
|
if (anchors[anchor_index].component_key == component.component_key) {
|
|
if (override) return PreparationStatus::invalid_argument;
|
|
override = &anchors[anchor_index];
|
|
}
|
|
}
|
|
bool anchors_valid = false;
|
|
if (override) {
|
|
const auto pose = std::find_if(candidate.cameras.begin(), candidate.cameras.end(),
|
|
[&](const auto &camera) {
|
|
return camera.component_key == component.component_key &&
|
|
camera.image_id ==
|
|
override->pose_anchor_image_id;
|
|
});
|
|
const auto scale = std::find_if(candidate.cameras.begin(), candidate.cameras.end(),
|
|
[&](const auto &camera) {
|
|
return camera.component_key == component.component_key &&
|
|
camera.image_id ==
|
|
override->scale_anchor_image_id;
|
|
});
|
|
anchors_valid = pose != candidate.cameras.end() &&
|
|
scale != candidate.cameras.end() && pose != scale &&
|
|
override->scale_axis >=
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X &&
|
|
override->scale_axis <=
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_Z;
|
|
if (anchors_valid) {
|
|
diagnostic.pose_anchor_image_id = override->pose_anchor_image_id;
|
|
diagnostic.scale_anchor_image_id = override->scale_anchor_image_id;
|
|
diagnostic.scale_axis = override->scale_axis;
|
|
diagnostic.has_anchors = true;
|
|
}
|
|
} else {
|
|
anchors_valid = select_anchors(candidate.cameras, component.component_key,
|
|
&diagnostic);
|
|
}
|
|
diagnostic.eligible = component.registered_image_count >= 2 &&
|
|
component.landmark_count >= 1 && anchors_valid;
|
|
if (diagnostic.eligible)
|
|
diagnostic.rejection_reason = LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE;
|
|
else if (!diagnostic.has_anchors && component.registered_image_count >= 2)
|
|
diagnostic.rejection_reason =
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE;
|
|
}
|
|
*preparation = std::move(candidate);
|
|
return PreparationStatus::prepared;
|
|
} catch (const std::bad_alloc &) {
|
|
return PreparationStatus::out_of_memory;
|
|
} catch (...) {
|
|
return PreparationStatus::internal_error;
|
|
}
|
|
}
|
|
|
|
bool component_metrics(
|
|
const Preparation &preparation, const ComponentView &view,
|
|
const std::vector<Lardon3DSparseIncrementalCamera> &cameras,
|
|
const std::vector<Lardon3DSparseIncrementalLandmark> &landmarks,
|
|
double *rmse, double *cost) {
|
|
std::vector<double> residuals;
|
|
if (view.observations.size() > SIZE_MAX / 2) return false;
|
|
residuals.resize(view.observations.size() * 2);
|
|
for (size_t local = 0; local < view.observations.size(); ++local) {
|
|
const size_t observation_index = view.observations[local];
|
|
const auto &observation = preparation.observations[observation_index];
|
|
auto camera = std::lower_bound(
|
|
cameras.begin(), cameras.end(), observation.image_id,
|
|
[](const auto &value, uint64_t key) { return value.image_id < key; });
|
|
auto landmark = std::lower_bound(
|
|
landmarks.begin(), landmarks.end(), observation.track_id,
|
|
[](const auto &value, uint64_t key) { return value.track_id < key; });
|
|
if (camera == cameras.end() || camera->image_id != observation.image_id ||
|
|
landmark == landmarks.end() || landmark->track_id != observation.track_id)
|
|
return false;
|
|
const auto &resolved = preparation.resolved_observations[observation_index];
|
|
Lardon3DSparseGeometryPoint2 predicted = {};
|
|
if (!project(preparation.images[resolved.image_index].calibration,
|
|
camera->pose_cw, landmark->point, &predicted))
|
|
return false;
|
|
residuals[local * 2] = predicted.x - resolved.source.x;
|
|
residuals[local * 2 + 1] = predicted.y - resolved.source.y;
|
|
}
|
|
return residual_metrics(residuals.data(), view.observations.size(), rmse, cost);
|
|
}
|
|
|
|
bool build_public_candidate(
|
|
const Preparation &preparation,
|
|
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic,
|
|
const std::vector<SolverCamera> &cameras,
|
|
const std::vector<SolverLandmark> &landmarks,
|
|
std::vector<Lardon3DSparseIncrementalCamera> *candidate_cameras,
|
|
std::vector<Lardon3DSparseIncrementalLandmark> *candidate_landmarks) {
|
|
if (!candidate_cameras || !candidate_landmarks) return false;
|
|
*candidate_cameras = preparation.cameras;
|
|
*candidate_landmarks = preparation.landmarks;
|
|
for (size_t index = 0; index < cameras.size(); ++index) {
|
|
const size_t source_index = cameras[index].source_index;
|
|
if (preparation.cameras[source_index].image_id ==
|
|
diagnostic.pose_anchor_image_id)
|
|
continue;
|
|
if (!quaternion_to_pose(
|
|
cameras[index].quaternion, cameras[index].center,
|
|
&(*candidate_cameras)[source_index].pose_cw))
|
|
return false;
|
|
}
|
|
for (const auto &landmark : landmarks) {
|
|
const Lardon3DSparseGeometryPoint3 point = {
|
|
landmark.point[0], landmark.point[1], landmark.point[2]};
|
|
if (!finite_point(point)) return false;
|
|
(*candidate_landmarks)[landmark.source_index].point = point;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
PrivateTermination translate_termination(ceres::TerminationType value) {
|
|
if (value == ceres::CONVERGENCE) return PrivateTermination::converged;
|
|
if (value == ceres::NO_CONVERGENCE) return PrivateTermination::no_convergence;
|
|
return PrivateTermination::failure;
|
|
}
|
|
|
|
Lardon3DSparseBundleAdjustmentTermination public_termination(
|
|
PrivateTermination value) {
|
|
if (value == PrivateTermination::converged)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED;
|
|
if (value == PrivateTermination::no_convergence)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE;
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_FAILURE;
|
|
}
|
|
|
|
CandidateDecision validate_candidate(PrivateTermination termination,
|
|
bool final_metrics_valid,
|
|
bool gauge_valid,
|
|
double initial_cost,
|
|
double final_cost) {
|
|
if (!termination_accepted(termination)) {
|
|
return {false, false,
|
|
termination == PrivateTermination::no_convergence
|
|
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE
|
|
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE};
|
|
}
|
|
if (!final_metrics_valid || !gauge_valid)
|
|
return {false, false,
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE};
|
|
if (!cost_acceptable(initial_cost, final_cost))
|
|
return {false, true,
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION};
|
|
return {true, true, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE};
|
|
}
|
|
|
|
void apply_component_decision(
|
|
const CandidateDecision &decision, const ComponentView &view,
|
|
const std::vector<Lardon3DSparseIncrementalCamera> &candidate_cameras,
|
|
const std::vector<Lardon3DSparseIncrementalLandmark> &candidate_landmarks,
|
|
std::vector<Lardon3DSparseIncrementalCamera> *published_cameras,
|
|
std::vector<Lardon3DSparseIncrementalLandmark> *published_landmarks) {
|
|
if (!decision.accepted) return;
|
|
for (size_t source_index : view.cameras)
|
|
(*published_cameras)[source_index] = candidate_cameras[source_index];
|
|
for (size_t source_index : view.landmarks)
|
|
(*published_landmarks)[source_index] = candidate_landmarks[source_index];
|
|
}
|
|
|
|
bool configure_parameter_blocks(
|
|
ceres::Problem *problem, ceres::ParameterBlockOrdering *ordering,
|
|
const Preparation &preparation,
|
|
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic,
|
|
std::vector<SolverCamera> *cameras,
|
|
std::vector<SolverLandmark> *landmarks,
|
|
std::vector<ParameterRecord> *records) {
|
|
if (!problem || !ordering || !cameras || !landmarks) return false;
|
|
if (records) records->clear();
|
|
for (auto &landmark : *landmarks) {
|
|
problem->AddParameterBlock(landmark.point, 3);
|
|
ordering->AddElementToGroup(landmark.point, 0);
|
|
if (records)
|
|
records->push_back({ParameterKind::landmark,
|
|
preparation.landmarks[landmark.source_index].track_id,
|
|
0, false, -1});
|
|
}
|
|
for (auto &camera : *cameras) {
|
|
const uint64_t image_id = preparation.cameras[camera.source_index].image_id;
|
|
problem->AddParameterBlock(camera.quaternion, 4,
|
|
new ceres::QuaternionManifold);
|
|
problem->AddParameterBlock(camera.center, 3);
|
|
ordering->AddElementToGroup(camera.quaternion, 1);
|
|
ordering->AddElementToGroup(camera.center, 1);
|
|
if (records) {
|
|
records->push_back({ParameterKind::camera_quaternion, image_id, 1,
|
|
image_id == diagnostic.pose_anchor_image_id, -1});
|
|
records->push_back({ParameterKind::camera_center, image_id, 1,
|
|
image_id == diagnostic.pose_anchor_image_id,
|
|
image_id == diagnostic.scale_anchor_image_id
|
|
? static_cast<int>(diagnostic.scale_axis) - 1
|
|
: -1});
|
|
}
|
|
}
|
|
auto pose_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) {
|
|
return preparation.cameras[camera.source_index].image_id ==
|
|
diagnostic.pose_anchor_image_id;
|
|
});
|
|
auto scale_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) {
|
|
return preparation.cameras[camera.source_index].image_id ==
|
|
diagnostic.scale_anchor_image_id;
|
|
});
|
|
if (pose_anchor == cameras->end() || scale_anchor == cameras->end()) return false;
|
|
problem->SetParameterBlockConstant(pose_anchor->quaternion);
|
|
problem->SetParameterBlockConstant(pose_anchor->center);
|
|
const int scale_axis = static_cast<int>(diagnostic.scale_axis) - 1;
|
|
problem->SetManifold(scale_anchor->center,
|
|
new ceres::SubsetManifold(3, {scale_axis}));
|
|
return true;
|
|
}
|
|
|
|
bool solve_component(
|
|
const Preparation &preparation, const ComponentView &view,
|
|
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic,
|
|
std::vector<Lardon3DSparseIncrementalCamera> *published_cameras,
|
|
std::vector<Lardon3DSparseIncrementalLandmark> *published_landmarks) {
|
|
if (!diagnostic || !published_cameras || !published_landmarks) return false;
|
|
std::vector<SolverCamera> cameras;
|
|
std::vector<SolverLandmark> landmarks;
|
|
std::vector<SolverObservation> observations;
|
|
cameras.reserve(view.cameras.size());
|
|
landmarks.reserve(view.landmarks.size());
|
|
observations.reserve(view.observations.size());
|
|
|
|
for (size_t source_index : view.cameras) {
|
|
const auto &source = preparation.cameras[source_index];
|
|
auto image = std::lower_bound(
|
|
preparation.images.begin(), preparation.images.end(), source.image_id,
|
|
[](const auto &value, uint64_t key) { return value.image_id < key; });
|
|
SolverCamera camera = {};
|
|
camera.source_index = source_index;
|
|
camera.image_index = static_cast<size_t>(image - preparation.images.begin());
|
|
if (image == preparation.images.end() || image->image_id != source.image_id ||
|
|
!rotation_to_quaternion(source.pose_cw.rotation_cw, camera.quaternion) ||
|
|
!camera_center(source.pose_cw, camera.center))
|
|
return false;
|
|
std::copy(camera.center, camera.center + 3, camera.initial_center);
|
|
cameras.push_back(camera);
|
|
}
|
|
for (size_t source_index : view.landmarks) {
|
|
const auto &source = preparation.landmarks[source_index];
|
|
landmarks.push_back(
|
|
{source_index, {source.point.x, source.point.y, source.point.z}});
|
|
}
|
|
for (size_t resolved_index : view.observations) {
|
|
const auto &source = preparation.observations[resolved_index];
|
|
auto camera = std::lower_bound(
|
|
cameras.begin(), cameras.end(), source.image_id,
|
|
[&](const auto &value, uint64_t key) {
|
|
return preparation.cameras[value.source_index].image_id < key;
|
|
});
|
|
auto landmark = std::lower_bound(
|
|
landmarks.begin(), landmarks.end(), source.track_id,
|
|
[&](const auto &value, uint64_t key) {
|
|
return preparation.landmarks[value.source_index].track_id < key;
|
|
});
|
|
if (camera == cameras.end() || landmark == landmarks.end()) return false;
|
|
observations.push_back({static_cast<size_t>(camera - cameras.begin()),
|
|
static_cast<size_t>(landmark - landmarks.begin()),
|
|
resolved_index});
|
|
}
|
|
|
|
if (!component_metrics(preparation, view, preparation.cameras,
|
|
preparation.landmarks,
|
|
&diagnostic->initial_reprojection_rmse_px,
|
|
&diagnostic->initial_robust_cost)) {
|
|
diagnostic->rejection_reason =
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE;
|
|
return true;
|
|
}
|
|
ceres::Problem problem;
|
|
auto ordering = std::make_shared<ceres::ParameterBlockOrdering>();
|
|
if (!configure_parameter_blocks(&problem, ordering.get(), preparation,
|
|
*diagnostic, &cameras, &landmarks, nullptr))
|
|
return false;
|
|
|
|
auto *loss = new ceres::HuberLoss(2.0);
|
|
for (const auto &observation : observations) {
|
|
const auto &resolved = preparation.resolved_observations[observation.resolved_index];
|
|
const auto &calibration = preparation.images[cameras[observation.camera_index]
|
|
.image_index].calibration;
|
|
auto *cost = new ceres::AutoDiffCostFunction<ReprojectionResidual, 2, 4, 3, 3>(
|
|
new ReprojectionResidual{calibration, resolved.source.x, resolved.source.y});
|
|
problem.AddResidualBlock(cost, loss,
|
|
cameras[observation.camera_index].quaternion,
|
|
cameras[observation.camera_index].center,
|
|
landmarks[observation.landmark_index].point);
|
|
}
|
|
|
|
auto pose_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) {
|
|
return preparation.cameras[camera.source_index].image_id ==
|
|
diagnostic->pose_anchor_image_id;
|
|
});
|
|
auto scale_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) {
|
|
return preparation.cameras[camera.source_index].image_id ==
|
|
diagnostic->scale_anchor_image_id;
|
|
});
|
|
if (pose_anchor == cameras.end() || scale_anchor == cameras.end()) return false;
|
|
const int scale_axis = static_cast<int>(diagnostic->scale_axis) - 1;
|
|
|
|
ceres::Solver::Options options;
|
|
options.minimizer_type = ceres::TRUST_REGION;
|
|
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
|
|
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
|
|
options.preconditioner_type = ceres::SCHUR_JACOBI;
|
|
options.num_threads = 1;
|
|
options.max_num_iterations = 50;
|
|
options.function_tolerance = 1e-6;
|
|
options.gradient_tolerance = 1e-10;
|
|
options.parameter_tolerance = 1e-8;
|
|
options.linear_solver_ordering = std::move(ordering);
|
|
ceres::Solver::Summary summary;
|
|
ceres::Solve(options, &problem, &summary);
|
|
|
|
const PrivateTermination termination = translate_termination(summary.termination_type);
|
|
diagnostic->termination = public_termination(termination);
|
|
diagnostic->iteration_count = summary.iterations.size() > UINT32_MAX
|
|
? UINT32_MAX
|
|
: static_cast<uint32_t>(summary.iterations.size());
|
|
std::vector<Lardon3DSparseIncrementalCamera> candidate_cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> candidate_landmarks;
|
|
bool candidate_valid = false;
|
|
if (termination_accepted(termination) &&
|
|
build_public_candidate(preparation, *diagnostic, cameras, landmarks,
|
|
&candidate_cameras, &candidate_landmarks)) {
|
|
candidate_valid = component_metrics(
|
|
preparation, view, candidate_cameras, candidate_landmarks,
|
|
&diagnostic->final_reprojection_rmse_px,
|
|
&diagnostic->final_robust_cost);
|
|
}
|
|
const bool gauge_valid =
|
|
scale_anchor->center[scale_axis] == scale_anchor->initial_center[scale_axis];
|
|
const CandidateDecision decision = validate_candidate(
|
|
termination, candidate_valid, gauge_valid, diagnostic->initial_robust_cost,
|
|
diagnostic->final_robust_cost);
|
|
diagnostic->has_costs = decision.has_metrics;
|
|
diagnostic->has_rmse = decision.has_metrics;
|
|
diagnostic->accepted = decision.accepted;
|
|
diagnostic->rejection_reason = decision.rejection_reason;
|
|
apply_component_decision(decision, view, candidate_cameras,
|
|
candidate_landmarks, published_cameras,
|
|
published_landmarks);
|
|
return true;
|
|
}
|
|
|
|
template <typename T>
|
|
bool allocate_copy(const std::vector<T> &source, T **destination) {
|
|
*destination = nullptr;
|
|
if (source.empty()) return true;
|
|
if (source.size() > SIZE_MAX / sizeof(T)) return false;
|
|
*destination = static_cast<T *>(std::malloc(source.size() * sizeof(T)));
|
|
if (!*destination) return false;
|
|
std::copy(source.begin(), source.end(), *destination);
|
|
return true;
|
|
}
|
|
|
|
void destroy_result(Lardon3DSparseBundleAdjustmentResult *result) {
|
|
if (!result) return;
|
|
std::free(result->components);
|
|
std::free(result->cameras);
|
|
std::free(result->landmarks);
|
|
std::free(result->observations);
|
|
std::free(result->diagnostics);
|
|
*result = {};
|
|
}
|
|
|
|
} // namespace lardon3d::sparse_bundle_adjustment
|
|
|
|
static Lardon3DSparseBundleAdjustmentExecutionStatus run_bundle_adjustment(
|
|
const Lardon3DSparseBundleAdjustmentInput *input,
|
|
const Lardon3DSparseBundleAdjustmentAnchor *anchors, size_t anchor_count,
|
|
Lardon3DSparseBundleAdjustmentResult *result) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!result) return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT;
|
|
destroy_result(result);
|
|
if (!input || (anchor_count != 0 && !anchors))
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT;
|
|
try {
|
|
Preparation preparation;
|
|
const PreparationStatus preparation_status =
|
|
prepare(*input, &preparation, anchors, anchor_count);
|
|
if (preparation_status == PreparationStatus::invalid_argument)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT;
|
|
if (preparation_status == PreparationStatus::out_of_memory)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY;
|
|
if (preparation_status == PreparationStatus::internal_error)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
|
|
|
std::vector<ComponentView> views;
|
|
if (!build_component_views(preparation, &views))
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
|
std::vector<Lardon3DSparseIncrementalCamera> published_cameras =
|
|
preparation.cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> published_landmarks =
|
|
preparation.landmarks;
|
|
size_t accepted = 0;
|
|
size_t rejected_eligible = 0;
|
|
for (size_t index = 0; index < views.size(); ++index) {
|
|
auto &diagnostic = preparation.diagnostics[index];
|
|
if (!diagnostic.eligible) continue;
|
|
const UnderconstraintResult underconstraint =
|
|
underconstraint_result(preparation, views[index], diagnostic);
|
|
const UnderconstraintAction underconstraint_action =
|
|
classify_underconstraint(underconstraint);
|
|
if (underconstraint_action == UnderconstraintAction::internal_error)
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
|
if (underconstraint_action == UnderconstraintAction::reject) {
|
|
diagnostic.eligible = false;
|
|
diagnostic.rejection_reason =
|
|
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED;
|
|
continue;
|
|
}
|
|
if (!solve_component(preparation, views[index], &diagnostic,
|
|
&published_cameras, &published_landmarks))
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
|
if (diagnostic.accepted)
|
|
++accepted;
|
|
else
|
|
++rejected_eligible;
|
|
}
|
|
|
|
Lardon3DSparseBundleAdjustmentResult candidate = {};
|
|
candidate.status = accepted == 0
|
|
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED
|
|
: rejected_eligible == 0
|
|
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE
|
|
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL;
|
|
candidate.component_count = preparation.components.size();
|
|
candidate.camera_count = published_cameras.size();
|
|
candidate.landmark_count = published_landmarks.size();
|
|
candidate.observation_count = preparation.observations.size();
|
|
if (!allocate_copy(preparation.components, &candidate.components) ||
|
|
!allocate_copy(published_cameras, &candidate.cameras) ||
|
|
!allocate_copy(published_landmarks, &candidate.landmarks) ||
|
|
!allocate_copy(preparation.observations, &candidate.observations) ||
|
|
!allocate_copy(preparation.diagnostics, &candidate.diagnostics)) {
|
|
destroy_result(&candidate);
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY;
|
|
}
|
|
*result = candidate;
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK;
|
|
} catch (const std::bad_alloc &) {
|
|
destroy_result(result);
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY;
|
|
} catch (...) {
|
|
destroy_result(result);
|
|
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
|
}
|
|
}
|
|
|
|
extern "C" Lardon3DSparseBundleAdjustmentExecutionStatus
|
|
lardon3d_sparse_bundle_adjustment_run(
|
|
const Lardon3DSparseBundleAdjustmentInput *input,
|
|
Lardon3DSparseBundleAdjustmentResult *result) {
|
|
return run_bundle_adjustment(input, nullptr, 0, result);
|
|
}
|
|
|
|
Lardon3DSparseBundleAdjustmentExecutionStatus
|
|
lardon3d_sparse_bundle_adjustment_run_with_anchors(
|
|
const Lardon3DSparseBundleAdjustmentInput *input,
|
|
const Lardon3DSparseBundleAdjustmentAnchor *anchors, size_t anchor_count,
|
|
Lardon3DSparseBundleAdjustmentResult *result) {
|
|
return run_bundle_adjustment(input, anchors, anchor_count, result);
|
|
}
|
|
|
|
extern "C" void lardon3d_sparse_bundle_adjustment_result_destroy(
|
|
Lardon3DSparseBundleAdjustmentResult *result) {
|
|
lardon3d::sparse_bundle_adjustment::destroy_result(result);
|
|
}
|
|
|
|
#ifdef LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TESTING
|
|
extern "C" int lardon3d_sparse_bundle_adjustment_test_prepare(
|
|
const Lardon3DSparseBundleAdjustmentInput *input,
|
|
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostics,
|
|
size_t diagnostic_capacity, Lardon3DSparseIncrementalObservation *resolved,
|
|
size_t resolved_capacity, size_t *diagnostic_count, size_t *resolved_count) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!input || !diagnostic_count || !resolved_count) return 1;
|
|
Preparation preparation;
|
|
const auto status = prepare(*input, &preparation);
|
|
if (status != PreparationStatus::prepared)
|
|
return status == PreparationStatus::out_of_memory ? 2 : 1;
|
|
*diagnostic_count = preparation.diagnostics.size();
|
|
*resolved_count = preparation.resolved_observations.size();
|
|
if ((preparation.diagnostics.size() > diagnostic_capacity) ||
|
|
(preparation.resolved_observations.size() > resolved_capacity) ||
|
|
(!preparation.diagnostics.empty() && !diagnostics) ||
|
|
(!preparation.resolved_observations.empty() && !resolved))
|
|
return 1;
|
|
std::copy(preparation.diagnostics.begin(), preparation.diagnostics.end(), diagnostics);
|
|
for (size_t index = 0; index < preparation.resolved_observations.size(); ++index)
|
|
resolved[index] = preparation.resolved_observations[index].source;
|
|
return 0;
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_project(
|
|
const Lardon3DSparseGeometryCalibration *calibration,
|
|
const Lardon3DSparseGeometryPose *pose,
|
|
const Lardon3DSparseGeometryPoint3 *point,
|
|
Lardon3DSparseGeometryPoint2 *pixel) {
|
|
return calibration && pose && point &&
|
|
lardon3d::sparse_bundle_adjustment::project(*calibration, *pose, *point, pixel);
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_metrics(
|
|
const double *residuals, size_t count, double *rmse, double *huber_cost) {
|
|
return lardon3d::sparse_bundle_adjustment::residual_metrics(
|
|
residuals, count, rmse, huber_cost);
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_cost_acceptable(
|
|
double initial_cost, double final_cost) {
|
|
return lardon3d::sparse_bundle_adjustment::cost_acceptable(initial_cost, final_cost);
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_termination_accepted(
|
|
int value) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (value < 0 || value > 2) return false;
|
|
return termination_accepted(static_cast<PrivateTermination>(value));
|
|
}
|
|
|
|
extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_underconstraint(
|
|
const Lardon3DSparseBundleAdjustmentInput *input, size_t component_index) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!input) return UINT32_MAX;
|
|
Preparation preparation;
|
|
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
|
component_index >= preparation.components.size())
|
|
return UINT32_MAX;
|
|
std::vector<ComponentView> views;
|
|
if (!build_component_views(preparation, &views)) return UINT32_MAX;
|
|
const UnderconstraintResult result = underconstraint_result(
|
|
preparation, views[component_index], preparation.diagnostics[component_index]);
|
|
return result.valid ? result.mask : UINT32_MAX;
|
|
}
|
|
|
|
extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
|
size_t camera_count, size_t landmark_count, size_t observation_count,
|
|
size_t pose_anchor, const size_t *camera_indices,
|
|
const size_t *landmark_indices, size_t edge_count) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (edge_count != 0 && (!camera_indices || !landmark_indices)) return UINT32_MAX;
|
|
std::vector<std::pair<size_t, size_t>> edges;
|
|
edges.reserve(edge_count);
|
|
for (size_t index = 0; index < edge_count; ++index)
|
|
edges.emplace_back(camera_indices[index], landmark_indices[index]);
|
|
return structural_underconstraint_mask(camera_count, landmark_count,
|
|
observation_count, pose_anchor,
|
|
std::move(edges));
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_decision(
|
|
int termination, bool final_metrics_valid, bool gauge_valid,
|
|
double initial_cost, double final_cost, bool *has_metrics,
|
|
Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (termination < 0 || termination > 2 || !has_metrics || !reason) return false;
|
|
const CandidateDecision decision = validate_candidate(
|
|
static_cast<PrivateTermination>(termination), final_metrics_valid,
|
|
gauge_valid, initial_cost, final_cost);
|
|
*has_metrics = decision.has_metrics;
|
|
*reason = decision.rejection_reason;
|
|
return decision.accepted;
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_counts_valid(
|
|
size_t image_count, size_t camera_count, size_t landmark_count,
|
|
size_t result_observation_count, size_t input_observation_count) {
|
|
return lardon3d::sparse_bundle_adjustment::public_counts_valid(
|
|
image_count, camera_count, landmark_count, result_observation_count,
|
|
input_observation_count);
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_publication(
|
|
bool final_metrics_valid, double initial_cost, double final_cost,
|
|
double original_x, double candidate_x, double *published_x,
|
|
bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!published_x || !has_metrics || !reason) return false;
|
|
const CandidateDecision decision = validate_candidate(
|
|
PrivateTermination::converged, final_metrics_valid, true, initial_cost,
|
|
final_cost);
|
|
ComponentView view;
|
|
view.landmarks.push_back(0);
|
|
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> published(1);
|
|
std::vector<Lardon3DSparseIncrementalLandmark> candidate(1);
|
|
published[0].point.x = original_x;
|
|
candidate[0].point.x = candidate_x;
|
|
apply_component_decision(decision, view, cameras, candidate, &cameras,
|
|
&published);
|
|
*published_x = published[0].point.x;
|
|
*has_metrics = decision.has_metrics;
|
|
*reason = decision.rejection_reason;
|
|
return decision.accepted;
|
|
}
|
|
|
|
extern "C" int lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
Preparation preparation;
|
|
preparation.cameras.resize(1);
|
|
preparation.cameras[0].image_id = 10;
|
|
preparation.landmarks.resize(1);
|
|
preparation.landmarks[0].track_id = 20;
|
|
preparation.observations.resize(1);
|
|
preparation.observations[0].image_id = 99;
|
|
preparation.observations[0].track_id = 20;
|
|
ComponentView view;
|
|
view.cameras.push_back(0);
|
|
view.landmarks.push_back(0);
|
|
view.observations.push_back(0);
|
|
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostic = {};
|
|
diagnostic.pose_anchor_image_id = 10;
|
|
const UnderconstraintAction action = classify_underconstraint(
|
|
underconstraint_result(preparation, view, diagnostic));
|
|
return action == UnderconstraintAction::internal_error
|
|
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR
|
|
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK;
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_parameter_ordering(
|
|
const Lardon3DSparseBundleAdjustmentInput *input, int *kinds,
|
|
uint64_t *identities, int *groups, bool *constants, int *subset_axes,
|
|
size_t capacity, size_t *count) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!input || !count) return false;
|
|
Preparation preparation;
|
|
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
|
preparation.components.size() != 1)
|
|
return false;
|
|
std::vector<ComponentView> views;
|
|
if (!build_component_views(preparation, &views)) return false;
|
|
std::vector<SolverCamera> cameras;
|
|
std::vector<SolverLandmark> landmarks;
|
|
for (size_t source_index : views[0].cameras) {
|
|
SolverCamera camera = {};
|
|
camera.source_index = source_index;
|
|
if (!rotation_to_quaternion(preparation.cameras[source_index].pose_cw.rotation_cw,
|
|
camera.quaternion) ||
|
|
!camera_center(preparation.cameras[source_index].pose_cw, camera.center))
|
|
return false;
|
|
cameras.push_back(camera);
|
|
}
|
|
for (size_t source_index : views[0].landmarks) {
|
|
const auto &point = preparation.landmarks[source_index].point;
|
|
landmarks.push_back({source_index, {point.x, point.y, point.z}});
|
|
}
|
|
ceres::Problem problem;
|
|
ceres::ParameterBlockOrdering ordering;
|
|
std::vector<ParameterRecord> records;
|
|
if (!configure_parameter_blocks(&problem, &ordering, preparation,
|
|
preparation.diagnostics[0], &cameras,
|
|
&landmarks, &records))
|
|
return false;
|
|
*count = records.size();
|
|
if (records.size() > capacity ||
|
|
(!records.empty() &&
|
|
(!kinds || !identities || !groups || !constants || !subset_axes)))
|
|
return false;
|
|
for (size_t index = 0; index < records.size(); ++index) {
|
|
kinds[index] = static_cast<int>(records[index].kind);
|
|
identities[index] = records[index].identity;
|
|
groups[index] = records[index].group;
|
|
constants[index] = records[index].constant;
|
|
subset_axes[index] = records[index].subset_axis;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
extern "C" bool lardon3d_sparse_bundle_adjustment_test_invalid_candidate(
|
|
const Lardon3DSparseBundleAdjustmentInput *input, double *published_z,
|
|
bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
|
using namespace lardon3d::sparse_bundle_adjustment;
|
|
if (!input || !published_z || !has_metrics || !reason) return false;
|
|
Preparation preparation;
|
|
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
|
preparation.components.size() != 1 || preparation.landmarks.empty())
|
|
return false;
|
|
std::vector<ComponentView> views;
|
|
if (!build_component_views(preparation, &views)) return false;
|
|
std::vector<Lardon3DSparseIncrementalCamera> candidate_cameras =
|
|
preparation.cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> candidate_landmarks =
|
|
preparation.landmarks;
|
|
candidate_landmarks[views[0].landmarks[0]].point.z = -1.0;
|
|
double initial_rmse = 0.0;
|
|
double initial_cost = 0.0;
|
|
double final_rmse = 0.0;
|
|
double final_cost = 0.0;
|
|
if (!component_metrics(preparation, views[0], preparation.cameras,
|
|
preparation.landmarks, &initial_rmse, &initial_cost))
|
|
return false;
|
|
const bool final_valid = component_metrics(
|
|
preparation, views[0], candidate_cameras, candidate_landmarks,
|
|
&final_rmse, &final_cost);
|
|
const CandidateDecision decision = validate_candidate(
|
|
PrivateTermination::converged, final_valid, true, initial_cost, final_cost);
|
|
std::vector<Lardon3DSparseIncrementalCamera> published_cameras =
|
|
preparation.cameras;
|
|
std::vector<Lardon3DSparseIncrementalLandmark> published_landmarks =
|
|
preparation.landmarks;
|
|
apply_component_decision(decision, views[0], candidate_cameras,
|
|
candidate_landmarks, &published_cameras,
|
|
&published_landmarks);
|
|
*published_z = published_landmarks[views[0].landmarks[0]].point.z;
|
|
*has_metrics = decision.has_metrics;
|
|
*reason = decision.rejection_reason;
|
|
return !decision.accepted && !final_valid;
|
|
}
|
|
#endif
|