fy59/lardon3d
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
lardon3d/tests/test_sparse_sfm_bundle_adjustment.cpp
fy59 fa58c8a225 secure commit
2026-08-25 07:58:46 +02:00

1189 lines
57 KiB
C++
Raw Blame History

#include <lardon3d/sparse_sfm_bundle_adjustment.h>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <limits>
#include <vector>
#define CHECK(value) \
do { \
if (!(value)) { \
std::fprintf(stderr, "bundle adjustment check failed at line %d: %s\n", \
__LINE__, #value); \
return 1; \
} \
} while (0)
extern "C" bool lardon3d_sparse_bundle_adjustment_test_termination_accepted(
int value);
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);
extern "C" bool lardon3d_sparse_bundle_adjustment_test_project(
const Lardon3DSparseGeometryCalibration *calibration,
const Lardon3DSparseGeometryPose *pose,
const Lardon3DSparseGeometryPoint3 *point,
Lardon3DSparseGeometryPoint2 *pixel);
extern "C" bool lardon3d_sparse_bundle_adjustment_test_metrics(
const double *residuals, size_t count, double *rmse, double *huber_cost);
extern "C" bool lardon3d_sparse_bundle_adjustment_test_cost_acceptable(
double initial_cost, double final_cost);
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);
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);
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);
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);
extern "C" int lardon3d_sparse_bundle_adjustment_test_internal_underconstraint();
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);
extern "C" bool lardon3d_sparse_bundle_adjustment_test_invalid_candidate(
const Lardon3DSparseBundleAdjustmentInput *input, double *published_z,
bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason);
static Lardon3DSparseGeometryCalibration calibration() {
return {1280, 960, 800.0, 810.0, 640.0, 480.0, 0.0, 0.0, 0.0, 0.0};
}
static double camera_center_x(const Lardon3DSparseGeometryPose &pose) {
return -(pose.rotation_cw[0] * pose.translation_cw[0] +
pose.rotation_cw[3] * pose.translation_cw[1] +
pose.rotation_cw[6] * pose.translation_cw[2]);
}
static bool close_scalar(double a, double b) {
return std::isfinite(a) && std::isfinite(b) &&
std::abs(a - b) <=
1e-12 * std::max({1.0, std::abs(a), std::abs(b)});
}
static bool close_rotation(const double a[9], const double b[9]) {
bool identical = true;
for (size_t index = 0; index < 9; ++index)
identical = identical && a[index] == b[index];
if (identical) return true;
double trace = 0.0;
for (size_t row = 0; row < 3; ++row)
for (size_t column = 0; column < 3; ++column)
trace += a[row * 3 + column] * b[row * 3 + column];
const double cosine = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5));
return std::acos(cosine) <= 1e-12;
}
static bool equal_results(const Lardon3DSparseBundleAdjustmentResult &a,
const Lardon3DSparseBundleAdjustmentResult &b) {
if (a.status != b.status || a.component_count != b.component_count ||
a.camera_count != b.camera_count || a.landmark_count != b.landmark_count ||
a.observation_count != b.observation_count)
return false;
for (size_t index = 0; index < a.component_count; ++index) {
const auto &ac = a.components[index];
const auto &bc = b.components[index];
if (ac.component_key != bc.component_key || ac.image_count != bc.image_count ||
ac.registered_image_count != bc.registered_image_count ||
ac.landmark_count != bc.landmark_count)
return false;
const auto &ad = a.diagnostics[index];
const auto &bd = b.diagnostics[index];
if (ad.component_key != bd.component_key || ad.camera_count != bd.camera_count ||
ad.landmark_count != bd.landmark_count ||
ad.observation_count != bd.observation_count || ad.eligible != bd.eligible ||
ad.has_anchors != bd.has_anchors ||
ad.pose_anchor_image_id != bd.pose_anchor_image_id ||
ad.scale_anchor_image_id != bd.scale_anchor_image_id ||
ad.scale_axis != bd.scale_axis || ad.has_costs != bd.has_costs ||
ad.has_rmse != bd.has_rmse || ad.iteration_count != bd.iteration_count ||
ad.termination != bd.termination || ad.accepted != bd.accepted ||
ad.rejection_reason != bd.rejection_reason)
return false;
if (ad.has_costs &&
(!close_scalar(ad.initial_robust_cost, bd.initial_robust_cost) ||
!close_scalar(ad.final_robust_cost, bd.final_robust_cost)))
return false;
if (ad.has_rmse &&
(!close_scalar(ad.initial_reprojection_rmse_px,
bd.initial_reprojection_rmse_px) ||
!close_scalar(ad.final_reprojection_rmse_px,
bd.final_reprojection_rmse_px)))
return false;
}
for (size_t index = 0; index < a.camera_count; ++index) {
if (a.cameras[index].image_id != b.cameras[index].image_id ||
a.cameras[index].component_key != b.cameras[index].component_key ||
!close_rotation(a.cameras[index].pose_cw.rotation_cw,
b.cameras[index].pose_cw.rotation_cw))
return false;
for (size_t item = 0; item < 3; ++item)
if (!close_scalar(a.cameras[index].pose_cw.translation_cw[item],
b.cameras[index].pose_cw.translation_cw[item]))
return false;
}
for (size_t index = 0; index < a.landmark_count; ++index) {
const auto &al = a.landmarks[index];
const auto &bl = b.landmarks[index];
if (al.landmark_id != bl.landmark_id || al.track_id != bl.track_id ||
al.component_key != bl.component_key ||
al.observation_count != bl.observation_count ||
!close_scalar(al.point.x, bl.point.x) ||
!close_scalar(al.point.y, bl.point.y) ||
!close_scalar(al.point.z, bl.point.z) ||
!close_scalar(al.reprojection_rmse_px, bl.reprojection_rmse_px) ||
!close_scalar(al.reprojection_median_px, bl.reprojection_median_px))
return false;
}
for (size_t index = 0; index < a.observation_count; ++index) {
const auto &ao = a.observations[index];
const auto &bo = b.observations[index];
if (ao.landmark_id != bo.landmark_id || ao.track_id != bo.track_id ||
ao.image_id != bo.image_id || ao.feature_set_id != bo.feature_set_id ||
ao.feature_index != bo.feature_index ||
ao.position_in_track != bo.position_in_track)
return false;
}
return true;
}
static int test_helpers() {
const Lardon3DSparseGeometryPose pose = {
{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, {0.0, 0.0, 0.0}};
const Lardon3DSparseGeometryPoint3 point = {0.2, -0.1, 2.0};
auto camera = calibration();
Lardon3DSparseGeometryPoint2 pixel = {};
CHECK(lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &point, &pixel));
CHECK(std::abs(pixel.x - 720.0) < 1e-12);
CHECK(std::abs(pixel.y - 439.5) < 1e-12);
camera.k1 = 0.1;
camera.k2 = -0.02;
camera.p1 = 0.003;
camera.p2 = -0.004;
CHECK(lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &point, &pixel));
const double xn = 0.1;
const double yn = -0.05;
const double r2 = xn * xn + yn * yn;
const double radial = 1.0 + camera.k1 * r2 + camera.k2 * r2 * r2;
const double xd = xn * radial + 2.0 * camera.p1 * xn * yn +
camera.p2 * (r2 + 2.0 * xn * xn);
const double yd = yn * radial + camera.p1 * (r2 + 2.0 * yn * yn) +
2.0 * camera.p2 * xn * yn;
CHECK(std::abs(pixel.x - (camera.fx * xd + camera.cx)) < 1e-12);
CHECK(std::abs(pixel.y - (camera.fy * yd + camera.cy)) < 1e-12);
double residuals[4] = {3.0, 4.0, 0.0, 0.0};
double rmse = 0.0;
double cost = 0.0;
CHECK(lardon3d_sparse_bundle_adjustment_test_metrics(residuals, 2, &rmse, &cost));
CHECK(std::abs(rmse - std::sqrt(12.5)) < 1e-12);
CHECK(cost == 8.0);
double boundary[2] = {2.0, 0.0};
CHECK(lardon3d_sparse_bundle_adjustment_test_metrics(boundary, 1, &rmse, &cost));
CHECK(rmse == 2.0 && cost == 2.0);
CHECK(lardon3d_sparse_bundle_adjustment_test_cost_acceptable(10.0, 10.0 + 1e-11));
CHECK(!lardon3d_sparse_bundle_adjustment_test_cost_acceptable(10.0,
10.0 + 2e-11));
CHECK(!lardon3d_sparse_bundle_adjustment_test_cost_acceptable(NAN, 0.0));
residuals[0] = std::numeric_limits<double>::infinity();
CHECK(!lardon3d_sparse_bundle_adjustment_test_metrics(residuals, 2, &rmse, &cost));
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(nullptr, &pose, &point, &pixel));
Lardon3DSparseGeometryPoint3 behind = {0.0, 0.0, -1.0};
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &behind,
&pixel));
Lardon3DSparseGeometryPoint3 depth_boundary = {0.0, 0.0, 1e-9};
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
&camera, &pose, &depth_boundary, &pixel));
depth_boundary.z = std::nextafter(1e-9, 1.0);
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
&camera, &pose, &depth_boundary, &pixel));
auto invalid_pose = pose;
invalid_pose.rotation_cw[0] += 1e-6;
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
&camera, &invalid_pose, &point, &pixel));
auto invalid_calibration = camera;
invalid_calibration.cx = -1.0;
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
&invalid_calibration, &pose, &point, &pixel));
CHECK(lardon3d_sparse_bundle_adjustment_test_termination_accepted(0));
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(1));
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(2));
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(3));
bool has_metrics = false;
Lardon3DSparseBundleAdjustmentRejectionReason reason =
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE;
CHECK(lardon3d_sparse_bundle_adjustment_test_candidate_decision(
0, true, true, 10.0, 9.0, &has_metrics, &reason));
CHECK(has_metrics && reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE);
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision(
1, false, true, 10.0, 0.0, &has_metrics, &reason));
CHECK(!has_metrics && reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE);
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision(
2, false, true, 10.0, 0.0, &has_metrics, &reason));
CHECK(!has_metrics && reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE);
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision(
0, false, true, 10.0, 0.0, &has_metrics, &reason));
CHECK(!has_metrics && reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE);
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision(
0, true, true, 10.0, 11.0, &has_metrics, &reason));
CHECK(has_metrics && reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION);
double published_x = 0.0;
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_publication(
false, 10.0, 0.0, 3.0, 9.0, &published_x, &has_metrics, &reason));
CHECK(published_x == 3.0 && !has_metrics &&
reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE);
CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_publication(
true, 10.0, 11.0, 3.0, 9.0, &published_x, &has_metrics, &reason));
CHECK(published_x == 3.0 && has_metrics &&
reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION);
CHECK(lardon3d_sparse_bundle_adjustment_test_counts_valid(
4095, 4095, 249999, 999999, 999999));
CHECK(lardon3d_sparse_bundle_adjustment_test_counts_valid(
4096, 4096, 250000, 1000000, 1000000));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
4097, 4096, 250000, 1000000, 1000000));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
4096, 4097, 250000, 1000000, 1000000));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
4096, 4096, 250001, 1000000, 1000000));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
4096, 4096, 250000, 1000001, 1000000));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
4096, 4096, 250000, 1000000, 1000001));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
0, 0, 0, std::numeric_limits<size_t>::max(), 0));
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
std::numeric_limits<size_t>::max(), 0, 0, 0, 0));
CHECK(lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR);
return 0;
}
struct Fixture {
Lardon3DSparseIncrementalImage images[3];
Lardon3DSparseIncrementalObservation input_observations[3];
Lardon3DSparseIncrementalComponent components[1];
Lardon3DSparseIncrementalCamera cameras[3];
Lardon3DSparseIncrementalLandmark landmarks[1];
Lardon3DSparseIncrementalLandmarkObservation result_observations[3];
Lardon3DSparseIncrementalUnregisteredImage unregistered_images[1];
Lardon3DSparseIncrementalResult result;
Lardon3DSparseBundleAdjustmentInput input;
};
static Fixture fixture() {
Fixture value = {};
const auto intrinsic = calibration();
value.images[0] = {10, intrinsic};
value.images[1] = {20, intrinsic};
value.images[2] = {30, intrinsic};
value.input_observations[0] = {7, 10, 100, 1, 2, 650.0, 480.0};
value.input_observations[1] = {7, 20, 200, 1, 2, 500.0, 480.0};
value.input_observations[2] = {7, 30, 300, 1, 2, 700.0, 480.0};
value.components[0] = {10, 3, 3, 1};
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
std::memcpy(value.cameras[0].pose_cw.rotation_cw, identity, sizeof(identity));
std::memcpy(value.cameras[1].pose_cw.rotation_cw, identity, sizeof(identity));
std::memcpy(value.cameras[2].pose_cw.rotation_cw, identity, sizeof(identity));
value.cameras[0].image_id = 10;
value.cameras[1].image_id = 20;
value.cameras[2].image_id = 30;
for (auto &camera : value.cameras) camera.component_key = 10;
value.cameras[1].pose_cw.translation_cw[0] = -2.0;
value.cameras[1].pose_cw.translation_cw[1] = -2.0;
value.cameras[2].pose_cw.translation_cw[0] = 2.0;
value.cameras[2].pose_cw.translation_cw[1] = 2.0;
value.landmarks[0] = {1, 7, 10, {0.0, 0.0, 5.0}, 0.5, 0.4, 3};
for (size_t index = 0; index < 3; ++index) {
value.result_observations[index] = {
1, 7, value.images[index].image_id,
value.input_observations[index].feature_set_id,
value.input_observations[index].feature_index,
static_cast<uint32_t>(index)};
}
value.result.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
value.result.track_set_id = 77;
value.result.calibration_scope_id = 88;
value.result.component_count = 1;
value.result.camera_count = 3;
value.result.landmark_count = 1;
value.result.observation_count = 3;
value.unregistered_images[0] = {99, 10};
value.result.unregistered_image_count = 1;
value.result.seed_candidates_considered = 3;
value.result.seed_candidates_available = 4;
value.result.seed_image_a = 10;
value.result.seed_image_b = 20;
value.result.last_seed_geometry_status = 2;
value.result.last_seed_parallax_rad = 0.25;
value.result.registration_rounds = 5;
value.result.registration_attempts = 6;
value.result.registration_successes = 3;
value.result.registration_failures = 3;
value.result.last_pnp_inlier_count = 8;
value.result.triangulation_attempts = 9;
value.result.triangulation_failures = 1;
value.result.rejected_behind_camera = 2;
value.result.rejected_reprojection = 3;
value.result.rejected_landmarks = 4;
value.result.last_triangulation_status = 5;
value.result.landmark_update_attempts = 6;
value.result.landmark_update_successes = 7;
value.result.landmark_update_failures = 8;
value.result.no_growth_terminations = 9;
value.result.round_limit_terminations = 10;
value.result.point_refinement_attempts = 11;
value.result.point_refinement_successes = 12;
return value;
}
static void bind_fixture(Fixture *value) {
value->result.components = value->components;
value->result.cameras = value->cameras;
value->result.landmarks = value->landmarks;
value->result.observations = value->result_observations;
value->result.unregistered_images = value->unregistered_images;
value->input = {&value->result, value->images, 3, value->input_observations, 3};
}
static int test_preparation() {
Fixture value = fixture();
bind_fixture(&value);
const Fixture original = value;
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[1] = {};
Lardon3DSparseIncrementalObservation resolved[3] = {};
size_t diagnostic_count = 0;
size_t resolved_count = 0;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 0);
CHECK(diagnostic_count == 1 && resolved_count == 3);
CHECK(resolved[1].image_id == 20 && resolved[1].x == 500.0);
CHECK(diagnostics[0].eligible);
CHECK(diagnostics[0].pose_anchor_image_id == 10);
CHECK(diagnostics[0].scale_anchor_image_id == 20);
CHECK(diagnostics[0].scale_axis ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X);
CHECK(diagnostics[0].termination ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NOT_RUN);
CHECK(!diagnostics[0].has_costs && !diagnostics[0].has_rmse);
CHECK(!diagnostics[0].accepted);
CHECK(std::memcmp(&value.result, &original.result, sizeof(value.result)) == 0);
CHECK(std::memcmp(value.images, original.images, sizeof(value.images)) == 0);
CHECK(std::memcmp(value.input_observations, original.input_observations,
sizeof(value.input_observations)) == 0);
CHECK(std::memcmp(value.cameras, original.cameras, sizeof(value.cameras)) == 0);
CHECK(std::memcmp(value.landmarks, original.landmarks, sizeof(value.landmarks)) == 0);
CHECK(std::memcmp(value.components, original.components, sizeof(value.components)) == 0);
CHECK(std::memcmp(value.result_observations, original.result_observations,
sizeof(value.result_observations)) == 0);
CHECK(std::memcmp(value.unregistered_images, original.unregistered_images,
sizeof(value.unregistered_images)) == 0);
value = fixture();
bind_fixture(&value);
value.cameras[1].pose_cw.translation_cw[0] = -1e-9;
value.cameras[1].pose_cw.translation_cw[1] = 0.0;
value.cameras[2].pose_cw.translation_cw[0] = 1e-9;
value.cameras[2].pose_cw.translation_cw[1] = 0.0;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 0);
CHECK(!diagnostics[0].eligible);
CHECK(diagnostics[0].rejection_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE);
value = fixture();
bind_fixture(&value);
value.cameras[1].pose_cw.translation_cw[0] =
-std::nextafter(1e-9, 1.0);
value.cameras[1].pose_cw.translation_cw[1] = 0.0;
value.cameras[2].pose_cw.translation_cw[0] =
std::nextafter(1e-9, 1.0);
value.cameras[2].pose_cw.translation_cw[1] = 0.0;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 0);
CHECK(diagnostics[0].eligible);
CHECK(diagnostics[0].scale_anchor_image_id == 20);
CHECK(diagnostics[0].scale_axis ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X);
return 0;
}
static int test_invalid() {
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[1] = {};
Lardon3DSparseIncrementalObservation resolved[3] = {};
size_t diagnostic_count = 0;
size_t resolved_count = 0;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
nullptr, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
Fixture value = fixture();
bind_fixture(&value);
value.input_observations[1].feature_set_id = 999;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
value = fixture();
bind_fixture(&value);
value.input_observations[1].image_id = 30;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
value = fixture();
bind_fixture(&value);
value.input_observations[1].x = NAN;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
value = fixture();
bind_fixture(&value);
value.input_observations[1].feature_set_id = value.input_observations[0].feature_set_id;
value.input_observations[1].feature_index = value.input_observations[0].feature_index;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
value = fixture();
bind_fixture(&value);
std::swap(value.cameras[0], value.cameras[1]);
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
value = fixture();
bind_fixture(&value);
value.input.image_count = 4097;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
&resolved_count) == 1);
Lardon3DSparseIncrementalResult empty = {};
empty.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
Lardon3DSparseBundleAdjustmentInput empty_input = {&empty, nullptr, 0, nullptr, 0};
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&empty_input, nullptr, 0, nullptr, 0, &diagnostic_count,
&resolved_count) == 0);
CHECK(diagnostic_count == 0 && resolved_count == 0);
return 0;
}
static int test_multiple_components() {
const auto intrinsic = calibration();
Lardon3DSparseIncrementalImage images[4] = {
{10, intrinsic}, {20, intrinsic}, {30, intrinsic}, {40, intrinsic}};
Lardon3DSparseIncrementalObservation source_observations[4] = {
{7, 10, 100, 1, 2, 640.0, 480.0},
{7, 20, 200, 1, 2, 600.0, 480.0},
{8, 30, 300, 1, 2, 640.0, 480.0},
{8, 40, 400, 1, 2, 600.0, 480.0}};
Lardon3DSparseIncrementalComponent components[2] = {
{10, 2, 2, 1}, {30, 2, 2, 1}};
Lardon3DSparseIncrementalCamera cameras[4] = {};
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
for (size_t index = 0; index < 4; ++index) {
cameras[index].image_id = images[index].image_id;
cameras[index].component_key = index < 2 ? 10 : 30;
std::memcpy(cameras[index].pose_cw.rotation_cw, identity, sizeof(identity));
}
cameras[1].pose_cw.translation_cw[0] = -1.0;
cameras[3].pose_cw.translation_cw[1] = -1.0;
Lardon3DSparseIncrementalLandmark landmarks[2] = {
{1, 7, 10, {0.0, 0.0, 5.0}, 0.5, 0.4, 2},
{2, 8, 30, {0.0, 0.0, 6.0}, 0.5, 0.4, 2}};
Lardon3DSparseIncrementalLandmarkObservation observations[4] = {
{1, 7, 10, 100, 1, 0}, {1, 7, 20, 200, 1, 1},
{2, 8, 30, 300, 1, 0}, {2, 8, 40, 400, 1, 1}};
Lardon3DSparseIncrementalResult result = {};
result.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
result.components = components;
result.component_count = 2;
result.cameras = cameras;
result.camera_count = 4;
result.landmarks = landmarks;
result.landmark_count = 2;
result.observations = observations;
result.observation_count = 4;
Lardon3DSparseBundleAdjustmentInput input = {
&result, images, 4, source_observations, 4};
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[2] = {};
Lardon3DSparseIncrementalObservation resolved[4] = {};
size_t diagnostic_count = 0;
size_t resolved_count = 0;
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
&input, diagnostics, 2, resolved, 4, &diagnostic_count,
&resolved_count) == 0);
CHECK(diagnostic_count == 2 && resolved_count == 4);
CHECK(diagnostics[0].component_key == 10 && diagnostics[0].observation_count == 2);
CHECK(diagnostics[1].component_key == 30 && diagnostics[1].observation_count == 2);
CHECK(diagnostics[0].scale_axis == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X);
CHECK(diagnostics[1].scale_axis == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_Y);
return 0;
}
static int test_underconstraint_rules() {
std::vector<size_t> cameras;
std::vector<size_t> landmarks;
auto complete_edges = [&](size_t camera_count, size_t landmark_count) {
cameras.clear();
landmarks.clear();
for (size_t camera = 0; camera < camera_count; ++camera) {
for (size_t landmark = 0; landmark < landmark_count; ++landmark) {
cameras.push_back(camera);
landmarks.push_back(landmark);
}
}
};
complete_edges(4, 3);
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
4, 3, cameras.size(), 0, cameras.data(), landmarks.data(),
cameras.size()) == 1U);
complete_edges(3, 6);
for (size_t index = cameras.size(); index-- > 0;) {
if (landmarks[index] == 0 && cameras[index] != 0) {
cameras.erase(cameras.begin() + static_cast<ptrdiff_t>(index));
landmarks.erase(landmarks.begin() + static_cast<ptrdiff_t>(index));
}
}
CHECK((lardon3d_sparse_bundle_adjustment_test_structural_mask(
3, 6, cameras.size(), 0, cameras.data(), landmarks.data(),
cameras.size()) & 2U) != 0);
complete_edges(3, 10);
for (size_t index = cameras.size(); index-- > 0;) {
if (cameras[index] == 1 && landmarks[index] >= 2) {
cameras.erase(cameras.begin() + static_cast<ptrdiff_t>(index));
landmarks.erase(landmarks.begin() + static_cast<ptrdiff_t>(index));
}
}
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
3, 10, cameras.size(), 0, cameras.data(), landmarks.data(),
cameras.size()) == 4U);
cameras.clear();
landmarks.clear();
for (size_t group = 0; group < 2; ++group) {
for (size_t camera = group * 3; camera < group * 3 + 3; ++camera) {
for (size_t landmark = group * 6; landmark < group * 6 + 6; ++landmark) {
cameras.push_back(camera);
landmarks.push_back(landmark);
}
}
}
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
6, 12, cameras.size(), 0, cameras.data(), landmarks.data(),
cameras.size()) == 8U);
return 0;
}
struct ScientificFixture {
std::vector<Lardon3DSparseIncrementalImage> images;
std::vector<Lardon3DSparseIncrementalObservation> input_observations;
std::vector<Lardon3DSparseIncrementalComponent> components;
std::vector<Lardon3DSparseIncrementalCamera> cameras;
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
Lardon3DSparseIncrementalResult incremental = {};
Lardon3DSparseBundleAdjustmentInput input = {};
};
static ScientificFixture scientific_fixture(bool perturb_camera,
bool perturb_landmarks,
size_t landmark_count = 5,
bool nontrivial_anchor = false) {
ScientificFixture value;
const auto intrinsic = calibration();
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
const double anchor_angle = 0.2;
const double anchor_rotation[9] = {
std::cos(anchor_angle), -std::sin(anchor_angle), 0.0,
std::sin(anchor_angle), std::cos(anchor_angle), 0.0,
0.0, 0.0, 1.0};
for (size_t camera = 0; camera < 4; ++camera) {
const uint64_t image_id = 10 + camera * 10;
value.images.push_back({image_id, intrinsic});
Lardon3DSparseIncrementalCamera output = {};
output.image_id = image_id;
output.component_key = 10;
std::memcpy(output.pose_cw.rotation_cw,
camera == 0 && nontrivial_anchor ? anchor_rotation : identity,
sizeof(identity));
output.pose_cw.translation_cw[0] = -static_cast<double>(camera);
value.cameras.push_back(output);
}
if (perturb_camera) value.cameras[1].pose_cw.translation_cw[1] = 0.08;
for (size_t landmark = 0; landmark < landmark_count; ++landmark) {
const uint64_t track_id = landmark + 1;
const Lardon3DSparseGeometryPoint3 truth = {
-0.6 + 0.25 * static_cast<double>(landmark),
-0.3 + 0.12 * static_cast<double>(landmark % 3),
4.5 + 0.3 * static_cast<double>(landmark)};
auto initial = truth;
if (perturb_landmarks) {
initial.x += 0.06;
initial.y -= 0.04;
}
value.landmarks.push_back(
{track_id, track_id, 10, initial, 0.0, 0.0, 4});
for (size_t camera = 0; camera < 4; ++camera) {
Lardon3DSparseGeometryPose truth_pose = {};
std::memcpy(truth_pose.rotation_cw,
camera == 0 && nontrivial_anchor ? anchor_rotation : identity,
sizeof(identity));
truth_pose.translation_cw[0] = -static_cast<double>(camera);
Lardon3DSparseGeometryPoint2 pixel = {};
if (!lardon3d_sparse_bundle_adjustment_test_project(
&intrinsic, &truth_pose, &truth, &pixel))
std::abort();
const uint64_t image_id = 10 + camera * 10;
const uint64_t feature_set_id = 100 + camera;
value.input_observations.push_back(
{track_id, image_id, feature_set_id,
static_cast<uint32_t>(landmark), 64, pixel.x, pixel.y});
value.observations.push_back(
{track_id, track_id, image_id, feature_set_id,
static_cast<uint32_t>(landmark), static_cast<uint32_t>(camera)});
}
}
value.components.push_back({10, 4, 4, landmark_count});
value.incremental.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
value.incremental.track_set_id = 77;
value.incremental.calibration_scope_id = 88;
return value;
}
static void bind_scientific_fixture(ScientificFixture *value) {
value->incremental.components = value->components.data();
value->incremental.component_count = value->components.size();
value->incremental.cameras = value->cameras.data();
value->incremental.camera_count = value->cameras.size();
value->incremental.landmarks = value->landmarks.data();
value->incremental.landmark_count = value->landmarks.size();
value->incremental.observations = value->observations.data();
value->incremental.observation_count = value->observations.size();
value->input = {&value->incremental, value->images.data(), value->images.size(),
value->input_observations.data(),
value->input_observations.size()};
}
static bool published_metrics(const ScientificFixture &fixture,
const Lardon3DSparseBundleAdjustmentResult &result,
double *rmse, double *cost) {
std::vector<double> residuals(result.observation_count * 2);
for (size_t index = 0; index < result.observation_count; ++index) {
const auto &association = result.observations[index];
auto camera = std::lower_bound(
result.cameras, result.cameras + result.camera_count, association.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
auto landmark = std::lower_bound(
result.landmarks, result.landmarks + result.landmark_count,
association.track_id,
[](const auto &item, uint64_t key) { return item.track_id < key; });
auto source = std::find_if(
fixture.input_observations.begin(), fixture.input_observations.end(),
[&](const auto &item) {
return item.feature_set_id == association.feature_set_id &&
item.feature_index == association.feature_index;
});
auto image = std::lower_bound(
fixture.images.begin(), fixture.images.end(), association.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
if (camera == result.cameras + result.camera_count ||
landmark == result.landmarks + result.landmark_count ||
source == fixture.input_observations.end() || image == fixture.images.end())
return false;
Lardon3DSparseGeometryPoint2 pixel = {};
if (!lardon3d_sparse_bundle_adjustment_test_project(
&image->calibration, &camera->pose_cw, &landmark->point, &pixel))
return false;
residuals[index * 2] = pixel.x - source->x;
residuals[index * 2 + 1] = pixel.y - source->y;
}
return lardon3d_sparse_bundle_adjustment_test_metrics(
residuals.data(), result.observation_count, rmse, cost);
}
static ScientificFixture two_component_fixture(bool reject_second) {
ScientificFixture first = scientific_fixture(true, true);
ScientificFixture second = scientific_fixture(true, true);
for (auto &image : second.images) image.image_id += 100;
for (auto &observation : second.input_observations) {
observation.track_id += 100;
observation.image_id += 100;
observation.feature_set_id += 1000;
}
for (auto &component : second.components) component.component_key += 100;
for (auto &camera : second.cameras) {
camera.image_id += 100;
camera.component_key += 100;
}
for (auto &landmark : second.landmarks) {
landmark.landmark_id += 100;
landmark.track_id += 100;
landmark.component_key += 100;
}
if (reject_second) second.landmarks[0].point.z = -1.0;
for (auto &observation : second.observations) {
observation.landmark_id += 100;
observation.track_id += 100;
observation.image_id += 100;
observation.feature_set_id += 1000;
}
first.images.insert(first.images.end(), second.images.begin(), second.images.end());
first.input_observations.insert(first.input_observations.end(),
second.input_observations.begin(),
second.input_observations.end());
first.components.insert(first.components.end(), second.components.begin(),
second.components.end());
first.cameras.insert(first.cameras.end(), second.cameras.begin(),
second.cameras.end());
first.landmarks.insert(first.landmarks.end(), second.landmarks.begin(),
second.landmarks.end());
first.observations.insert(first.observations.end(), second.observations.begin(),
second.observations.end());
return first;
}
static int run_scientific_case(bool perturb_camera, bool perturb_landmarks,
double noise_px, size_t outlier_percent) {
auto value = scientific_fixture(perturb_camera, perturb_landmarks,
noise_px == 0.0 && outlier_percent == 0 ? 5 : 10);
if (noise_px != 0.0) {
for (auto &landmark : value.landmarks) landmark.point.z = 5.0;
for (auto &observation : value.input_observations) {
const auto camera = std::lower_bound(
value.cameras.begin(), value.cameras.end(), observation.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
const auto landmark = std::lower_bound(
value.landmarks.begin(), value.landmarks.end(), observation.track_id,
[](const auto &item, uint64_t key) { return item.track_id < key; });
const auto image = std::lower_bound(
value.images.begin(), value.images.end(), observation.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
CHECK(camera != value.cameras.end());
CHECK(landmark != value.landmarks.end());
CHECK(image != value.images.end());
Lardon3DSparseGeometryPoint2 noiseless = {};
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
&image->calibration, &camera->pose_cw, &landmark->point, &noiseless));
observation.x = noiseless.x;
observation.y = noiseless.y;
}
for (size_t index = 0; index < value.input_observations.size(); ++index) {
auto &observation = value.input_observations[index];
const auto camera = std::lower_bound(
value.cameras.begin(), value.cameras.end(), observation.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
const auto landmark = std::lower_bound(
value.landmarks.begin(), value.landmarks.end(), observation.track_id,
[](const auto &item, uint64_t key) { return item.track_id < key; });
const auto image = std::lower_bound(
value.images.begin(), value.images.end(), observation.image_id,
[](const auto &item, uint64_t key) { return item.image_id < key; });
CHECK(camera != value.cameras.end());
CHECK(landmark != value.landmarks.end());
CHECK(image != value.images.end());
Lardon3DSparseGeometryPoint2 noiseless = {};
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
&image->calibration, &camera->pose_cw, &landmark->point, &noiseless));
CHECK(close_scalar(observation.x, noiseless.x));
CHECK(close_scalar(observation.y, noiseless.y));
observation.y += observation.track_id % 2 == 0 ? noise_px : -noise_px;
CHECK(std::hypot(observation.x - noiseless.x,
observation.y - noiseless.y) == noise_px);
}
}
const size_t outlier_count =
value.input_observations.size() * outlier_percent / 100;
CHECK(outlier_count * 100 ==
value.input_observations.size() * outlier_percent);
if (outlier_percent == 10) CHECK(outlier_count == 4);
if (outlier_percent == 20) CHECK(outlier_count == 8);
if (outlier_percent == 40) CHECK(outlier_count == 16);
for (size_t index = 0; index < outlier_count; ++index) {
value.input_observations[index].x += index % 2 == 0 ? 2.000001 : -2.000001;
}
bind_scientific_fixture(&value);
Lardon3DSparseBundleAdjustmentResult result = {};
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
if (outlier_percent != 0 &&
result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED) {
CHECK(!result.diagnostics[0].accepted);
CHECK(result.diagnostics[0].termination ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE);
CHECK(result.diagnostics[0].rejection_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE);
CHECK(!result.diagnostics[0].has_costs && !result.diagnostics[0].has_rmse);
CHECK(std::memcmp(result.cameras, value.cameras.data(),
value.cameras.size() * sizeof(value.cameras[0])) == 0);
CHECK(std::memcmp(result.landmarks, value.landmarks.data(),
value.landmarks.size() * sizeof(value.landmarks[0])) == 0);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
return 0;
}
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
CHECK(result.diagnostics[0].accepted);
CHECK(result.diagnostics[0].termination ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED);
CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse);
CHECK(std::isfinite(result.diagnostics[0].initial_robust_cost));
CHECK(std::isfinite(result.diagnostics[0].final_robust_cost));
CHECK(result.diagnostics[0].final_robust_cost <=
result.diagnostics[0].initial_robust_cost +
1e-12 * std::max(1.0,
std::abs(result.diagnostics[0].initial_robust_cost)));
if (perturb_camera || perturb_landmarks)
CHECK(result.diagnostics[0].final_robust_cost <
result.diagnostics[0].initial_robust_cost);
CHECK(std::memcmp(&result.cameras[0], &value.cameras[0],
sizeof(value.cameras[0])) == 0);
double published_rmse = 0.0;
double published_cost = 0.0;
CHECK(published_metrics(value, result, &published_rmse, &published_cost));
CHECK(close_scalar(published_rmse,
result.diagnostics[0].final_reprojection_rmse_px));
CHECK(close_scalar(published_cost, result.diagnostics[0].final_robust_cost));
const size_t scale_index = value.cameras.size() - 1;
const double scale_before = camera_center_x(value.cameras[scale_index].pose_cw);
const double scale_after = camera_center_x(result.cameras[scale_index].pose_cw);
CHECK(std::abs(scale_after - scale_before) <=
1e-12 * std::max({1.0, std::abs(scale_before), std::abs(scale_after)}));
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
return 0;
}
static int test_execution_and_solver() {
Lardon3DSparseBundleAdjustmentResult result = {};
CHECK(lardon3d_sparse_bundle_adjustment_run(nullptr, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT);
CHECK(result.components == nullptr && result.component_count == 0);
auto value = scientific_fixture(false, false);
bind_scientific_fixture(&value);
const auto original_cameras = value.cameras;
const auto original_landmarks = value.landmarks;
const auto original_images = value.images;
const auto original_observations = value.input_observations;
const auto original_components = value.components;
const auto original_result_observations = value.observations;
const auto original_incremental = value.incremental;
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
CHECK(result.diagnostics[0].eligible && result.diagnostics[0].accepted);
CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse);
CHECK(result.diagnostics[0].termination ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED);
CHECK(std::memcmp(value.cameras.data(), original_cameras.data(),
value.cameras.size() * sizeof(value.cameras[0])) == 0);
CHECK(std::memcmp(value.landmarks.data(), original_landmarks.data(),
value.landmarks.size() * sizeof(value.landmarks[0])) == 0);
CHECK(std::memcmp(value.input_observations.data(), original_observations.data(),
value.input_observations.size() *
sizeof(value.input_observations[0])) == 0);
CHECK(std::memcmp(value.images.data(), original_images.data(),
value.images.size() * sizeof(value.images[0])) == 0);
CHECK(std::memcmp(value.components.data(), original_components.data(),
value.components.size() * sizeof(value.components[0])) == 0);
CHECK(std::memcmp(value.observations.data(), original_result_observations.data(),
value.observations.size() * sizeof(value.observations[0])) == 0);
CHECK(std::memcmp(&value.incremental, &original_incremental,
sizeof(value.incremental)) == 0);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
lardon3d_sparse_bundle_adjustment_result_destroy(nullptr);
auto anchor_value = scientific_fixture(false, false, 5, true);
bind_scientific_fixture(&anchor_value);
CHECK(lardon3d_sparse_bundle_adjustment_run(&anchor_value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
CHECK(std::memcmp(&result.cameras[0], &anchor_value.cameras[0],
sizeof(result.cameras[0])) == 0);
double anchor_rmse = 0.0;
double anchor_cost = 0.0;
CHECK(published_metrics(anchor_value, result, &anchor_rmse, &anchor_cost));
CHECK(close_scalar(anchor_rmse,
result.diagnostics[0].final_reprojection_rmse_px));
CHECK(close_scalar(anchor_cost, result.diagnostics[0].final_robust_cost));
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
Lardon3DSparseIncrementalImage lone_image = {10, calibration()};
Lardon3DSparseIncrementalComponent lone_component = {10, 1, 1, 0};
Lardon3DSparseIncrementalCamera lone_camera = {};
lone_camera.image_id = 10;
lone_camera.component_key = 10;
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
std::memcpy(lone_camera.pose_cw.rotation_cw, identity, sizeof(identity));
Lardon3DSparseIncrementalResult lone_incremental = {};
lone_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
lone_incremental.components = &lone_component;
lone_incremental.component_count = 1;
lone_incremental.cameras = &lone_camera;
lone_incremental.camera_count = 1;
Lardon3DSparseBundleAdjustmentInput lone_input = {
&lone_incremental, &lone_image, 1, nullptr, 0};
CHECK(lardon3d_sparse_bundle_adjustment_run(&lone_input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED);
CHECK(!result.diagnostics[0].eligible);
CHECK(result.diagnostics[0].rejection_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
Lardon3DSparseIncrementalResult empty_incremental = {};
empty_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
Lardon3DSparseBundleAdjustmentInput empty_input = {
&empty_incremental, nullptr, 0, nullptr, 0};
CHECK(lardon3d_sparse_bundle_adjustment_run(&empty_input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED);
CHECK(result.component_count == 0 && result.camera_count == 0 &&
result.landmark_count == 0 && result.observation_count == 0);
CHECK(result.components == nullptr && result.cameras == nullptr &&
result.landmarks == nullptr && result.observations == nullptr &&
result.diagnostics == nullptr);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
value = scientific_fixture(true, true);
bind_scientific_fixture(&value);
int kinds[13] = {};
uint64_t identities[13] = {};
int groups[13] = {};
bool constants[13] = {};
int subset_axes[13] = {};
size_t parameter_count = 0;
CHECK(lardon3d_sparse_bundle_adjustment_test_parameter_ordering(
&value.input, kinds, identities, groups, constants, subset_axes, 13,
&parameter_count));
CHECK(parameter_count == 13);
for (size_t index = 0; index < 5; ++index)
CHECK(kinds[index] == 0 && identities[index] == index + 1 && groups[index] == 0);
for (size_t camera = 0; camera < 4; ++camera) {
const size_t quaternion = 5 + camera * 2;
const size_t center = quaternion + 1;
CHECK(kinds[quaternion] == 1 && kinds[center] == 2);
CHECK(identities[quaternion] == 10 + camera * 10 &&
identities[center] == identities[quaternion]);
CHECK(groups[quaternion] == 1 && groups[center] == 1);
CHECK(constants[quaternion] == (camera == 0));
CHECK(constants[center] == (camera == 0));
CHECK(subset_axes[center] == (camera == 3 ? 0 : -1));
}
double published_z = 0.0;
bool candidate_has_metrics = true;
Lardon3DSparseBundleAdjustmentRejectionReason candidate_reason =
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE;
CHECK(lardon3d_sparse_bundle_adjustment_test_invalid_candidate(
&value.input, &published_z, &candidate_has_metrics, &candidate_reason));
CHECK(published_z == value.landmarks[0].point.z);
CHECK(!candidate_has_metrics &&
candidate_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE);
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
CHECK(result.diagnostics[0].accepted);
CHECK(result.diagnostics[0].final_robust_cost <=
result.diagnostics[0].initial_robust_cost);
CHECK(std::memcmp(&result.cameras[0], &value.cameras[0],
sizeof(value.cameras[0])) == 0);
CHECK(std::abs(camera_center_x(result.cameras[3].pose_cw) -
camera_center_x(value.cameras[3].pose_cw)) < 1e-12);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
value = two_component_fixture(false);
bind_scientific_fixture(&value);
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
CHECK(result.component_count == 2 && result.diagnostics[0].accepted &&
result.diagnostics[1].accepted);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
value = two_component_fixture(true);
bind_scientific_fixture(&value);
const auto rejected_cameras = value.cameras;
const auto rejected_landmarks = value.landmarks;
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL);
CHECK(result.diagnostics[0].accepted && !result.diagnostics[1].accepted);
CHECK(result.diagnostics[1].rejection_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE);
CHECK(std::memcmp(result.cameras + 4, rejected_cameras.data() + 4,
4 * sizeof(result.cameras[0])) == 0);
CHECK(std::memcmp(result.landmarks + 5, rejected_landmarks.data() + 5,
5 * sizeof(result.landmarks[0])) == 0);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
CHECK(run_scientific_case(true, false, 0.0, 0) == 0);
CHECK(run_scientific_case(false, true, 0.0, 0) == 0);
CHECK(run_scientific_case(true, true, 0.0, 0) == 0);
CHECK(run_scientific_case(false, false, 0.5, 0) == 0);
CHECK(run_scientific_case(false, false, 1.0, 0) == 0);
CHECK(run_scientific_case(false, false, 2.0, 0) == 0);
CHECK(run_scientific_case(false, false, 0.0, 10) == 0);
CHECK(run_scientific_case(false, false, 0.0, 20) == 0);
CHECK(run_scientific_case(false, false, 0.0, 40) == 0);
value = scientific_fixture(true, true);
bind_scientific_fixture(&value);
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
for (size_t run = 1; run < 5; ++run) {
Lardon3DSparseBundleAdjustmentResult repeated = {};
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &repeated) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(equal_results(result, repeated));
lardon3d_sparse_bundle_adjustment_result_destroy(&repeated);
}
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
Fixture sparse = fixture();
bind_fixture(&sparse);
CHECK(lardon3d_sparse_bundle_adjustment_run(&sparse.input, &result) ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED);
CHECK(!result.diagnostics[0].eligible);
CHECK(result.diagnostics[0].rejection_reason ==
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED);
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
return 0;
}
template <typename T>
static bool write_items(FILE *file, const T *items, size_t count) {
return count == 0 || std::fwrite(items, sizeof(T), count, file) == count;
}
template <typename T>
static bool read_items(FILE *file, std::vector<T> *items, size_t count) {
items->resize(count);
return count == 0 || std::fread(items->data(), sizeof(T), count, file) == count;
}
struct SnapshotStorage {
Lardon3DSparseBundleAdjustmentResult result = {};
std::vector<Lardon3DSparseIncrementalComponent> components;
std::vector<Lardon3DSparseIncrementalCamera> cameras;
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
std::vector<Lardon3DSparseBundleAdjustmentComponentDiagnostic> diagnostics;
};
static bool write_snapshot(const char *path,
const Lardon3DSparseBundleAdjustmentResult &result) {
FILE *file = std::fopen(path, "wb");
if (!file) return false;
const char header[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'};
const bool ok = write_items(file, header, sizeof(header)) &&
write_items(file, &result.status, 1) &&
write_items(file, &result.component_count, 1) &&
write_items(file, &result.camera_count, 1) &&
write_items(file, &result.landmark_count, 1) &&
write_items(file, &result.observation_count, 1) &&
write_items(file, result.components, result.component_count) &&
write_items(file, result.cameras, result.camera_count) &&
write_items(file, result.landmarks, result.landmark_count) &&
write_items(file, result.observations, result.observation_count) &&
write_items(file, result.diagnostics, result.component_count);
return std::fclose(file) == 0 && ok;
}
static bool read_snapshot(const char *path, SnapshotStorage *snapshot) {
FILE *file = std::fopen(path, "rb");
if (!file) return false;
char header[8] = {};
const char expected[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'};
bool ok = read_items(file, &snapshot->components, 0) &&
std::fread(header, 1, sizeof(header), file) == sizeof(header) &&
std::memcmp(header, expected, sizeof(header)) == 0 &&
std::fread(&snapshot->result.status, sizeof(snapshot->result.status), 1,
file) == 1 &&
std::fread(&snapshot->result.component_count,
sizeof(snapshot->result.component_count), 1, file) == 1 &&
std::fread(&snapshot->result.camera_count,
sizeof(snapshot->result.camera_count), 1, file) == 1 &&
std::fread(&snapshot->result.landmark_count,
sizeof(snapshot->result.landmark_count), 1, file) == 1 &&
std::fread(&snapshot->result.observation_count,
sizeof(snapshot->result.observation_count), 1, file) == 1;
if (ok && (snapshot->result.component_count > 4096 ||
snapshot->result.camera_count > 4096 ||
snapshot->result.landmark_count > 250000 ||
snapshot->result.observation_count > 1000000))
ok = false;
if (ok)
ok = read_items(file, &snapshot->components, snapshot->result.component_count) &&
read_items(file, &snapshot->cameras, snapshot->result.camera_count) &&
read_items(file, &snapshot->landmarks, snapshot->result.landmark_count) &&
read_items(file, &snapshot->observations,
snapshot->result.observation_count) &&
read_items(file, &snapshot->diagnostics, snapshot->result.component_count);
if (ok) {
snapshot->result.components = snapshot->components.data();
snapshot->result.cameras = snapshot->cameras.data();
snapshot->result.landmarks = snapshot->landmarks.data();
snapshot->result.observations = snapshot->observations.data();
snapshot->result.diagnostics = snapshot->diagnostics.data();
ok = std::fgetc(file) == EOF;
}
return std::fclose(file) == 0 && ok;
}
static int snapshot_mode(const char *write_path, const char *compare_path) {
auto value = scientific_fixture(true, true);
bind_scientific_fixture(&value);
Lardon3DSparseBundleAdjustmentResult result = {};
if (lardon3d_sparse_bundle_adjustment_run(&value.input, &result) !=
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK)
return 1;
bool ok = false;
if (write_path) {
ok = write_snapshot(write_path, result);
} else {
SnapshotStorage reference;
ok = read_snapshot(compare_path, &reference) &&
equal_results(reference.result, result);
}
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
return ok ? 0 : 1;
}
int main(int argc, char **argv) {
if (argc == 3 && std::strcmp(argv[1], "--write-snapshot") == 0)
return snapshot_mode(argv[2], nullptr);
if (argc == 3 && std::strcmp(argv[1], "--compare-snapshot") == 0)
return snapshot_mode(nullptr, argv[2]);
if (test_helpers() != 0) return 1;
if (test_preparation() != 0) return 1;
if (test_invalid() != 0) return 1;
if (test_multiple_components() != 0) return 1;
if (test_underconstraint_rules() != 0) return 1;
return test_execution_and_solver();
}
Reference in a new issue View git blame Copy permalink