diff --git a/docs/architecture/reconstruction_pipeline.md b/docs/architecture/reconstruction_pipeline.md index ef9bd25..4dfa8a3 100644 --- a/docs/architecture/reconstruction_pipeline.md +++ b/docs/architecture/reconstruction_pipeline.md @@ -252,9 +252,10 @@ Image Catalog (B) ──► Feature Store (C) ## Statut du pipeline Import, Image Catalog, Feature Extraction, Feature Store, Visual Index, -Candidate Pair, Matching v1 et Geometric Verification Model v1 sont -**IMPLEMENTED**. Geometric Verifier, -Tracks et SfM sont **PLANNED**. +Candidate Pair, Matching v1, Geometric Verification, Track Model/Builder v1 +and Sparse SfM Gate C geometry are **IMPLEMENTED**. The synchronous in-memory +incremental Sparse SfM Gate D core is **IMPLEMENTED / PASS**. BA, +orchestration, MVS, mesh, texturing and viewer remain **PLANNED**. Ce document décrit la vision architecturale cible du pipeline de reconstruction. Les modules listés ici ne sont pas tous implémentés. diff --git a/docs/architecture/sparse_sfm.md b/docs/architecture/sparse_sfm.md index 0712045..0743e6d 100644 --- a/docs/architecture/sparse_sfm.md +++ b/docs/architecture/sparse_sfm.md @@ -437,6 +437,159 @@ Degeneracy checks use finite values, positive depth, rotation SO(3) residual collinearity covariance determinant `1e-10`. These are pure-geometry parameters and do not alter Project DB identity. +## Gate D — incremental Sparse SfM core + +**GATE D — PASS.** Gate D is the first executable link +between the immutable Track/Calibration contracts and the Gate C primitives. +The reference implementation is synchronous, deterministic, CPU-only, +in-memory, bounded and independent of Project DB, Task Runtime, Resource +Governor and persistence publication. + +### Inputs + +Gate D consumes exactly one immutable Track Set, one immutable calibration +scope, finite calibration values for participating images, bounded keypoint +coordinates addressed by `(feature_set_id, feature_index)`, and explicit +fingerprinted parameters. The Track Set is never mutated. + +### Algorithm + +The core sorts image and Track identities, builds sparse connected components, +orders seed candidates by shared Track count and image IDs, and tries a bounded +number of seeds. Each candidate uses the Gate C relative-pose, cheirality, +parallax and two-view triangulation contracts. A valid seed establishes a +component-local unit gauge. + +Unregistered images are then ordered by visible accepted-landmark count and +image ID. Gate C calibrated PnP registers at most one selected image per +bounded round. Failed registration leaves the image explicitly unregistered. +New landmarks use all currently registered observations, Gate C multi-view DLT +and bounded point-only refinement. A landmark is accepted or rejected as a +whole; Track observations are never dropped or rewritten. + +After each successful camera registration, an existing landmark whose Track +has gained registered observations is reconsidered in canonical image-ID +order. Gate C multi-view triangulation and point refinement use the complete +eligible observation set. The replacement is published in memory only after +finite-value, positive-depth and reprojection validation; otherwise the prior +valid landmark and its observations remain unchanged. + +The Gate D reference bounds are 4096 input images, 250,000 Tracks, 1,000,000 +observations, 32 seed candidates, 32 registration rounds and 4096 new +landmarks per growth round. The defaults use 1.5 px relative-pose/PnP robust +thresholds, a 2.0 px landmark reprojection threshold, 0.5 minimum inlier +ratios, `1e-4` rad minimum parallax, 6 minimum seed/PnP inliers and 30 +point-refinement iterations. These are Gate D policy defaults; changing them +changes the explicit parameter configuration. + +### Output and failure semantics + +The in-memory result contains deterministic components, registered cameras, +accepted landmarks, landmark observations, reprojection diagnostics and +explicit unregistered images. Results are `COMPLETE`, `PARTIAL` or `FAILED`. +Invalid input fails before computation. A rejected seed, camera or landmark +does not corrupt an accepted model. No partial result is persisted. + +Growth stops immediately when a complete registration round cannot register +an image. It also stops exactly at the configured registration-round bound. +Both paths retain valid cameras and landmarks, list every remaining image as +unregistered and produce `PARTIAL` when usable geometry exists. + +Components with fewer than two registered cameras are not valid 3D components. +Disconnected valid components retain independent unit gauges and are never +globally aligned by Gate D. + +### Gate D limits + +Gate D does not implement BA, persistence adapters, Task Runtime, checkpoints, +Governor integration, a Resource System, GPU execution, dense reconstruction, +metric alignment, viewer integration or any Project DB change. BA remains the +later Gate E; project/task orchestration remains Gate F; resource/freeze +integration remains Gate G. + +### Canonical Gate D functional matrix + +This table freezes the complete numbered validation contract. Evidence is the +minimum dedicated observation required; an earlier rejection never substitutes +for the named path. + +| Case | Purpose | Required path and evidence | Expected result | +|---|---|---|---| +| 01 Minimal two-view | Smallest valid reconstruction | One seed, two cameras, finite landmarks | `COMPLETE` | +| 02 Deterministic seed | Canonical seed identity | Same selected pair and pose on repeat | `COMPLETE` | +| 03 Multiple seed candidates | Candidate ordering | Multiple eligible pairs, canonical first pair | `COMPLETE` | +| 04 Rejected first seed / later seed | Seed fallback | At least two attempts, later pair selected | `COMPLETE` | +| 05 Camera-addition order | Registration ordering | Highest support then image ID, one per round | `COMPLETE` | +| 06 Clean PnP | Nominal registration | PnP attempted and succeeds with clean support | `COMPLETE` | +| 07 Noisy PnP | Bounded noise | PnP succeeds with finite pose | `COMPLETE` | +| 08 Deterministic PnP outliers | Robust registration | Stable inlier count and pose | `COMPLETE` | +| 09 Failed PnP | Registration rejection | Failure counted and image listed | `PARTIAL` | +| 10 Insufficient PnP support | Eligibility bound | Solver not called and image listed | `PARTIAL` | +| 11 Low-parallax rejection | Seed guard | Gate C low-parallax/degenerate status | `FAILED` | +| 12 Pure rotation | Translation degeneracy | Relative pose rejected, no camera | `FAILED` | +| 13 Planar degeneracy | Ambiguous seed | Gate C degeneracy, no camera | `FAILED` | +| 14 Far scene | Finite distant geometry | Seed and finite landmarks accepted | `COMPLETE` | +| 15 Disconnected graph | Component discovery | Valid component plus explicit singleton | `PARTIAL` | +| 16 Multiple valid components | Isolation | Two reconstructed components | `COMPLETE` | +| 17 Independent gauges | Per-component gauge | Each seed camera is identity | `COMPLETE` | +| 18 Unregistered images | Explicit output | Remaining image and component key listed | `PARTIAL` | +| 19 Behind-camera landmark | Cheirality | Exact Gate C status and distinct counter | `COMPLETE` model | +| 20 High reprojection error | Residual policy | Finite triangulation then residual rejection | `COMPLETE` model | +| 21 Failed triangulation | Geometry failure | Finite input calls triangulation and fails | `COMPLETE` model | +| 22 Repeated observations | Track coherence | Duplicate image or feature reference rejected | `INVALID_ARGUMENT` | +| 23 Many-camera Track | Landmark lifecycle | One landmark, six ordered observations | `COMPLETE` | +| 24 New landmark after registration | Incremental growth | Ineligible Track accepted after PnP | `COMPLETE` | +| 25 Multi-view growth | All eligible views | New landmark uses at least three views | `COMPLETE` | +| 26 Point refinement | Bounded refinement | Attempt and finite accepted point | `COMPLETE` | +| 27 No-growth termination | Progress bound | One zero-progress round and diagnostic | `PARTIAL` | +| 28 All-images termination | Natural completion | All images registered, no stop diagnostic | `COMPLETE` | +| 29 Seed exhaustion | Candidate bound | Every available candidate attempted | `FAILED` | +| 30 Registration-round exhaustion | Round bound | Exact rounds and remaining images | `PARTIAL` | +| 31 Component ordering | Canonical components | Increasing component keys | success | +| 32 Camera ordering | Canonical cameras | Increasing image IDs | success | +| 33 Landmark ordering | Canonical landmarks | Increasing `(component_key, track_id)` | success | +| 34 In-process repeatability | Local determinism | Complete scientific result equality | same status | +| 35 Fresh-process repeatability | Process determinism | 20 runs emit one signature | same status | + +### Gate D validation responsibility + +`GATE_D_REQUIRED` covers pointer/count coherence, identities carried by this +API, finite calibration/keypoints, feature-index bounds, Track observation +coherence, geometry failures, atomic result ownership and cleanup. Store-level +Feature Set/File existence is `UPSTREAM_RESPONSIBILITY`: Gate D receives +flattened validated coordinates and never opens a store. Two separate Track +objects with the same ID are `UNREPRESENTABLE_BY_API` because rows are grouped +by `track_id`; duplicate image observations and feature references remain +representable and are rejected. Allocation-failure injection is +`NOT_APPLICABLE_WITH_PROOF`: no allocator injection boundary exists, production +catches allocation failure at the C ABI, and global test allocator state would +violate the architecture. + +| Condition | Classification | +|---|---| +| Null parameters, missing arrays, empty input | `GATE_D_REQUIRED` | +| Zero Track/calibration/image/feature identity | `GATE_D_REQUIRED` | +| Missing per-image calibration coverage | `GATE_D_REQUIRED` | +| Zero/non-finite focal or distortion, invalid principal point | `GATE_D_REQUIRED` | +| Invalid feature index or non-finite keypoint | `GATE_D_REQUIRED` | +| Duplicate image/feature observation or singleton Track | `GATE_D_REQUIRED` | +| Seed/PnP/landmark failures and update rollback | `GATE_D_REQUIRED` | +| Missing Feature Set/File in persistent storage | `UPSTREAM_RESPONSIBILITY` | +| Two distinct Track objects sharing one ID | `UNREPRESENTABLE_BY_API` | +| Deterministic allocation-failure injection | `NOT_APPLICABLE_WITH_PROOF` | + +The caller retains all input allocations for the synchronous call. The result +owns its arrays; `lardon3d_sparse_incremental_result_destroy()` releases them +and accepts an empty result or null pointer. No C++ exception crosses the C17 +boundary. + +Count-limit validation uses structurally sufficient fixtures at a lowered +explicit configured limit and proves `LIMIT-1`, `LIMIT`, and `LIMIT+1` without +materializing the public hard maxima. Scientific scale is validated separately +by the small, medium and large resource workloads. Policy tests prove exact +seed-candidate, registration-round and new-landmark-per-round admission; no +policy loop performs a `limit + 1` attempt. + ## Out of scope No production Sparse SfM, triangulator, camera solver, BA, Project DB v16, diff --git a/include/lardon3d/sparse_sfm_incremental.h b/include/lardon3d/sparse_sfm_incremental.h new file mode 100644 index 0000000..2bd120d --- /dev/null +++ b/include/lardon3d/sparse_sfm_incremental.h @@ -0,0 +1,155 @@ +#ifndef LARDON3D_SPARSE_SFM_INCREMENTAL_H +#define LARDON3D_SPARSE_SFM_INCREMENTAL_H + +#include +#include +#include + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + LARDON3D_SPARSE_INCREMENTAL_COMPLETE = 0, + LARDON3D_SPARSE_INCREMENTAL_PARTIAL, + LARDON3D_SPARSE_INCREMENTAL_FAILED, + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT, + LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY +} Lardon3DSparseIncrementalStatus; + +typedef struct { + uint64_t image_id; + Lardon3DSparseGeometryCalibration calibration; +} Lardon3DSparseIncrementalImage; + +typedef struct { + uint64_t track_id; + uint64_t image_id; + uint64_t feature_set_id; + uint32_t feature_index; + uint32_t feature_count; + double x; + double y; +} Lardon3DSparseIncrementalObservation; + +typedef struct { + uint64_t track_set_id; + uint64_t calibration_scope_id; + const Lardon3DSparseIncrementalImage *images; + size_t image_count; + const Lardon3DSparseIncrementalObservation *observations; + size_t observation_count; +} Lardon3DSparseIncrementalInput; + +typedef struct { + uint32_t minimum_seed_tracks; + uint32_t minimum_seed_landmarks; + uint32_t minimum_pnp_correspondences; + uint32_t maximum_seed_candidates; + uint32_t maximum_registration_rounds; + uint32_t maximum_landmarks_per_round; + uint32_t maximum_images; + uint64_t maximum_observations; + uint64_t maximum_tracks; + double reprojection_threshold_px; + double minimum_track_parallax_rad; + Lardon3DSparseGeometryRelativePoseParameters relative_pose; + Lardon3DSparseGeometryPnPParameters pnp; + Lardon3DSparseGeometryPointRefinementParameters refinement; +} Lardon3DSparseIncrementalParameters; + +typedef struct { + uint64_t image_id; + uint64_t component_key; + Lardon3DSparseGeometryPose pose_cw; +} Lardon3DSparseIncrementalCamera; + +typedef struct { + uint64_t landmark_id; + uint64_t track_id; + uint64_t component_key; + Lardon3DSparseGeometryPoint3 point; + double reprojection_rmse_px; + double reprojection_median_px; + uint64_t observation_count; +} Lardon3DSparseIncrementalLandmark; + +typedef struct { + uint64_t landmark_id; + uint64_t track_id; + uint64_t image_id; + uint64_t feature_set_id; + uint32_t feature_index; + uint32_t position_in_track; +} Lardon3DSparseIncrementalLandmarkObservation; + +typedef struct { + uint64_t component_key; + uint64_t image_count; + uint64_t registered_image_count; + uint64_t landmark_count; +} Lardon3DSparseIncrementalComponent; + +typedef struct { + uint64_t image_id; + uint64_t component_key; +} Lardon3DSparseIncrementalUnregisteredImage; + +typedef struct { + Lardon3DSparseIncrementalStatus status; + uint64_t track_set_id; + uint64_t calibration_scope_id; + Lardon3DSparseIncrementalComponent *components; + size_t component_count; + Lardon3DSparseIncrementalCamera *cameras; + size_t camera_count; + Lardon3DSparseIncrementalLandmark *landmarks; + size_t landmark_count; + Lardon3DSparseIncrementalLandmarkObservation *observations; + size_t observation_count; + Lardon3DSparseIncrementalUnregisteredImage *unregistered_images; + size_t unregistered_image_count; + uint64_t seed_candidates_considered; + uint64_t seed_candidates_available; + uint64_t seed_image_a; + uint64_t seed_image_b; + int32_t last_seed_geometry_status; + double last_seed_parallax_rad; + uint64_t registration_rounds; + uint64_t registration_attempts; + uint64_t registration_successes; + uint64_t registration_failures; + uint32_t last_pnp_inlier_count; + uint64_t triangulation_attempts; + uint64_t triangulation_failures; + uint64_t rejected_behind_camera; + uint64_t rejected_reprojection; + uint64_t rejected_landmarks; + int32_t last_triangulation_status; + uint64_t landmark_update_attempts; + uint64_t landmark_update_successes; + uint64_t landmark_update_failures; + uint64_t no_growth_terminations; + uint64_t round_limit_terminations; + uint64_t point_refinement_attempts; + uint64_t point_refinement_successes; +} Lardon3DSparseIncrementalResult; + +bool lardon3d_sparse_incremental_parameters_default( + Lardon3DSparseIncrementalParameters *parameters); + +Lardon3DSparseIncrementalStatus lardon3d_sparse_incremental_run( + const Lardon3DSparseIncrementalInput *input, + const Lardon3DSparseIncrementalParameters *parameters, + Lardon3DSparseIncrementalResult *result); + +void lardon3d_sparse_incremental_result_destroy( + Lardon3DSparseIncrementalResult *result); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/meson.build b/meson.build index 10e670d..c4dfc1b 100644 --- a/meson.build +++ b/meson.build @@ -145,6 +145,7 @@ executable( 'src/project.c', 'src/project_db.c', 'src/project_db_sparse_sfm.c', 'src/sparse_sfm_geometry.cpp', + 'src/sparse_sfm_incremental.cpp', 'src/task.c', 'src/task_checkpoint.c', 'src/task_kind_registry.c', @@ -533,6 +534,28 @@ sparse_sfm_geometry_test = executable( test('sparse-sfm-geometry', sparse_sfm_geometry_test, timeout: 60) +sparse_sfm_incremental_test = executable( + 'test-sparse-sfm-incremental', + sources: [ + 'tests/test_sparse_sfm_incremental.cpp', + 'src/sparse_sfm_geometry.cpp', 'src/sparse_sfm_incremental.cpp', + ], + include_directories: include_directories('include'), + dependencies: [opencv_geometry], +) + +test('sparse-sfm-incremental', sparse_sfm_incremental_test, timeout: 60) + +executable( + 'benchmark-sparse-sfm-incremental', + sources: [ + 'tests/benchmark_sparse_sfm_incremental.cpp', + 'src/sparse_sfm_geometry.cpp', 'src/sparse_sfm_incremental.cpp', + ], + include_directories: include_directories('include'), + dependencies: [opencv_geometry], +) + sparse_sfm_resource_test = executable( 'test-sparse-sfm-resource', sources: [ diff --git a/src/sparse_sfm_incremental.cpp b/src/sparse_sfm_incremental.cpp new file mode 100644 index 0000000..7e21dd1 --- /dev/null +++ b/src/sparse_sfm_incremental.cpp @@ -0,0 +1,774 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +struct Observation { + uint64_t track_id; + uint64_t image_id; + uint64_t feature_set_id; + uint32_t feature_index; + Lardon3DSparseGeometryPoint2 pixel; +}; + +struct Track { + uint64_t id; + std::vector observations; +}; + +struct Image { + uint64_t id; + Lardon3DSparseGeometryCalibration calibration; + size_t component; +}; + +struct Pose { + uint64_t image_id; + Lardon3DSparseGeometryPose value; +}; + +struct Landmark { + uint64_t track_id; + uint64_t component_key; + Lardon3DSparseGeometryPoint3 point; + std::vector observations; + double rmse; + double median; +}; + +struct Component { + uint64_t key; + std::vector image_indices; +}; + +struct UnionFind { + std::vector parent; + + explicit UnionFind(size_t count) : parent(count) { + for (size_t index = 0; index < count; ++index) parent[index] = index; + } + + size_t find(size_t value) { + size_t root = value; + while (parent[root] != root) root = parent[root]; + while (parent[value] != value) { + size_t next = parent[value]; + parent[value] = root; + value = next; + } + return root; + } + + void unite(size_t left, size_t right) { + left = find(left); + right = find(right); + if (left == right) return; + if (left < right) + parent[right] = left; + else + parent[left] = right; + } +}; + +bool finite_calibration(const Lardon3DSparseGeometryCalibration &calibration) { + return calibration.width > 0 && calibration.height > 0 && + std::isfinite(calibration.fx) && std::isfinite(calibration.fy) && + std::isfinite(calibration.cx) && std::isfinite(calibration.cy) && + std::isfinite(calibration.k1) && std::isfinite(calibration.k2) && + std::isfinite(calibration.p1) && std::isfinite(calibration.p2) && + calibration.fx > 0.0 && calibration.fy > 0.0 && + calibration.cx >= 0.0 && calibration.cx < calibration.width && + calibration.cy >= 0.0 && calibration.cy < calibration.height; +} + +bool finite_pose(const Lardon3DSparseGeometryPose &pose) { + for (double value : pose.rotation_cw) + if (!std::isfinite(value)) return false; + for (double value : pose.translation_cw) + if (!std::isfinite(value)) return false; + return true; +} + +Lardon3DSparseGeometryPose identity_pose() { + return {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, + {0.0, 0.0, 0.0}}; +} + +bool project(const Image &image, const Pose &pose, + const Lardon3DSparseGeometryPoint3 &point, + Lardon3DSparseGeometryPoint2 *pixel) { + const double *r = pose.value.rotation_cw; + const double *t = pose.value.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 (!pixel || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || + z <= 1e-9) + return false; + pixel->x = image.calibration.fx * x / z + image.calibration.cx; + pixel->y = image.calibration.fy * y / z + image.calibration.cy; + return std::isfinite(pixel->x) && std::isfinite(pixel->y); +} + +double reprojection_error(const Image &image, const Pose &pose, + const Landmark &landmark, + const Observation &observation) { + Lardon3DSparseGeometryPoint2 projected; + if (!project(image, pose, landmark.point, &projected)) return INFINITY; + const double dx = projected.x - observation.pixel.x; + const double dy = projected.y - observation.pixel.y; + return std::sqrt(dx * dx + dy * dy); +} + +bool validate_parameters(const Lardon3DSparseIncrementalParameters &p) { + return p.minimum_seed_tracks >= 2 && p.minimum_seed_landmarks >= 1 && + p.minimum_pnp_correspondences >= 4 && p.maximum_seed_candidates > 0 && + p.maximum_registration_rounds > 0 && p.maximum_landmarks_per_round > 0 && + p.maximum_images >= 2 && p.maximum_observations >= p.maximum_tracks && + std::isfinite(p.reprojection_threshold_px) && + p.reprojection_threshold_px > 0.0 && + std::isfinite(p.minimum_track_parallax_rad) && + p.minimum_track_parallax_rad >= 0.0 && + p.relative_pose.minimum_inliers >= p.minimum_seed_tracks && + p.pnp.minimum_inliers >= p.minimum_pnp_correspondences && + p.relative_pose.minimum_inlier_ratio > 0.0 && + p.relative_pose.minimum_inlier_ratio <= 1.0 && + p.pnp.minimum_inlier_ratio > 0.0 && p.pnp.minimum_inlier_ratio <= 1.0; +} + +void destroy_result(Lardon3DSparseIncrementalResult *result) { + if (!result) return; + std::free(result->components); + std::free(result->cameras); + std::free(result->landmarks); + std::free(result->observations); + std::free(result->unregistered_images); + *result = {}; +} + +bool append_component(const Component &component, + const std::vector &images, + std::vector *out) { + Lardon3DSparseIncrementalComponent value = {}; + value.component_key = component.key; + value.image_count = component.image_indices.size(); + for (size_t index : component.image_indices) + if (images[index].id == component.key) value.component_key = images[index].id; + out->push_back(value); + return true; +} + +enum class LandmarkCandidateStatus { + accepted, + insufficient, + triangulation_failed, + behind_camera, + reprojection_failed, +}; + +LandmarkCandidateStatus build_landmark_candidate( + const Track &track, uint64_t component_key, + const std::vector &images, + const std::map &image_index, + const std::vector &cameras, const std::set ®istered, + const Lardon3DSparseIncrementalParameters ¶meters, + Landmark *candidate, Lardon3DSparseGeometryResult *geometry_status, + Lardon3DSparseIncrementalResult *result) { + std::vector normalized; + std::vector poses; + std::vector used; + for (const Observation &observation : track.observations) { + if (!registered.count(observation.image_id)) continue; + const auto pose_it = std::find_if( + cameras.begin(), cameras.end(), [&](const Pose &pose) { + return pose.image_id == observation.image_id; + }); + if (pose_it == cameras.end()) continue; + const Image &image = images[image_index.at(observation.image_id)]; + Lardon3DSparseGeometryPoint2 point; + if (lardon3d_sparse_geometry_normalize( + &image.calibration, &observation.pixel, 1, &point) != + LARDON3D_SPARSE_GEOMETRY_OK) + return LandmarkCandidateStatus::triangulation_failed; + normalized.push_back(point); + poses.push_back(pose_it->value); + used.push_back(observation); + } + if (used.size() < 2) return LandmarkCandidateStatus::insufficient; + + Lardon3DSparseGeometryPoint3 point; + *geometry_status = lardon3d_sparse_geometry_triangulate_multi_view( + normalized.data(), poses.data(), normalized.size(), &point); + if (*geometry_status != LARDON3D_SPARSE_GEOMETRY_OK) { + return *geometry_status == LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED + ? LandmarkCandidateStatus::behind_camera + : LandmarkCandidateStatus::triangulation_failed; + } + Lardon3DSparseGeometryPoint3 refined; + ++result->point_refinement_attempts; + const Lardon3DSparseGeometryResult refinement_status = + lardon3d_sparse_geometry_refine_point( + normalized.data(), poses.data(), normalized.size(), &point, + ¶meters.refinement, &refined); + if (refinement_status == LARDON3D_SPARSE_GEOMETRY_OK) { + point = refined; + ++result->point_refinement_successes; + } + + Landmark replacement = { + track.id, component_key, point, std::move(used), 0.0, 0.0}; + double squared = 0.0; + double maximum = 0.0; + for (const Observation &observation : replacement.observations) { + const Image &image = images[image_index.at(observation.image_id)]; + const Pose &pose = *std::find_if( + cameras.begin(), cameras.end(), [&](const Pose &item) { + return item.image_id == observation.image_id; + }); + const double error = reprojection_error( + image, pose, replacement, observation); + if (!std::isfinite(error)) return LandmarkCandidateStatus::behind_camera; + if (error > parameters.reprojection_threshold_px) + return LandmarkCandidateStatus::reprojection_failed; + squared += error * error; + maximum = std::max(maximum, error); + } + replacement.rmse = std::sqrt( + squared / static_cast(replacement.observations.size())); + replacement.median = maximum; + *candidate = std::move(replacement); + return LandmarkCandidateStatus::accepted; +} + +void record_landmark_rejection(LandmarkCandidateStatus status, + Lardon3DSparseIncrementalResult *result) { + if (status == LandmarkCandidateStatus::behind_camera) { + ++result->rejected_landmarks; + ++result->rejected_behind_camera; + } else if (status == LandmarkCandidateStatus::reprojection_failed) { + ++result->rejected_landmarks; + ++result->rejected_reprojection; + } else if (status == LandmarkCandidateStatus::triangulation_failed) { + ++result->triangulation_failures; + } +} + +} // namespace + +extern "C" bool lardon3d_sparse_incremental_parameters_default( + Lardon3DSparseIncrementalParameters *parameters) { + if (!parameters) return false; + *parameters = {}; + parameters->minimum_seed_tracks = 6; + parameters->minimum_seed_landmarks = 6; + parameters->minimum_pnp_correspondences = 6; + parameters->maximum_seed_candidates = 32; + parameters->maximum_registration_rounds = 32; + parameters->maximum_landmarks_per_round = 4096; + parameters->maximum_images = 4096; + parameters->maximum_observations = 1000000; + parameters->maximum_tracks = 250000; + parameters->reprojection_threshold_px = 2.0; + parameters->minimum_track_parallax_rad = 1e-4; + parameters->relative_pose = {1.5, 0.999, 1500, 6, 0.5, 1e-4, 0.5, 0}; + parameters->pnp = {1.5, 0.999, 1000, 6, 0.5, 0}; + parameters->refinement = {30, 1e-12}; + return true; +} + +extern "C" Lardon3DSparseIncrementalStatus lardon3d_sparse_incremental_run( + const Lardon3DSparseIncrementalInput *input, + const Lardon3DSparseIncrementalParameters *parameters, + Lardon3DSparseIncrementalResult *result) { + if (!result) return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + destroy_result(result); + try { + if (!input || !parameters || !validate_parameters(*parameters) || + !input->images || input->image_count == 0 || !input->observations || + input->observation_count == 0 || input->image_count > parameters->maximum_images || + input->observation_count > parameters->maximum_observations || + input->track_set_id == 0 || + input->calibration_scope_id == 0) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + + std::vector images; + images.reserve(input->image_count); + for (size_t index = 0; index < input->image_count; ++index) { + const auto &source = input->images[index]; + if (source.image_id == 0 || !finite_calibration(source.calibration)) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + images.push_back({source.image_id, source.calibration, 0}); + } + std::sort(images.begin(), images.end(), + [](const Image &a, const Image &b) { return a.id < b.id; }); + for (size_t index = 1; index < images.size(); ++index) + if (images[index - 1].id == images[index].id) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + + std::map image_index; + for (size_t index = 0; index < images.size(); ++index) + image_index[images[index].id] = index; + + std::map tracks_by_id; + for (size_t index = 0; index < input->observation_count; ++index) { + const auto &source = input->observations[index]; + if (source.track_id == 0 || source.image_id == 0 || + source.feature_set_id == 0 || source.feature_index >= source.feature_count || + image_index.find(source.image_id) == image_index.end() || + !std::isfinite(source.x) || !std::isfinite(source.y)) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + Track &track = tracks_by_id[source.track_id]; + track.id = source.track_id; + for (const Observation &old : track.observations) + if (old.image_id == source.image_id) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + track.observations.push_back( + {source.track_id, source.image_id, source.feature_set_id, + source.feature_index, {source.x, source.y}}); + } + if (tracks_by_id.empty() || tracks_by_id.size() > parameters->maximum_tracks) + return LARDON3D_SPARSE_INCREMENTAL_FAILED; + std::set> feature_observations; + for (const auto &entry : tracks_by_id) { + if (entry.second.observations.size() < 2) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + for (const Observation &observation : entry.second.observations) + if (!feature_observations.insert( + {observation.feature_set_id, observation.feature_index}) + .second) + return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT; + } + for (auto &entry : tracks_by_id) { + std::sort(entry.second.observations.begin(), + entry.second.observations.end(), + [](const Observation &left, const Observation &right) { + return left.image_id < right.image_id; + }); + } + + UnionFind union_find(images.size()); + for (const auto &entry : tracks_by_id) { + const auto &observations = entry.second.observations; + for (size_t index = 1; index < observations.size(); ++index) + union_find.unite(image_index[observations[0].image_id], + image_index[observations[index].image_id]); + } + std::map components_by_root; + for (size_t index = 0; index < images.size(); ++index) + components_by_root[union_find.find(index)].image_indices.push_back(index); + std::vector components; + for (auto &entry : components_by_root) { + auto &items = entry.second.image_indices; + std::sort(items.begin(), items.end(), [&](size_t a, size_t b) { + return images[a].id < images[b].id; + }); + entry.second.key = images[items.front()].id; + components.push_back(entry.second); + for (size_t item : items) images[item].component = components.size() - 1; + } + std::sort(components.begin(), components.end(), + [](const Component &a, const Component &b) { return a.key < b.key; }); + std::map component_by_image; + for (size_t component = 0; component < components.size(); ++component) + for (size_t image : components[component].image_indices) { + images[image].component = component; + component_by_image[images[image].id] = component; + } + + std::vector output_components; + for (const Component &component : components) + append_component(component, images, &output_components); + std::vector cameras; + std::vector landmarks; + std::set registered; + std::set accepted_tracks; + std::map component_keys; + for (const Component &component : components) component_keys[component.key] = component.key; + + for (const Component &component : components) { + if (component.image_indices.size() < 2) continue; + struct Candidate { uint64_t a; uint64_t b; uint32_t shared; }; + std::vector candidates; + std::map, uint32_t> shared_by_pair; + for (const auto &entry : tracks_by_id) { + std::vector track_images; + for (const Observation &observation : entry.second.observations) + if (std::find(track_images.begin(), track_images.end(), observation.image_id) == + track_images.end()) + track_images.push_back(observation.image_id); + std::sort(track_images.begin(), track_images.end()); + for (size_t left = 0; left < track_images.size(); ++left) + for (size_t right = left + 1; right < track_images.size(); ++right) + if (component_by_image[track_images[left]] == + component_by_image[track_images[right]] && + component_by_image[track_images[left]] == + component_by_image[component.key]) + ++shared_by_pair[{track_images[left], track_images[right]}]; + } + for (const auto &entry : shared_by_pair) + if (entry.second >= parameters->minimum_seed_tracks) + candidates.push_back({entry.first.first, entry.first.second, entry.second}); + std::sort(candidates.begin(), candidates.end(), + [](const Candidate &a, const Candidate &b) { + if (a.shared != b.shared) return a.shared > b.shared; + if (a.a != b.a) return a.a < b.a; + return a.b < b.b; + }); + const size_t candidate_limit = std::min( + candidates.size(), parameters->maximum_seed_candidates); + result->seed_candidates_available += candidates.size(); + bool seeded = false; + for (size_t candidate_index = 0; candidate_index < candidate_limit; + ++candidate_index) { + ++result->seed_candidates_considered; + const Candidate &candidate = candidates[candidate_index]; + const Image &image_a = images[image_index[candidate.a]]; + const Image &image_b = images[image_index[candidate.b]]; + std::vector pair_a, pair_b; + std::vector track_ids; + for (const auto &entry : tracks_by_id) { + const Observation *a = nullptr, *b = nullptr; + for (const Observation &observation : entry.second.observations) { + if (observation.image_id == candidate.a) a = &observation; + if (observation.image_id == candidate.b) b = &observation; + } + if (a && b) { + track_ids.push_back(entry.first); + pair_a.push_back(*a); + pair_b.push_back(*b); + } + } + std::vector pixels_a(pair_a.size()); + std::vector pixels_b(pair_b.size()); + for (size_t i = 0; i < pair_a.size(); ++i) { + pixels_a[i] = pair_a[i].pixel; + pixels_b[i] = pair_b[i].pixel; + } + std::vector mask(pair_a.size()); + Lardon3DSparseGeometryRelativePoseResult pose_result = {}; + pose_result.inlier_mask = mask.data(); + pose_result.inlier_mask_capacity = mask.size(); + auto relative = parameters->relative_pose; + relative.minimum_inliers = std::max( + relative.minimum_inliers, parameters->minimum_seed_tracks); + const Lardon3DSparseGeometryResult relative_status = + lardon3d_sparse_geometry_relative_pose( + &image_a.calibration, &image_b.calibration, pixels_a.data(), + pixels_b.data(), pixels_a.size(), &relative, &pose_result); + result->last_seed_geometry_status = relative_status; + result->last_seed_parallax_rad = pose_result.median_parallax_rad; + if (relative_status != LARDON3D_SPARSE_GEOMETRY_OK || + pose_result.inlier_count < parameters->minimum_seed_landmarks || + pose_result.median_parallax_rad < parameters->minimum_track_parallax_rad || + !finite_pose(pose_result.pose_ba)) + continue; + Pose pose_a = {candidate.a, identity_pose()}; + Pose pose_b = {candidate.b, pose_result.pose_ba}; + std::vector seed_landmarks; + for (size_t i = 0; i < pair_a.size(); ++i) { + if (!mask[i]) continue; + ++result->triangulation_attempts; + Lardon3DSparseGeometryPoint2 normalized[2]; + if (lardon3d_sparse_geometry_normalize(&image_a.calibration, + &pair_a[i].pixel, 1, + &normalized[0]) != + LARDON3D_SPARSE_GEOMETRY_OK || + lardon3d_sparse_geometry_normalize(&image_b.calibration, + &pair_b[i].pixel, 1, + &normalized[1]) != + LARDON3D_SPARSE_GEOMETRY_OK) + continue; + Lardon3DSparseGeometryPoint3 point; + if (lardon3d_sparse_geometry_triangulate_two_view( + &normalized[0], &normalized[1], &pose_a.value, &pose_b.value, + &point) != LARDON3D_SPARSE_GEOMETRY_OK) + { + ++result->triangulation_failures; + continue; + } + Lardon3DSparseGeometryPoint3 refined; + Lardon3DSparseGeometryPose seed_poses[2] = {pose_a.value, pose_b.value}; + if (lardon3d_sparse_geometry_refine_point( + normalized, seed_poses, 2, &point, + ¶meters->refinement, &refined) == + LARDON3D_SPARSE_GEOMETRY_OK) + point = refined; + Landmark landmark = {track_ids[i], component.key, point, {pair_a[i], pair_b[i]}, + 0.0, 0.0}; + const double error_a = reprojection_error(image_a, pose_a, landmark, pair_a[i]); + const double error_b = reprojection_error(image_b, pose_b, landmark, pair_b[i]); + if (!std::isfinite(error_a) || !std::isfinite(error_b) || + std::max(error_a, error_b) > parameters->reprojection_threshold_px) + { + if (!std::isfinite(error_a) || !std::isfinite(error_b)) + ++result->rejected_behind_camera; + else + ++result->rejected_reprojection; + continue; + } + landmark.rmse = std::sqrt((error_a * error_a + error_b * error_b) / 2.0); + landmark.median = std::max(error_a, error_b); + seed_landmarks.push_back(landmark); + } + if (seed_landmarks.size() < parameters->minimum_seed_landmarks) continue; + cameras.push_back(pose_a); + cameras.push_back(pose_b); + registered.insert(candidate.a); + registered.insert(candidate.b); + for (Landmark &landmark : seed_landmarks) { + accepted_tracks.insert(landmark.track_id); + landmarks.push_back(landmark); + } + seeded = true; + result->seed_image_a = candidate.a; + result->seed_image_b = candidate.b; + break; + } + if (!seeded) continue; + + bool stopped_without_growth = false; + for (uint32_t round = 0; + round < parameters->maximum_registration_rounds; ++round) { + bool all_component_images_registered = true; + for (size_t image_position : component.image_indices) + if (!registered.count(images[image_position].id)) { + all_component_images_registered = false; + break; + } + if (all_component_images_registered) break; + ++result->registration_rounds; + struct CandidateImage { uint64_t id; uint32_t support; }; + std::vector image_candidates; + for (size_t image_position : component.image_indices) { + const uint64_t image_id = images[image_position].id; + if (registered.count(image_id)) continue; + uint32_t support = 0; + for (const Landmark &landmark : landmarks) { + for (const Observation &observation : tracks_by_id[landmark.track_id].observations) + if (observation.image_id == image_id) { ++support; break; } + } + if (support >= parameters->minimum_pnp_correspondences) + image_candidates.push_back({image_id, support}); + } + std::sort(image_candidates.begin(), image_candidates.end(), + [](const CandidateImage &a, const CandidateImage &b) { + if (a.support != b.support) return a.support > b.support; + return a.id < b.id; + }); + if (image_candidates.empty()) { + stopped_without_growth = true; + break; + } + bool registered_one = false; + for (const CandidateImage &candidate : image_candidates) { + const Image &image = images[image_index[candidate.id]]; + std::vector points; + std::vector pixels; + std::vector track_ids; + for (const Landmark &landmark : landmarks) { + const Track &track = tracks_by_id[landmark.track_id]; + for (const Observation &observation : track.observations) + if (observation.image_id == candidate.id) { + points.push_back(landmark.point); + pixels.push_back(observation.pixel); + track_ids.push_back(landmark.track_id); + break; + } + } + std::vector mask(points.size()); + Lardon3DSparseGeometryPnPResult pnp_result = {}; + pnp_result.inlier_mask = mask.data(); + pnp_result.inlier_mask_capacity = mask.size(); + ++result->registration_attempts; + if (lardon3d_sparse_geometry_pnp( + &image.calibration, points.data(), pixels.data(), points.size(), + ¶meters->pnp, &pnp_result) != LARDON3D_SPARSE_GEOMETRY_OK || + !finite_pose(pnp_result.pose_cw)) + { + ++result->registration_failures; + continue; + } + ++result->registration_successes; + result->last_pnp_inlier_count = pnp_result.inlier_count; + Pose pose = {candidate.id, pnp_result.pose_cw}; + cameras.push_back(pose); + registered.insert(candidate.id); + registered_one = true; + break; + } + if (!registered_one) { + stopped_without_growth = true; + break; + } + + for (Landmark &landmark : landmarks) { + if (landmark.component_key != component.key) continue; + const Track &track = tracks_by_id[landmark.track_id]; + size_t eligible_count = 0; + for (const Observation &observation : track.observations) + if (registered.count(observation.image_id)) ++eligible_count; + if (eligible_count <= landmark.observations.size()) continue; + ++result->landmark_update_attempts; + ++result->triangulation_attempts; + Landmark replacement; + Lardon3DSparseGeometryResult geometry_status = + LARDON3D_SPARSE_GEOMETRY_OK; + const LandmarkCandidateStatus update_status = + build_landmark_candidate( + track, component.key, images, image_index, cameras, + registered, *parameters, &replacement, &geometry_status, + result); + result->last_triangulation_status = geometry_status; + if (update_status == LandmarkCandidateStatus::accepted) { + landmark = std::move(replacement); + ++result->landmark_update_successes; + } else { + ++result->landmark_update_failures; + record_landmark_rejection(update_status, result); + } + } + + size_t added = 0; + for (const auto &entry : tracks_by_id) { + if (accepted_tracks.count(entry.first) || added >= parameters->maximum_landmarks_per_round) + continue; + size_t eligible_count = 0; + for (const Observation &observation : entry.second.observations) + if (registered.count(observation.image_id)) ++eligible_count; + if (eligible_count < 2) continue; + ++result->triangulation_attempts; + Landmark landmark; + Lardon3DSparseGeometryResult geometry_status = + LARDON3D_SPARSE_GEOMETRY_OK; + const LandmarkCandidateStatus candidate_status = + build_landmark_candidate( + entry.second, component.key, images, image_index, cameras, + registered, *parameters, &landmark, &geometry_status, result); + result->last_triangulation_status = geometry_status; + if (candidate_status != LandmarkCandidateStatus::accepted) { + record_landmark_rejection(candidate_status, result); + continue; + } + landmarks.push_back(landmark); + accepted_tracks.insert(entry.first); + ++added; + } + } + size_t component_registered = 0; + for (size_t image_position : component.image_indices) + if (registered.count(images[image_position].id)) ++component_registered; + if (component_registered < component.image_indices.size()) { + if (stopped_without_growth) + ++result->no_growth_terminations; + else + ++result->round_limit_terminations; + } + } + + std::sort(cameras.begin(), cameras.end(), + [](const Pose &a, const Pose &b) { return a.image_id < b.image_id; }); + std::sort(landmarks.begin(), landmarks.end(), + [](const Landmark &a, const Landmark &b) { + if (a.component_key != b.component_key) + return a.component_key < b.component_key; + return a.track_id < b.track_id; + }); + for (const Component &component : components) { + for (auto &output : output_components) + if (output.component_key == component.key) { + for (const Pose &camera : cameras) + if (component_by_image[camera.image_id] == + component_by_image[component.key]) ++output.registered_image_count; + for (const Landmark &landmark : landmarks) + if (landmark.component_key == component.key) ++output.landmark_count; + } + for (size_t image_index_value : component.image_indices) { + const uint64_t image_id = images[image_index_value].id; + if (!registered.count(image_id)) + result->unregistered_image_count++; + } + } + + result->component_count = output_components.size(); + result->camera_count = cameras.size(); + result->landmark_count = landmarks.size(); + if (result->component_count) { + result->components = static_cast( + std::malloc(result->component_count * sizeof(*result->components))); + if (!result->components) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; } + std::copy(output_components.begin(), output_components.end(), result->components); + } + if (result->camera_count) { + result->cameras = static_cast( + std::malloc(result->camera_count * sizeof(*result->cameras))); + if (!result->cameras) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; } + for (size_t i = 0; i < cameras.size(); ++i) + result->cameras[i] = { + cameras[i].image_id, + components[component_by_image[cameras[i].image_id]].key, + cameras[i].value}; + } + if (result->landmark_count) { + result->landmarks = static_cast( + std::malloc(result->landmark_count * sizeof(*result->landmarks))); + if (!result->landmarks) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; } + size_t observation_count = 0; + for (const Landmark &landmark : landmarks) observation_count += landmark.observations.size(); + result->observation_count = observation_count; + result->observations = static_cast( + std::malloc(observation_count * sizeof(*result->observations))); + if (!result->observations) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; } + size_t observation_offset = 0; + for (size_t i = 0; i < landmarks.size(); ++i) { + const Landmark &landmark = landmarks[i]; + result->landmarks[i] = {landmark.track_id, landmark.track_id, landmark.component_key, + landmark.point, landmark.rmse, landmark.median, + landmark.observations.size()}; + for (size_t position = 0; position < landmark.observations.size(); ++position) { + const Observation &observation = landmark.observations[position]; + result->observations[observation_offset++] = { + landmark.track_id, landmark.track_id, observation.image_id, + observation.feature_set_id, observation.feature_index, + static_cast(position)}; + } + } + } + if (result->unregistered_image_count) { + result->unregistered_images = static_cast( + std::malloc(result->unregistered_image_count * sizeof(*result->unregistered_images))); + if (!result->unregistered_images) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; } + size_t offset = 0; + for (const Image &image : images) + if (!registered.count(image.id)) + result->unregistered_images[offset++] = { + image.id, components[component_by_image[image.id]].key}; + } + result->track_set_id = input->track_set_id; + result->calibration_scope_id = input->calibration_scope_id; + result->status = result->camera_count == 0 + ? LARDON3D_SPARSE_INCREMENTAL_FAILED + : result->unregistered_image_count == 0 + ? LARDON3D_SPARSE_INCREMENTAL_COMPLETE + : LARDON3D_SPARSE_INCREMENTAL_PARTIAL; + return result->status; + } catch (const std::bad_alloc &) { + destroy_result(result); + return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; + } catch (...) { + destroy_result(result); + return LARDON3D_SPARSE_INCREMENTAL_FAILED; + } +} + +extern "C" void lardon3d_sparse_incremental_result_destroy( + Lardon3DSparseIncrementalResult *result) { + destroy_result(result); +} diff --git a/tests/benchmark_sparse_sfm_incremental.cpp b/tests/benchmark_sparse_sfm_incremental.cpp new file mode 100644 index 0000000..be2e39f --- /dev/null +++ b/tests/benchmark_sparse_sfm_incremental.cpp @@ -0,0 +1,91 @@ +#include + +#include +#include +#include +#include +#include +#include + +static size_t open_file_descriptor_count() { + DIR *directory = opendir("/proc/self/fd"); + if (!directory) return 0; + size_t count = 0; + while (readdir(directory)) ++count; + closedir(directory); + return count >= 2 ? count - 2 : 0; +} + +static Lardon3DSparseGeometryPoint2 project( + const Lardon3DSparseGeometryCalibration &calibration, + const Lardon3DSparseGeometryPose &pose, + const Lardon3DSparseGeometryPoint3 &point) { + const double x = point.x + pose.translation_cw[0]; + const double y = point.y + pose.translation_cw[1]; + const double z = point.z + pose.translation_cw[2]; + return {calibration.fx * x / z + calibration.cx, + calibration.fy * y / z + calibration.cy}; +} + +int main(int argc, char **argv) { + const size_t camera_count = argc > 1 ? std::strtoull(argv[1], nullptr, 10) : 8; + const size_t track_count = argc > 2 ? std::strtoull(argv[2], nullptr, 10) : 500; + if (camera_count < 2 || track_count < 6 || camera_count > 64 || + track_count > 10000) + return EXIT_FAILURE; + const Lardon3DSparseGeometryCalibration calibration = { + 4000, 3000, 2000.0, 2000.0, 2000.0, 1500.0, 0.0, 0.0, 0.0, 0.0}; + std::vector images; + std::vector observations; + images.reserve(camera_count); + observations.reserve(camera_count * track_count); + for (size_t camera = 0; camera < camera_count; ++camera) + images.push_back({100 + camera, calibration}); + for (size_t track = 0; track < track_count; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -1.5 + static_cast(track % 31) * 0.1, + -1.0 + static_cast((track / 31) % 23) * 0.09, + 8.0 + static_cast(track % 17) * 0.07}; + for (size_t camera = 0; camera < camera_count; ++camera) { + Lardon3DSparseGeometryPose pose = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, + {static_cast(camera) * 0.3, 0, 0}}; + const Lardon3DSparseGeometryPoint2 pixel = project(calibration, pose, point); + observations.push_back({track + 1, 100 + camera, 1000 + camera, + static_cast(track), 8192, + pixel.x, pixel.y}); + } + } + Lardon3DSparseIncrementalParameters parameters; + if (!lardon3d_sparse_incremental_parameters_default(¶meters)) return EXIT_FAILURE; + parameters.maximum_registration_rounds = 64; + Lardon3DSparseIncrementalInput input = { + 1, 2, images.data(), images.size(), observations.data(), observations.size()}; + Lardon3DSparseIncrementalResult result = {}; + const auto started = std::chrono::steady_clock::now(); + const Lardon3DSparseIncrementalStatus status = + lardon3d_sparse_incremental_run(&input, ¶meters, &result); + const auto stopped = std::chrono::steady_clock::now(); + struct rusage usage = {}; + getrusage(RUSAGE_SELF, &usage); + const double wall_seconds = + std::chrono::duration(stopped - started).count(); + const double cpu_seconds = + static_cast(usage.ru_utime.tv_sec + usage.ru_stime.tv_sec) + + static_cast(usage.ru_utime.tv_usec + usage.ru_stime.tv_usec) / + 1000000.0; + std::printf("status=%d cameras=%zu tracks=%zu observations=%zu components=%zu " + "registered=%zu landmarks=%zu rounds=%llu seeds=%llu " + "wall_s=%.6f cpu_s=%.6f peak_rss_kib=%ld fds=%zu\n", + static_cast(status), camera_count, track_count, + observations.size(), result.component_count, result.camera_count, + result.landmark_count, + static_cast(result.registration_rounds), + static_cast(result.seed_candidates_considered), + wall_seconds, cpu_seconds, usage.ru_maxrss, + open_file_descriptor_count()); + const bool valid = status == LARDON3D_SPARSE_INCREMENTAL_COMPLETE && + result.camera_count == camera_count && result.landmark_count > 0; + lardon3d_sparse_incremental_result_destroy(&result); + return valid ? EXIT_SUCCESS : EXIT_FAILURE; +} diff --git a/tests/test_sparse_sfm_incremental.cpp b/tests/test_sparse_sfm_incremental.cpp new file mode 100644 index 0000000..0dd1d5f --- /dev/null +++ b/tests/test_sparse_sfm_incremental.cpp @@ -0,0 +1,1364 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define CHECK(value) \ + do { \ + if (!(value)) { \ + std::fprintf(stderr, "incremental check failed at line %d: %s\n", \ + __LINE__, #value); \ + return false; \ + } \ + } while (0) + +static Lardon3DSparseGeometryPoint2 project( + const Lardon3DSparseGeometryCalibration &calibration, + const Lardon3DSparseGeometryPose &pose, + const Lardon3DSparseGeometryPoint3 &point) { + const double x = pose.rotation_cw[0] * point.x + pose.rotation_cw[1] * point.y + + pose.rotation_cw[2] * point.z + pose.translation_cw[0]; + const double y = pose.rotation_cw[3] * point.x + pose.rotation_cw[4] * point.y + + pose.rotation_cw[5] * point.z + pose.translation_cw[1]; + const double z = pose.rotation_cw[6] * point.x + pose.rotation_cw[7] * point.y + + pose.rotation_cw[8] * point.z + pose.translation_cw[2]; + return {calibration.fx * x / z + calibration.cx, + calibration.fy * y / z + calibration.cy}; +} + +static bool run_test() { + Lardon3DSparseIncrementalParameters parameters; + CHECK(lardon3d_sparse_incremental_parameters_default(¶meters)); + parameters.minimum_seed_tracks = 6; + parameters.minimum_seed_landmarks = 6; + parameters.minimum_pnp_correspondences = 6; + parameters.maximum_registration_rounds = 8; + + const Lardon3DSparseGeometryCalibration calibration = { + 4000, 3000, 2000.0, 2000.0, 2000.0, 1500.0, 0.0, 0.0, 0.0, 0.0}; + const Lardon3DSparseGeometryPose poses[3] = { + {{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}}, + {{1, 0, 0, 0, 1, 0, 0, 0, 1}, {1, 0, 0}}, + {{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 0, 0}}, + }; + Lardon3DSparseIncrementalImage images[3] = { + {10, calibration}, {20, calibration}, {30, calibration}}; + std::vector observations; + for (uint64_t track = 1; track <= 12; ++track) { + Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(track % 4) * 0.5, + -0.8 + static_cast(track / 4) * 0.4, + 5.0 + static_cast(track % 3) * 0.25}; + for (size_t camera = 0; camera < 3; ++camera) { + Lardon3DSparseGeometryPoint2 pixel = project(calibration, poses[camera], point); + observations.push_back({track, images[camera].image_id, 100 + camera, + static_cast(track), 64, pixel.x, pixel.y}); + } + } + Lardon3DSparseIncrementalInput input = { + 77, 88, images, 3, observations.data(), observations.size()}; + Lardon3DSparseIncrementalResult result = {}; + CHECK(lardon3d_sparse_incremental_run(&input, ¶meters, &result) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(result.component_count == 1); + CHECK(result.camera_count == 3); + CHECK(result.landmark_count >= 6); + CHECK(result.unregistered_image_count == 0); + CHECK(result.components[0].component_key == 10); + for (size_t index = 0; index < result.camera_count; ++index) + CHECK(result.cameras[index].component_key == 10); + for (size_t index = 1; index < result.camera_count; ++index) + CHECK(result.cameras[index - 1].image_id < result.cameras[index].image_id); + for (size_t index = 1; index < result.landmark_count; ++index) + CHECK(result.landmarks[index - 1].track_id < result.landmarks[index].track_id); + + Lardon3DSparseIncrementalResult repeated = {}; + CHECK(lardon3d_sparse_incremental_run(&input, ¶meters, &repeated) == + result.status); + CHECK(repeated.camera_count == result.camera_count); + CHECK(repeated.landmark_count == result.landmark_count); + for (size_t index = 0; index < result.camera_count; ++index) { + CHECK(repeated.cameras[index].image_id == result.cameras[index].image_id); + for (size_t value = 0; value < 9; ++value) + CHECK(repeated.cameras[index].pose_cw.rotation_cw[value] == + result.cameras[index].pose_cw.rotation_cw[value]); + } + lardon3d_sparse_incremental_result_destroy(&repeated); + + Lardon3DSparseIncrementalInput disconnected = input; + Lardon3DSparseIncrementalImage singleton = {99, calibration}; + std::vector more_images = {images[0], images[1], + images[2], singleton}; + disconnected.images = more_images.data(); + disconnected.image_count = more_images.size(); + Lardon3DSparseIncrementalResult partial = {}; + CHECK(lardon3d_sparse_incremental_run(&disconnected, ¶meters, &partial) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(partial.unregistered_image_count == 1); + CHECK(partial.unregistered_images[0].image_id == 99); + lardon3d_sparse_incremental_result_destroy(&partial); + + Lardon3DSparseIncrementalResult invalid = {}; + Lardon3DSparseIncrementalInput invalid_input = input; + invalid_input.observations = nullptr; + CHECK(lardon3d_sparse_incremental_run(&invalid_input, ¶meters, &invalid) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + CHECK(invalid.camera_count == 0 && invalid.landmark_count == 0); + + auto expect_invalid = [&](const Lardon3DSparseIncrementalInput &candidate, + const Lardon3DSparseIncrementalParameters &limits) { + Lardon3DSparseIncrementalResult output = {}; + const auto status = lardon3d_sparse_incremental_run( + &candidate, &limits, &output); + const bool valid = status == LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT && + output.camera_count == 0 && output.landmark_count == 0; + lardon3d_sparse_incremental_result_destroy(&output); + return valid; + }; + Lardon3DSparseIncrementalInput malformed = input; + malformed.images = nullptr; + CHECK(expect_invalid(malformed, parameters)); + malformed = input; + malformed.observations = observations.data(); + std::vector bad_observations = observations; + bad_observations[0].feature_count = 1; + malformed.observations = bad_observations.data(); + CHECK(expect_invalid(malformed, parameters)); + bad_observations = observations; + bad_observations[0].x = NAN; + malformed.observations = bad_observations.data(); + CHECK(expect_invalid(malformed, parameters)); + bad_observations = observations; + bad_observations.back().track_id = bad_observations.front().track_id; + bad_observations.back().image_id = bad_observations.front().image_id; + malformed.observations = bad_observations.data(); + CHECK(expect_invalid(malformed, parameters)); + Lardon3DSparseIncrementalParameters bounded = parameters; + bounded.maximum_images = 2; + CHECK(expect_invalid(input, bounded)); + bounded = parameters; + bounded.maximum_observations = observations.size() - 1; + CHECK(expect_invalid(input, bounded)); + bounded = parameters; + bounded.maximum_tracks = 1; + Lardon3DSparseIncrementalResult bounded_result = {}; + CHECK(lardon3d_sparse_incremental_run(&input, &bounded, &bounded_result) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + lardon3d_sparse_incremental_result_destroy(&bounded_result); + Lardon3DSparseIncrementalParameters invalid_limits = parameters; + invalid_limits.maximum_registration_rounds = 0; + CHECK(expect_invalid(input, invalid_limits)); + + std::vector flat = observations; + for (auto &observation : flat) { + observation.x = 2100.0; + observation.y = 1500.0; + } + malformed = input; + malformed.observations = flat.data(); + Lardon3DSparseIncrementalResult exhausted = {}; + CHECK(lardon3d_sparse_incremental_run(&malformed, ¶meters, &exhausted) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(exhausted.seed_candidates_considered > 0); + lardon3d_sparse_incremental_result_destroy(&exhausted); + + lardon3d_sparse_incremental_result_destroy(&result); + return true; +} + +struct BatchFixture { + Lardon3DSparseGeometryCalibration calibration; + std::vector images; + std::vector observations; +}; + +static BatchFixture make_batch_fixture(size_t camera_count, size_t track_count, + bool reject_first_seed, bool noisy, + bool outliers, bool fail_pnp) { + BatchFixture fixture; + fixture.calibration = {4000, 3000, 2000.0, 2000.0, 2000.0, 1500.0, + 0.0, 0.0, 0.0, 0.0}; + for (size_t camera = 0; camera < camera_count; ++camera) + fixture.images.push_back({10 + camera, fixture.calibration}); + for (size_t track = 0; track < track_count; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(track % 4) * 0.5, + -0.8 + static_cast(track / 4) * 0.4, + 5.0 + static_cast(track % 3) * 0.25}; + for (size_t camera = 0; camera < camera_count; ++camera) { + double translation = static_cast(camera); + if (reject_first_seed && camera == 1) translation = 1e-3; + const Lardon3DSparseGeometryPose pose = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {translation, 0, 0}}; + Lardon3DSparseGeometryPoint2 pixel = + project(fixture.calibration, pose, point); + if (noisy && camera == 2) { + pixel.x += static_cast((track * 17) % 5) * 0.12 - 0.24; + pixel.y += static_cast((track * 11) % 5) * 0.10 - 0.20; + } + if (outliers && camera == 2 && track < 3) { + pixel.x += 900.0 + static_cast(track) * 30.0; + pixel.y -= 700.0; + } + if (fail_pnp && camera == 2) { + pixel.x = 300.0 + static_cast((track * 613) % 3300); + pixel.y = 200.0 + static_cast((track * 977) % 2300); + } + fixture.observations.push_back( + {track + 1, 10 + camera, 100 + camera, + static_cast(track), + static_cast(track_count + 1), pixel.x, pixel.y}); + } + } + return fixture; +} + +static BatchFixture make_growth_fixture(int kind) { + BatchFixture fixture = make_batch_fixture(3, 12, false, false, false, false); + fixture.images[2].image_id = 13; + for (auto &observation : fixture.observations) + if (observation.image_id == 12) observation.image_id = 13; + fixture.observations.erase( + std::remove_if(fixture.observations.begin(), fixture.observations.end(), + [](const auto &observation) { + return observation.image_id == 13 && observation.track_id > 6; + }), + fixture.observations.end()); + const Lardon3DSparseGeometryPose growth_pose = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, + {2, 0, kind == 19 ? -4.5 : 0}}; + if (kind == 19) { + for (auto &observation : fixture.observations) { + if (observation.image_id != 13) continue; + const size_t track = static_cast(observation.track_id - 1); + const Lardon3DSparseGeometryPoint3 support = { + -1.0 + static_cast(track % 4) * 0.5, + -0.8 + static_cast(track / 4) * 0.4, + 5.0 + static_cast(track % 3) * 0.25}; + const auto pixel = project(fixture.calibration, growth_pose, support); + observation.x = pixel.x; + observation.y = pixel.y; + } + } + const Lardon3DSparseGeometryPoint3 point = { + 0.25, 0.15, kind == 19 ? 4.0 : 5.5}; + const Lardon3DSparseGeometryPose pose_a = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}}; + const auto pixel_a = project(fixture.calibration, pose_a, point); + auto pixel_b = project(fixture.calibration, growth_pose, point); + if (kind == 21) pixel_b = pixel_a; + if (kind == 19 || kind == 20 || kind == 21 || kind == 25) { + const Lardon3DSparseGeometryPose pose_mid = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {1, 0, 0}}; + const auto pixel_mid = project(fixture.calibration, pose_mid, point); + fixture.observations.push_back({ + 13, 11, 101, 12, 64, + kind == 21 ? pixel_a.x + : kind == 20 ? pixel_mid.x + 700.0 : pixel_mid.x, + kind == 21 ? pixel_a.y + : kind == 20 ? pixel_mid.y - 500.0 : pixel_mid.y}); + } + fixture.observations.push_back({13, 10, 100, 12, 64, pixel_a.x, pixel_a.y}); + fixture.observations.push_back({13, 13, 102, 12, 64, pixel_b.x, pixel_b.y}); + return fixture; +} + +static bool run_batch_cases() { + Lardon3DSparseIncrementalParameters parameters; + CHECK(lardon3d_sparse_incremental_parameters_default(¶meters)); + parameters.minimum_seed_tracks = 6; + parameters.minimum_seed_landmarks = 6; + parameters.minimum_pnp_correspondences = 6; + parameters.maximum_registration_rounds = 8; + + /* CASE 01: minimal valid two-view reconstruction. */ + { + const BatchFixture minimal = + make_batch_fixture(2, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 301, 302, minimal.images.data(), minimal.images.size(), + minimal.observations.data(), minimal.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.camera_count == 2); + CHECK(output.landmark_count >= parameters.minimum_seed_landmarks); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 02: deterministic seed selection. */ + { + const BatchFixture deterministic = + make_batch_fixture(3, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 303, 304, deterministic.images.data(), deterministic.images.size(), + deterministic.observations.data(), deterministic.observations.size()}; + Lardon3DSparseIncrementalResult first = {}, second = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &first) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &second) == + first.status); + CHECK(first.seed_image_a == 10 && first.seed_image_b == 11); + CHECK(second.seed_image_a == first.seed_image_a); + CHECK(second.seed_image_b == first.seed_image_b); + CHECK(std::memcmp(first.cameras, second.cameras, + first.camera_count * sizeof(*first.cameras)) == 0); + lardon3d_sparse_incremental_result_destroy(&second); + lardon3d_sparse_incremental_result_destroy(&first); + } + + /* CASE 03: multiple eligible candidates use canonical ordering. */ + { + const BatchFixture multiple = + make_batch_fixture(4, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 305, 306, multiple.images.data(), multiple.images.size(), + multiple.observations.data(), multiple.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.seed_candidates_available == 6); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.seed_image_a == 10 && output.seed_image_b == 11); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 04: first seed rejected, later seed accepted. */ + const BatchFixture fixture = make_batch_fixture(4, 12, true, false, false, false); + const Lardon3DSparseIncrementalInput input = { + 401, 402, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult result = {}; + CHECK(lardon3d_sparse_incremental_run(&input, ¶meters, &result) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(result.seed_candidates_considered >= 2); + CHECK(result.seed_image_a == 10 && result.seed_image_b == 12); + CHECK(result.camera_count == 4); + lardon3d_sparse_incremental_result_destroy(&result); + + /* CASE 05: equal-support cameras are added by image ID, one per round. */ + { + const BatchFixture ordered = + make_batch_fixture(5, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters one_round = parameters; + one_round.maximum_registration_rounds = 1; + const Lardon3DSparseIncrementalInput candidate = { + 307, 308, ordered.images.data(), ordered.images.size(), + ordered.observations.data(), ordered.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, &one_round, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.registration_rounds == 1); + CHECK(output.registration_successes == 1); + CHECK(output.camera_count == 3); + CHECK(output.cameras[2].image_id == 12); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 06: clean calibrated PnP registers one camera. */ + { + const BatchFixture clean = + make_batch_fixture(3, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 309, 310, clean.images.data(), clean.images.size(), + clean.observations.data(), clean.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.registration_attempts == 1); + CHECK(output.registration_successes == 1); + CHECK(output.last_pnp_inlier_count == 12); + CHECK(output.camera_count == 3); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 07: deterministic noisy PnP registration. */ + { + const BatchFixture noisy = make_batch_fixture(3, 12, false, true, false, false); + const Lardon3DSparseIncrementalInput noisy_input = { + 403, 404, noisy.images.data(), noisy.images.size(), + noisy.observations.data(), noisy.observations.size()}; + Lardon3DSparseIncrementalResult noisy_result = {}; + CHECK(lardon3d_sparse_incremental_run(&noisy_input, ¶meters, + &noisy_result) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(noisy_result.registration_attempts >= 1); + CHECK(noisy_result.registration_successes >= 1); + CHECK(noisy_result.last_pnp_inlier_count >= + parameters.minimum_pnp_correspondences); + CHECK(noisy_result.camera_count == 3); + for (size_t index = 0; index < noisy_result.camera_count; ++index) + for (double value : noisy_result.cameras[index].pose_cw.rotation_cw) + CHECK(std::isfinite(value)); + lardon3d_sparse_incremental_result_destroy(&noisy_result); + } + + /* CASE 08: deterministic PnP outliers. */ + { + const BatchFixture outliers = make_batch_fixture(3, 12, false, false, true, false); + const Lardon3DSparseIncrementalInput outlier_input = { + 405, 406, outliers.images.data(), outliers.images.size(), + outliers.observations.data(), outliers.observations.size()}; + Lardon3DSparseIncrementalResult outlier_result = {}; + CHECK(lardon3d_sparse_incremental_run(&outlier_input, ¶meters, + &outlier_result) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(outlier_result.registration_attempts >= 1); + CHECK(outlier_result.registration_successes == 1); + CHECK(outlier_result.last_pnp_inlier_count >= 9); + CHECK(outlier_result.camera_count == 3); + lardon3d_sparse_incremental_result_destroy(&outlier_result); + } + + /* CASE 09: PnP-stage registration failure. */ + { + const BatchFixture failed = make_batch_fixture(3, 12, false, false, false, true); + const Lardon3DSparseIncrementalInput failed_input = { + 407, 408, failed.images.data(), failed.images.size(), + failed.observations.data(), failed.observations.size()}; + Lardon3DSparseIncrementalResult failed_result = {}; + CHECK(lardon3d_sparse_incremental_run(&failed_input, ¶meters, + &failed_result) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(failed_result.registration_attempts >= 1); + CHECK(failed_result.registration_failures >= 1); + CHECK(failed_result.registration_successes == 0); + CHECK(failed_result.camera_count == 2); + CHECK(failed_result.unregistered_image_count == 1); + CHECK(failed_result.unregistered_images[0].image_id == 12); + lardon3d_sparse_incremental_result_destroy(&failed_result); + } + + /* CASE 10: insufficient PnP support skips the solver. */ + { + const BatchFixture fixture = make_batch_fixture(3, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters insufficient = parameters; + insufficient.minimum_pnp_correspondences = 20; + insufficient.pnp.minimum_inliers = 20; + const Lardon3DSparseIncrementalInput candidate = { + 409, 410, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, &insufficient, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.camera_count == 2); + CHECK(output.unregistered_image_count == 1); + CHECK(output.registration_attempts == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 11: valid low-parallax seed reaches Gate C and is rejected. */ + { + const BatchFixture fixture = make_batch_fixture(2, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters low_parallax = parameters; + low_parallax.relative_pose.minimum_parallax_rad = 1.0; + const Lardon3DSparseIncrementalInput candidate = { + 411, 412, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, &low_parallax, &output) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.last_seed_geometry_status == LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX || + output.last_seed_geometry_status == LARDON3D_SPARSE_GEOMETRY_DEGENERATE); + CHECK(output.camera_count == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 12: pure rotation reaches relative pose handling and rejects. */ + { + BatchFixture fixture = make_batch_fixture(2, 12, true, false, false, false); + const double c = std::cos(0.25), s = std::sin(0.25); + for (size_t track = 0; track < 12; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(track % 4) * 0.5, + -0.8 + static_cast(track / 4) * 0.4, + 5.0 + static_cast(track % 3) * 0.25}; + const Lardon3DSparseGeometryPose rotation = { + {c, -s, 0, s, c, 0, 0, 0, 1}, {0, 0, 0}}; + const auto pixel = project(fixture.calibration, rotation, point); + fixture.observations[track * 2 + 1].x = pixel.x; + fixture.observations[track * 2 + 1].y = pixel.y; + } + const Lardon3DSparseIncrementalInput candidate = { + 413, 414, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.last_seed_geometry_status != LARDON3D_SPARSE_GEOMETRY_OK || + output.landmark_count == 0); + CHECK(output.camera_count == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 13: planar seed is consumed as a Gate C degeneracy rejection. */ + { + BatchFixture fixture = make_batch_fixture(2, 12, false, false, false, false); + for (size_t track = 0; track < 12; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(track % 4) * 0.5, 0.0, 5.0}; + const Lardon3DSparseGeometryPose shifted = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {1, 0, 0}}; + const auto pixel = project(fixture.calibration, shifted, point); + fixture.observations[track * 2 + 1].x = pixel.x; + fixture.observations[track * 2 + 1].y = pixel.y; + } + const Lardon3DSparseIncrementalInput candidate = { + 415, 416, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.last_seed_geometry_status != LARDON3D_SPARSE_GEOMETRY_OK); + CHECK(output.camera_count == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 14: far but recoverable scene remains finite and bounded. */ + { + BatchFixture fixture = make_batch_fixture(2, 12, false, false, false, false); + for (size_t track = 0; track < 12; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(track % 4) * 0.5, + -0.8 + static_cast(track / 4) * 0.4, 100.0}; + const Lardon3DSparseGeometryPose shifted = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {1, 0, 0}}; + const auto pixel = project(fixture.calibration, shifted, point); + fixture.observations[track * 2 + 1].x = pixel.x; + fixture.observations[track * 2 + 1].y = pixel.y; + } + const Lardon3DSparseIncrementalInput candidate = { + 417, 418, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.seed_image_a == 10 && output.seed_image_b == 11); + CHECK(output.camera_count == 2); + CHECK(output.landmark_count >= 6); + for (size_t index = 0; index < output.landmark_count; ++index) { + CHECK(std::isfinite(output.landmarks[index].point.x)); + CHECK(std::isfinite(output.landmarks[index].point.y)); + CHECK(std::isfinite(output.landmarks[index].point.z)); + } + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 15: disconnected graph discovery retains a singleton component. */ + { + BatchFixture disconnected = + make_batch_fixture(3, 12, false, false, false, false); + disconnected.images.push_back({99, disconnected.calibration}); + const Lardon3DSparseIncrementalInput candidate = { + 4171, 4172, disconnected.images.data(), disconnected.images.size(), + disconnected.observations.data(), disconnected.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.component_count == 2); + CHECK(output.components[1].component_key == 99); + CHECK(output.components[1].registered_image_count == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 16: two independently reconstructable components. */ + { + BatchFixture fixture = make_batch_fixture(4, 24, false, false, false, false); + std::vector split; + for (const auto &observation : fixture.observations) { + const bool first = observation.track_id <= 12; + const bool keep = first ? observation.image_id <= 11 : observation.image_id >= 12; + if (!keep) continue; + auto copy = observation; + if (!first) { + copy.image_id += 10; + copy.feature_set_id += 10; + } + split.push_back(copy); + } + fixture.images = {{10, fixture.calibration}, {11, fixture.calibration}, + {22, fixture.calibration}, {23, fixture.calibration}}; + fixture.observations = split; + const Lardon3DSparseIncrementalInput candidate = { + 419, 420, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.component_count == 2); + CHECK(output.components[0].component_key == 10); + CHECK(output.components[1].component_key == 22); + CHECK(output.components[0].registered_image_count == 2); + CHECK(output.components[1].registered_image_count == 2); + CHECK(output.components[0].landmark_count >= 6); + CHECK(output.components[1].landmark_count >= 6); + /* CASE 17: both independent component seed cameras fix identity gauge. */ + size_t identity_gauges = 0; + for (size_t index = 0; index < output.camera_count; ++index) { + if (output.cameras[index].image_id != output.cameras[index].component_key) + continue; + const Lardon3DSparseGeometryPose identity = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}}; + CHECK(std::memcmp(&output.cameras[index].pose_cw, &identity, + sizeof(identity)) == 0); + ++identity_gauges; + } + CHECK(identity_gauges == 2); + /* CASE 31: component output is canonical. */ + for (size_t index = 1; index < output.component_count; ++index) + CHECK(output.components[index - 1].component_key < + output.components[index].component_key); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 18: every unreconstructed image is explicit and component-keyed. */ + { + BatchFixture incomplete = + make_batch_fixture(3, 12, false, false, false, false); + incomplete.images.push_back({99, incomplete.calibration}); + const Lardon3DSparseIncrementalInput candidate = { + 4191, 4192, incomplete.images.data(), incomplete.images.size(), + incomplete.observations.data(), incomplete.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.unregistered_image_count == 1); + CHECK(output.unregistered_images[0].image_id == 99); + CHECK(output.unregistered_images[0].component_key == 99); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 19: Gate C returns exact cheirality failure for a finite point that + * is in front of the seed cameras and behind the newly registered camera. */ + { + const BatchFixture fixture = make_growth_fixture(19); + const Lardon3DSparseIncrementalInput candidate = { + 421, 422, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.registration_successes >= 1); + CHECK(output.triangulation_attempts > 0); + CHECK(output.last_triangulation_status == + LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED); + CHECK(output.triangulation_failures == 0); + CHECK(output.rejected_behind_camera > 0); + CHECK(output.rejected_reprojection == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 20: a finite growth candidate is rejected by reprojection error. */ + { + const BatchFixture fixture = make_growth_fixture(20); + const Lardon3DSparseIncrementalInput candidate = { + 423, 424, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.registration_successes >= 1); + CHECK(output.triangulation_attempts > 0); + CHECK(output.rejected_reprojection > 0); + CHECK(output.rejected_behind_camera == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 21: a growth Track reaches and fails triangulation. */ + { + const BatchFixture fixture = make_growth_fixture(21); + const Lardon3DSparseIncrementalInput candidate = { + 425, 426, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.registration_successes >= 1); + CHECK(output.triangulation_attempts > 0); + CHECK(output.triangulation_failures > 0); + CHECK(output.rejected_behind_camera == 0); + CHECK(output.rejected_reprojection == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 22: repeated image observations and feature references are rejected. */ + { + const BatchFixture valid = + make_batch_fixture(2, 12, false, false, false, false); + std::vector duplicate_image = + valid.observations; + duplicate_image.push_back(duplicate_image.front()); + duplicate_image.back().feature_index += 1; + Lardon3DSparseIncrementalInput candidate = { + 4251, 4252, valid.images.data(), valid.images.size(), + duplicate_image.data(), duplicate_image.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + lardon3d_sparse_incremental_result_destroy(&output); + + std::vector duplicate_feature = + valid.observations; + duplicate_feature[2].feature_set_id = duplicate_feature[0].feature_set_id; + duplicate_feature[2].feature_index = duplicate_feature[0].feature_index; + candidate.observations = duplicate_feature.data(); + candidate.observation_count = duplicate_feature.size(); + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 23: one seed Track is atomically updated to all six cameras. */ + { + const BatchFixture fixture = make_batch_fixture(6, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 425, 426, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + size_t found = 0; + for (size_t index = 0; index < output.landmark_count; ++index) + if (output.landmarks[index].track_id == 1) { + ++found; + CHECK(output.landmarks[index].landmark_id == 1); + CHECK(output.landmarks[index].observation_count == 6); + CHECK(std::isfinite(output.landmarks[index].point.x)); + CHECK(std::isfinite(output.landmarks[index].point.y)); + CHECK(std::isfinite(output.landmarks[index].point.z)); + } + CHECK(found == 1); + size_t observation_count = 0; + uint64_t previous_image_id = 0; + for (size_t index = 0; index < output.observation_count; ++index) { + if (output.observations[index].track_id != 1) continue; + CHECK(output.observations[index].image_id > previous_image_id); + CHECK(output.observations[index].position_in_track == observation_count); + previous_image_id = output.observations[index].image_id; + ++observation_count; + } + CHECK(observation_count == 6); + CHECK(output.landmark_update_attempts > 0); + CHECK(output.landmark_update_successes > 0); + + Lardon3DSparseIncrementalResult repeated = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &repeated) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(repeated.landmark_count == output.landmark_count); + CHECK(repeated.observation_count == output.observation_count); + CHECK(std::memcmp(repeated.landmarks, output.landmarks, + output.landmark_count * sizeof(*output.landmarks)) == 0); + CHECK(std::memcmp(repeated.observations, output.observations, + output.observation_count * sizeof(*output.observations)) == 0); + lardon3d_sparse_incremental_result_destroy(&repeated); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 23 rollback: one bad newly registered observation cannot replace the + * valid two-view landmark. */ + { + BatchFixture rollback = + make_batch_fixture(3, 12, false, false, false, false); + BatchFixture reference = rollback; + reference.images.pop_back(); + reference.observations.erase( + std::remove_if(reference.observations.begin(), + reference.observations.end(), + [](const auto &observation) { + return observation.image_id == 12; + }), + reference.observations.end()); + const Lardon3DSparseIncrementalInput reference_input = { + 427, 428, reference.images.data(), reference.images.size(), + reference.observations.data(), reference.observations.size()}; + Lardon3DSparseIncrementalResult reference_output = {}; + CHECK(lardon3d_sparse_incremental_run( + &reference_input, ¶meters, &reference_output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + Lardon3DSparseGeometryPoint3 reference_point = {}; + for (size_t index = 0; index < reference_output.landmark_count; ++index) + if (reference_output.landmarks[index].track_id == 1) + reference_point = reference_output.landmarks[index].point; + for (auto &observation : rollback.observations) + if (observation.track_id == 1 && observation.image_id == 12) { + observation.x += 900.0; + observation.y -= 700.0; + } + const Lardon3DSparseIncrementalInput candidate = { + 427, 428, rollback.images.data(), rollback.images.size(), + rollback.observations.data(), rollback.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.landmark_update_failures > 0); + size_t found = 0; + for (size_t index = 0; index < output.landmark_count; ++index) + if (output.landmarks[index].track_id == 1) { + ++found; + CHECK(output.landmarks[index].observation_count == 2); + CHECK(std::memcmp(&output.landmarks[index].point, &reference_point, + sizeof(reference_point)) == 0); + } + CHECK(found == 1); + lardon3d_sparse_incremental_result_destroy(&reference_output); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 24: a Track with no seed landmark becomes eligible after PnP. */ + { + const BatchFixture growth = make_growth_fixture(0); + const Lardon3DSparseIncrementalInput candidate = { + 4281, 4282, growth.images.data(), growth.images.size(), + growth.observations.data(), growth.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.registration_successes == 1); + size_t found = 0; + for (size_t index = 0; index < output.landmark_count; ++index) + if (output.landmarks[index].track_id == 13) ++found; + CHECK(found == 1); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 25: a new growth landmark consumes all three registered views. */ + { + const BatchFixture growth = make_growth_fixture(25); + const Lardon3DSparseIncrementalInput candidate = { + 4283, 4284, growth.images.data(), growth.images.size(), + growth.observations.data(), growth.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + size_t found = 0; + for (size_t index = 0; index < output.landmark_count; ++index) + if (output.landmarks[index].track_id == 13) { + ++found; + CHECK(output.landmarks[index].observation_count == 3); + } + CHECK(found == 1); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 26: point-only refinement is attempted and accepted. */ + { + const BatchFixture growth = make_growth_fixture(25); + const Lardon3DSparseIncrementalInput candidate = { + 4285, 4286, growth.images.data(), growth.images.size(), + growth.observations.data(), growth.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.point_refinement_attempts > 0); + CHECK(output.point_refinement_successes > 0); + for (size_t index = 0; index < output.landmark_count; ++index) { + CHECK(std::isfinite(output.landmarks[index].point.x)); + CHECK(std::isfinite(output.landmarks[index].point.y)); + CHECK(std::isfinite(output.landmarks[index].point.z)); + } + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 27: a valid seed remains intact when a full registration attempt + * makes no progress. */ + { + const BatchFixture blocked = + make_batch_fixture(3, 12, false, false, false, true); + const Lardon3DSparseIncrementalInput candidate = { + 429, 430, blocked.images.data(), blocked.images.size(), + blocked.observations.data(), blocked.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.camera_count == 2); + CHECK(output.landmark_count >= parameters.minimum_seed_landmarks); + CHECK(output.registration_rounds == 1); + CHECK(output.registration_attempts > 0); + CHECK(output.registration_successes == 0); + CHECK(output.no_growth_terminations == 1); + CHECK(output.round_limit_terminations == 0); + CHECK(output.unregistered_image_count == 1); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 28: natural completion registers every image without stop reason. */ + { + const BatchFixture complete = + make_batch_fixture(5, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput candidate = { + 4301, 4302, complete.images.data(), complete.images.size(), + complete.observations.data(), complete.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.camera_count == 5); + CHECK(output.registration_rounds == 3); + CHECK(output.unregistered_image_count == 0); + CHECK(output.no_growth_terminations == 0); + CHECK(output.round_limit_terminations == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 29: the only available seed is attempted and exhausted. */ + { + BatchFixture exhausted = + make_batch_fixture(2, 12, false, false, false, false); + for (auto &observation : exhausted.observations) { + observation.x = 2100.0; + observation.y = 1500.0; + } + const Lardon3DSparseIncrementalInput candidate = { + 4303, 4304, exhausted.images.data(), exhausted.images.size(), + exhausted.observations.data(), exhausted.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(output.seed_candidates_available == 1); + CHECK(output.seed_candidates_considered == 1); + CHECK(output.camera_count == 0); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 30: one permitted round registers exactly one of three remaining + * cameras and stops at the configured bound. */ + { + const BatchFixture bounded = + make_batch_fixture(5, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters one_round = parameters; + one_round.maximum_registration_rounds = 1; + const Lardon3DSparseIncrementalInput candidate = { + 431, 432, bounded.images.data(), bounded.images.size(), + bounded.observations.data(), bounded.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, &one_round, &output) == + LARDON3D_SPARSE_INCREMENTAL_PARTIAL); + CHECK(output.registration_rounds == 1); + CHECK(output.registration_successes == 1); + CHECK(output.camera_count == 3); + CHECK(output.unregistered_image_count == 2); + CHECK(output.no_growth_terminations == 0); + CHECK(output.round_limit_terminations == 1); + + Lardon3DSparseIncrementalResult repeated = {}; + CHECK(lardon3d_sparse_incremental_run( + &candidate, &one_round, &repeated) == output.status); + CHECK(repeated.registration_rounds == output.registration_rounds); + CHECK(repeated.camera_count == output.camera_count); + CHECK(repeated.unregistered_image_count == output.unregistered_image_count); + CHECK(std::memcmp(repeated.cameras, output.cameras, + output.camera_count * sizeof(*output.cameras)) == 0); + CHECK(std::memcmp(repeated.unregistered_images, + output.unregistered_images, + output.unregistered_image_count * + sizeof(*output.unregistered_images)) == 0); + lardon3d_sparse_incremental_result_destroy(&repeated); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* CASE 32: camera output is strictly ordered by image identity. */ + /* CASE 33: landmark output is strictly ordered by Track identity. */ + /* CASE 34: a complete in-process rerun has identical scientific arrays and + * deterministic diagnostics. */ + { + BatchFixture canonical = + make_batch_fixture(5, 12, false, false, false, false); + std::reverse(canonical.images.begin(), canonical.images.end()); + std::reverse(canonical.observations.begin(), canonical.observations.end()); + const Lardon3DSparseIncrementalInput candidate = { + 433, 434, canonical.images.data(), canonical.images.size(), + canonical.observations.data(), canonical.observations.size()}; + Lardon3DSparseIncrementalResult first = {}, second = {}; + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &first) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(lardon3d_sparse_incremental_run(&candidate, ¶meters, &second) == + first.status); + for (size_t index = 1; index < first.camera_count; ++index) + CHECK(first.cameras[index - 1].image_id < first.cameras[index].image_id); + for (size_t index = 1; index < first.landmark_count; ++index) + CHECK(first.landmarks[index - 1].component_key < + first.landmarks[index].component_key || + (first.landmarks[index - 1].component_key == + first.landmarks[index].component_key && + first.landmarks[index - 1].track_id < + first.landmarks[index].track_id)); + CHECK(first.component_count == second.component_count); + CHECK(first.camera_count == second.camera_count); + CHECK(first.landmark_count == second.landmark_count); + CHECK(first.observation_count == second.observation_count); + CHECK(first.unregistered_image_count == second.unregistered_image_count); + CHECK(std::memcmp(first.components, second.components, + first.component_count * sizeof(*first.components)) == 0); + CHECK(std::memcmp(first.cameras, second.cameras, + first.camera_count * sizeof(*first.cameras)) == 0); + CHECK(std::memcmp(first.landmarks, second.landmarks, + first.landmark_count * sizeof(*first.landmarks)) == 0); + CHECK(std::memcmp(first.observations, second.observations, + first.observation_count * sizeof(*first.observations)) == 0); + CHECK(first.seed_candidates_considered == + second.seed_candidates_considered); + CHECK(first.registration_rounds == second.registration_rounds); + CHECK(first.triangulation_attempts == second.triangulation_attempts); + CHECK(first.landmark_update_successes == + second.landmark_update_successes); + lardon3d_sparse_incremental_result_destroy(&second); + lardon3d_sparse_incremental_result_destroy(&first); + } + return true; +} + +static bool run_boundary_cases() { + Lardon3DSparseIncrementalParameters parameters; + CHECK(lardon3d_sparse_incremental_parameters_default(¶meters)); + parameters.minimum_seed_tracks = 6; + parameters.minimum_seed_landmarks = 6; + parameters.minimum_pnp_correspondences = 6; + + /* Input count validation: maximum_images LIMIT-1, LIMIT, LIMIT+1. */ + for (size_t camera_count : {size_t{2}, size_t{3}, size_t{4}}) { + const BatchFixture fixture = make_batch_fixture( + camera_count, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters bounded = parameters; + bounded.maximum_images = 3; + const Lardon3DSparseIncrementalInput input = { + 501, 502, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + const auto status = + lardon3d_sparse_incremental_run(&input, &bounded, &output); + CHECK(camera_count <= 3 ? status == LARDON3D_SPARSE_INCREMENTAL_COMPLETE + : status == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* Input count validation: maximum_tracks LIMIT-1, LIMIT, LIMIT+1. */ + for (size_t track_count : {size_t{11}, size_t{12}, size_t{13}}) { + const BatchFixture fixture = make_batch_fixture( + 2, track_count, false, false, false, false); + Lardon3DSparseIncrementalParameters bounded = parameters; + bounded.maximum_tracks = 12; + const Lardon3DSparseIncrementalInput input = { + 503, 504, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + const auto status = + lardon3d_sparse_incremental_run(&input, &bounded, &output); + CHECK(track_count <= 12 ? status == LARDON3D_SPARSE_INCREMENTAL_COMPLETE + : status == LARDON3D_SPARSE_INCREMENTAL_FAILED); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* Input count validation: maximum_observations LIMIT-1, LIMIT, LIMIT+1. */ + const BatchFixture observation_fixture = + make_batch_fixture(3, 12, false, false, false, false); + for (size_t observation_count : {size_t{34}, size_t{35}, size_t{36}}) { + std::vector observations = + observation_fixture.observations; + if (observation_count <= 35) observations.erase(observations.begin() + 35); + if (observation_count == 34) observations.erase(observations.begin() + 32); + Lardon3DSparseIncrementalParameters bounded = parameters; + bounded.maximum_observations = 35; + bounded.maximum_tracks = 12; + const Lardon3DSparseIncrementalInput input = { + 505, 506, observation_fixture.images.data(), + observation_fixture.images.size(), observations.data(), + observations.size()}; + Lardon3DSparseIncrementalResult output = {}; + const auto status = + lardon3d_sparse_incremental_run(&input, &bounded, &output); + CHECK(observation_count <= 35 + ? status == LARDON3D_SPARSE_INCREMENTAL_COMPLETE + : status == LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + lardon3d_sparse_incremental_result_destroy(&output); + } + + /* Policy boundary: one seed candidate fails, two reach the valid fallback. */ + const BatchFixture seeds = + make_batch_fixture(4, 12, true, false, false, false); + const Lardon3DSparseIncrementalInput seed_input = { + 507, 508, seeds.images.data(), seeds.images.size(), + seeds.observations.data(), seeds.observations.size()}; + Lardon3DSparseIncrementalParameters one_seed = parameters; + one_seed.maximum_seed_candidates = 1; + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(&seed_input, &one_seed, &output) == + LARDON3D_SPARSE_INCREMENTAL_FAILED); + CHECK(output.seed_candidates_considered == 1); + lardon3d_sparse_incremental_result_destroy(&output); + one_seed.maximum_seed_candidates = 2; + CHECK(lardon3d_sparse_incremental_run(&seed_input, &one_seed, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.seed_candidates_considered == 2); + lardon3d_sparse_incremental_result_destroy(&output); + + /* Policy boundary: at most two new landmarks are admitted in one round. */ + BatchFixture growth = + make_batch_fixture(3, 12, false, false, false, false); + growth.observations.erase( + std::remove_if( + growth.observations.begin(), growth.observations.end(), + [](const auto &observation) { + return observation.image_id == 12 && observation.track_id > 6; + }), + growth.observations.end()); + for (uint64_t track = 13; track <= 15; ++track) { + const Lardon3DSparseGeometryPoint3 point = { + -0.5 + static_cast(track - 13) * 0.3, 0.2, 6.0}; + for (size_t camera : {size_t{0}, size_t{2}}) { + const Lardon3DSparseGeometryPose pose = { + {1, 0, 0, 0, 1, 0, 0, 0, 1}, + {static_cast(camera), 0, 0}}; + const auto pixel = project(growth.calibration, pose, point); + growth.observations.push_back( + {track, 10 + camera, 100 + camera, + static_cast(track), 16, pixel.x, pixel.y}); + } + } + Lardon3DSparseIncrementalParameters two_landmarks = parameters; + two_landmarks.maximum_landmarks_per_round = 2; + const Lardon3DSparseIncrementalInput growth_input = { + 509, 510, growth.images.data(), growth.images.size(), + growth.observations.data(), growth.observations.size()}; + CHECK(lardon3d_sparse_incremental_run( + &growth_input, &two_landmarks, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + CHECK(output.landmark_count == 14); + lardon3d_sparse_incremental_result_destroy(&output); + return true; +} + +static bool run_failure_cases() { + Lardon3DSparseIncrementalParameters parameters; + CHECK(lardon3d_sparse_incremental_parameters_default(¶meters)); + parameters.minimum_seed_tracks = 6; + parameters.minimum_seed_landmarks = 6; + parameters.minimum_pnp_correspondences = 6; + const BatchFixture base = + make_batch_fixture(2, 12, false, false, false, false); + const Lardon3DSparseIncrementalInput valid = { + 601, 602, base.images.data(), base.images.size(), + base.observations.data(), base.observations.size()}; + + auto expect_invalid = [&](const Lardon3DSparseIncrementalInput &input) { + Lardon3DSparseIncrementalResult output = {}; + const bool accepted = lardon3d_sparse_incremental_run( + &input, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT && + output.camera_count == 0 && output.landmark_count == 0; + lardon3d_sparse_incremental_result_destroy(&output); + return accepted; + }; + + Lardon3DSparseIncrementalResult output = {}; + CHECK(lardon3d_sparse_incremental_run(nullptr, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + CHECK(lardon3d_sparse_incremental_run(&valid, nullptr, &output) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + CHECK(lardon3d_sparse_incremental_run(&valid, ¶meters, nullptr) == + LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT); + + Lardon3DSparseIncrementalInput invalid = valid; + invalid.images = nullptr; + CHECK(expect_invalid(invalid)); + invalid = valid; + invalid.observations = nullptr; + CHECK(expect_invalid(invalid)); + invalid = valid; + invalid.observation_count = 0; + CHECK(expect_invalid(invalid)); + invalid = valid; + invalid.track_set_id = 0; + CHECK(expect_invalid(invalid)); + invalid = valid; + invalid.calibration_scope_id = 0; + CHECK(expect_invalid(invalid)); + + for (int kind = 0; kind < 5; ++kind) { + std::vector images = base.images; + if (kind == 0) images[0].calibration.fx = 0.0; + if (kind == 1) images[0].calibration.fx = NAN; + if (kind == 2) images[0].calibration.k1 = INFINITY; + if (kind == 3) images[0].calibration.cx = -1.0; + if (kind == 4) images[0].image_id = 0; + invalid = valid; + invalid.images = images.data(); + CHECK(expect_invalid(invalid)); + } + + for (int kind = 0; kind < 6; ++kind) { + std::vector observations = + base.observations; + if (kind == 0) observations[0].feature_set_id = 0; + if (kind == 1) + observations[0].feature_index = observations[0].feature_count; + if (kind == 2) observations[0].x = NAN; + if (kind == 3) observations[0].y = INFINITY; + if (kind == 4) observations[0].image_id = 999; + if (kind == 5) observations[0].track_id = 0; + invalid = valid; + invalid.observations = observations.data(); + CHECK(expect_invalid(invalid)); + } + + std::vector singleton = { + base.observations.front()}; + invalid = valid; + invalid.observations = singleton.data(); + invalid.observation_count = singleton.size(); + CHECK(expect_invalid(invalid)); + + /* Repeated failure, safe destruction, and a valid call after failure. */ + invalid = valid; + invalid.calibration_scope_id = 0; + CHECK(expect_invalid(invalid)); + CHECK(expect_invalid(invalid)); + CHECK(lardon3d_sparse_incremental_run(&valid, ¶meters, &output) == + LARDON3D_SPARSE_INCREMENTAL_COMPLETE); + lardon3d_sparse_incremental_result_destroy(&output); + lardon3d_sparse_incremental_result_destroy(&output); + lardon3d_sparse_incremental_result_destroy(nullptr); + return true; +} + +static void signature_mix(uint64_t *hash, const void *data, size_t size) { + const auto *bytes = static_cast(data); + for (size_t index = 0; index < size; ++index) { + *hash ^= bytes[index]; + *hash *= UINT64_C(1099511628211); + } +} + +static bool scientific_signature(uint64_t *signature) { + const BatchFixture fixture = + make_batch_fixture(5, 12, false, false, false, false); + Lardon3DSparseIncrementalParameters parameters; + if (!lardon3d_sparse_incremental_parameters_default(¶meters)) return false; + parameters.minimum_seed_tracks = 6; + parameters.minimum_seed_landmarks = 6; + parameters.minimum_pnp_correspondences = 6; + const Lardon3DSparseIncrementalInput input = { + 701, 702, fixture.images.data(), fixture.images.size(), + fixture.observations.data(), fixture.observations.size()}; + Lardon3DSparseIncrementalResult result = {}; + if (lardon3d_sparse_incremental_run(&input, ¶meters, &result) != + LARDON3D_SPARSE_INCREMENTAL_COMPLETE) + return false; + + uint64_t hash = UINT64_C(1469598103934665603); +#define MIX(value) signature_mix(&hash, &(value), sizeof(value)) + MIX(result.status); + MIX(result.track_set_id); + MIX(result.calibration_scope_id); + for (size_t index = 0; index < result.component_count; ++index) { + MIX(result.components[index].component_key); + MIX(result.components[index].image_count); + MIX(result.components[index].registered_image_count); + MIX(result.components[index].landmark_count); + } + for (size_t index = 0; index < result.camera_count; ++index) { + MIX(result.cameras[index].image_id); + MIX(result.cameras[index].component_key); + signature_mix(&hash, result.cameras[index].pose_cw.rotation_cw, + sizeof(result.cameras[index].pose_cw.rotation_cw)); + signature_mix(&hash, result.cameras[index].pose_cw.translation_cw, + sizeof(result.cameras[index].pose_cw.translation_cw)); + } + for (size_t index = 0; index < result.landmark_count; ++index) { + MIX(result.landmarks[index].landmark_id); + MIX(result.landmarks[index].track_id); + MIX(result.landmarks[index].component_key); + MIX(result.landmarks[index].point.x); + MIX(result.landmarks[index].point.y); + MIX(result.landmarks[index].point.z); + MIX(result.landmarks[index].reprojection_rmse_px); + MIX(result.landmarks[index].reprojection_median_px); + MIX(result.landmarks[index].observation_count); + } + for (size_t index = 0; index < result.observation_count; ++index) { + MIX(result.observations[index].landmark_id); + MIX(result.observations[index].track_id); + MIX(result.observations[index].image_id); + MIX(result.observations[index].feature_set_id); + MIX(result.observations[index].feature_index); + MIX(result.observations[index].position_in_track); + } + MIX(result.seed_candidates_available); + MIX(result.seed_candidates_considered); + MIX(result.seed_image_a); + MIX(result.seed_image_b); + MIX(result.registration_rounds); + MIX(result.registration_attempts); + MIX(result.registration_successes); + MIX(result.triangulation_attempts); + MIX(result.landmark_update_attempts); + MIX(result.landmark_update_successes); + MIX(result.point_refinement_attempts); + MIX(result.point_refinement_successes); +#undef MIX + lardon3d_sparse_incremental_result_destroy(&result); + *signature = hash; + return true; +} + +static bool run_case_35(const char *executable) { + /* CASE 35: twenty fresh executable images emit one scientific signature. */ + uint64_t expected = 0; + for (size_t run = 0; run < 20; ++run) { + int descriptors[2]; + CHECK(pipe(descriptors) == 0); + const pid_t child = fork(); + CHECK(child >= 0); + if (child == 0) { + close(descriptors[0]); + CHECK(dup2(descriptors[1], STDOUT_FILENO) >= 0); + close(descriptors[1]); + execl(executable, executable, "--scientific-signature", nullptr); + _exit(127); + } + close(descriptors[1]); + uint64_t actual = 0; + size_t received = 0; + while (received < sizeof(actual)) { + const ssize_t count = read( + descriptors[0], reinterpret_cast(&actual) + received, + sizeof(actual) - received); + if (count <= 0) break; + received += static_cast(count); + } + close(descriptors[0]); + int status = 0; + CHECK(waitpid(child, &status, 0) == child); + CHECK(WIFEXITED(status) && WEXITSTATUS(status) == 0); + CHECK(received == sizeof(actual)); + if (run == 0) + expected = actual; + else + CHECK(actual == expected); + } + return true; +} + +int main(int argc, char **argv) { + if (argc == 2 && std::strcmp(argv[1], "--scientific-signature") == 0) { + uint64_t signature = 0; + if (!scientific_signature(&signature)) return EXIT_FAILURE; + return write(STDOUT_FILENO, &signature, sizeof(signature)) == + static_cast(sizeof(signature)) + ? EXIT_SUCCESS + : EXIT_FAILURE; + } + return run_test() && run_batch_cases() && run_boundary_cases() && + run_failure_cases() && run_case_35(argv[0]) + ? EXIT_SUCCESS + : EXIT_FAILURE; +}