From fa58c8a225d94cc44b80faccf18dccaeb91502eb Mon Sep 17 00:00:00 2001 From: fy59 Date: Tue, 25 Aug 2026 07:58:46 +0200 Subject: [PATCH] secure commit --- README.md | 11 +- docs/architecture/feature_store.md | 3 +- docs/architecture/overview.md | 9 + docs/architecture/reconstruction_pipeline.md | 11 +- docs/architecture/sparse_sfm.md | 40 +- docs/roadmap/roadmap.md | 1 + .../lardon3d/sparse_sfm_bundle_adjustment.h | 26 + meson.build | 9 +- src/sparse_sfm_bundle_adjustment.cpp | 942 +++++++++++++++++- tests/test_sparse_sfm_bundle_adjustment.cpp | 837 +++++++++++++++- 10 files changed, 1854 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index ed58361..1781804 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,8 @@ persistante, enrichissable et versionnable. - **Task** : moteur de tâches avec pause/reprise, annulation et séquences - **Task Checkpoint v1** : snapshot durable, fichier atomique et reprise sûre - **Project Database v16** : résultats géométriques, Track Model v1 et persistance Sparse SfM +- **Sparse SfM Gates C/D/E** : géométrie calibrée, noyau incrémental et Bundle + Adjustment final par composante, tous PASS / FROZEN - **Geometric Verification Model v1** : identité, masque d'inliers et modèle 3×3 persistants - **Geometric Verifier v1** : Fundamental USAC/MAGSAC, reprise et lots resource-aware - **Task Kind Registry** : identité métier durable et reconstruction runtime explicite @@ -165,10 +167,11 @@ Matcher v1, Geometric Verification Model v1 et Geometric Verifier Fundamental v1 sont implémentés. Le runtime Feature + Matcher + Verifier emploie des tâches durables, de petits lots, le Resource Governor interactif et un hot path Vulkan ORB exact avec fallback CPU. La feasibility Vulkan SIFT/RootSIFT a été rejetée ; ces deux matchers -restent sur OpenCV L2. Track Model/Builder, les primitives géométriques Gate C -et le noyau Sparse SfM incrémental Gate D sont implémentés ; BA, l'orchestration -Sparse SfM, le DAG, le viewer et les étapes denses restent des tickets séparés -planifiés. Le Resource Governor ne +restent sur OpenCV L2. Track Model/Builder, les primitives géométriques Gate C, +le noyau Sparse SfM incrémental Gate D et le Bundle Adjustment final Gate E sont +implémentés et validés. L'orchestration Sparse SfM Gate F, l'intégration +Governor Gate G, le DAG, le viewer et les étapes denses restent des tickets +séparés planifiés. Le Resource Governor ne constitue pas un Resource System générique : voir la décision d’architecture. ## Licence diff --git a/docs/architecture/feature_store.md b/docs/architecture/feature_store.md index ec40fc7..9693c73 100644 --- a/docs/architecture/feature_store.md +++ b/docs/architecture/feature_store.md @@ -121,7 +121,8 @@ les Feature Sets persistés. **IMPLEMENTED** — vérification géométrique et tracks. **Sparse SfM Gate A PASS**, the Sparse SfM v16 persistence model is **FROZEN** after Gate B; Gate C geometry and the synchronous in-memory Gate D incremental core are -**IMPLEMENTED / PASS**. BA and project/task orchestration remain **PLANNED**. +**IMPLEMENTED / PASS**. Final per-component Gate E BA is **PASS / FROZEN**; +Gate F project/task orchestration remains **PLANNED**. ## Extension v2 multi-descriptor diff --git a/docs/architecture/overview.md b/docs/architecture/overview.md index 2954e82..2421039 100644 --- a/docs/architecture/overview.md +++ b/docs/architecture/overview.md @@ -128,6 +128,15 @@ Déterministe, idempotent, borné. **Statut :** IMPLEMENTED +### Sparse SfM +Les primitives géométriques calibrées Gate C, le noyau incrémental synchrone +Gate D et le Bundle Adjustment final par composante Gate E sont implémentés. +Gate E traite en mémoire une copie du résultat Gate D immutable, sur CPU avec +un thread Ceres, et publie atomiquement chaque candidate acceptée. Il n'intègre +ni persistance, ni Task Runtime, ni Resource Governor. + +**Statut :** GATES C/D/E — PASS / FROZEN + ## Résultats et publication live Les traitements fonctionnent par séquences adaptatives : lire un lot borné, diff --git a/docs/architecture/reconstruction_pipeline.md b/docs/architecture/reconstruction_pipeline.md index 862fce4..07da7a9 100644 --- a/docs/architecture/reconstruction_pipeline.md +++ b/docs/architecture/reconstruction_pipeline.md @@ -126,7 +126,8 @@ USAC/MAGSAC avec configuration, seed et fingerprint déterministes. implémenté dans Project DB v15 (`track_sets`, `tracks`, `track_observations` et le payload de tâche). Les primitives de géométrie calibrée Gate C sont implémentées et le noyau Sparse SfM incrémental Gate D est IMPLEMENTED / PASS ; -l'orchestration projet et tâche reste PLANNED. +le Bundle Adjustment final Gate E est PASS / FROZEN ; l'orchestration projet +et tâche Gate F reste PLANNED. Le modèle de persistance Sparse SfM v16 est gelé après Gate B. **Sparse SfM Gate A : PASS.** Le contrat géométrique, la stratégie @@ -134,7 +135,8 @@ incremental, la triangulation candidate, le gauge, les conventions de pose, les limites BA et l'enveloppe matérielle sont documentés dans `architecture/sparse_sfm.md`. Ses primitives pures calibrées Gate C sont **IMPLEMENTED / PASS** et son noyau incrémental synchrone en mémoire Gate D est -**IMPLEMENTED / PASS**. BA et orchestration restent **PLANNED**, tandis que sa +**IMPLEMENTED / PASS**. Le Bundle Adjustment final par composante Gate E est +**PASS / FROZEN**. L'orchestration Gate F reste **PLANNED**, tandis que la persistance v16 et ses lecteurs bornés restent ceux de B2. --- @@ -255,8 +257,9 @@ Image Catalog (B) ──► Feature Store (C) Import, Image Catalog, Feature Extraction, Feature Store, Visual Index, 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**. +incremental Sparse SfM Gate D core is **IMPLEMENTED / PASS**, and final +per-component Gate E BA is **PASS / FROZEN**. Gate F orchestration, +Gate G Governor integration, 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 a40b4f4..7dd0b89 100644 --- a/docs/architecture/sparse_sfm.md +++ b/docs/architecture/sparse_sfm.md @@ -194,6 +194,12 @@ as a whole, so Track identity and observation ownership remain simple. ## Gate E v1 — Final Bundle Adjustment decision +**GATE E — PASS / FROZEN.** The synchronous CPU-only final per-component +Bundle Adjustment implementation, E01--E35 matrix, normal suite, targeted +ASan/UBSan with LeakSanitizer, full sequential ASan/UBSan suite and at least +20 fresh-process E27 comparisons are validated. Gate F project orchestration +and Gate G resource integration remain later gates. + **DECISION: Gate E v1 is a synchronous, independent final per-component Bundle Adjustment applied as post-processing to a copy of the immutable final Gate D result.** It consumes two caller-owned immutable views that must remain coherent @@ -426,7 +432,7 @@ per component and requires no in-place rollback. ### Result and diagnostics -The future solver-independent Gate E result has these conceptual states: +The solver-independent Gate E result has these conceptual states: - `COMPLETE`: at least one component is eligible and every eligible component is optimized and accepted; @@ -439,7 +445,7 @@ The future solver-independent Gate E result has these conceptual states: result states. In particular, `FAILED` is not an invalid-argument, out-of-memory or internal execution error. -The future synchronous execution function returns the separate, +The synchronous execution function returns the separate, solver-independent `Lardon3DSparseBundleAdjustmentExecutionStatus`: ```text @@ -549,6 +555,10 @@ pre/post geometric error and robust cost. Fixtures intended to improve define their own scientifically measurable improvement; no universal pose or landmark threshold is invented. +The complete E01--E35 matrix is implemented and validated. E27 passed at least +20 fresh processes using exact structural comparison, the frozen binary64 +tolerance and geometric rotation comparison. + Local BA after registration is deferred. Gate D exposes no intermediate scientific seam or complete registration history, and an interleaved BA could change its subsequent growth. Introducing that policy requires a future @@ -561,13 +571,10 @@ fingerprint, defines no persistent identity, and publishes nothing. Gate F retains project/task orchestration and persistence; Gate G retains Resource Governor integration and final resource validation. -Ceres availability on a host must be distinguished from Lardon3D dependency -declaration. Gate E selects the Ceres Solver 2.2.x API scientifically, but -Lardon3D currently declares no production Ceres dependency in Meson. A future -dependency slice must verify the used API, licensing, CPU-only construction and -a build without required SuiteSparse or CUDA. Package discovery may use CMake; -a pkg-config miss alone does not prove host unavailability, and an installed -host package is not a declared Lardon3D dependency. +Ceres availability on a host remains distinct from Lardon3D dependency +declaration. Gate E declares Ceres Solver `>=2.2.0,<2.3.0` through Meson CMake +discovery and uses its 2.2.x CPU API. CUDA is not required and Lardon3D has no +functional direct SuiteSparse dependency. ## Determinism and scientific identity @@ -639,8 +646,8 @@ zram device, and zero current memory/IO PSI average. The host is the Ryzen 7 The project already links OpenCV 5.0.0. Host-installed libraries and their pkg-config or CMake discovery metadata are capabilities, not Lardon3D -production dependencies. Lardon3D currently declares no Ceres dependency in -Meson. No package, system setting, swap device or GPU mode was changed. +production dependencies. Gate E now declares Ceres 2.2.x through Meson CMake +discovery. No system setting, swap device or GPU mode was changed. Gate A probes use deterministic synthetic camera arcs, controlled noise and degenerate planar/pure-rotation cases. Every RSS probe is a separate normal @@ -697,7 +704,7 @@ Triangulation candidates: |---|---|---|---| | Dense normal equations | Prohibited for serious `C×P` problems | No | Rejected | | OpenCV generic optimization | Not a sparse BA contract | Present, wrong abstraction | Rejected | -| Ceres 2.2.x iterative Schur | Block-sparse | Scientific selection; not in Meson | **Selected Gate E v1** | +| Ceres 2.2.x iterative Schur | Block-sparse | Declared through Meson CMake discovery | **Implemented Gate E v1** | ### Synthetic geometry probe @@ -722,9 +729,8 @@ The project already links OpenCV 5.0.0. Host probes found Eigen 5.0.1, BLAS 3.12.0, LAPACK 3.12.0 and TBB 2023.1 as host capabilities or transitive facilities rather than current Lardon3D production dependencies. Ceres may use CMake discovery, so pkg-config alone does not establish host availability. -Ceres 2.2.x is the selected Gate E scientific API, but Lardon3D declares no -production Ceres dependency yet. No new dependency is added by this contract -slice. The measured machine has 16 logical CPUs, +Ceres 2.2.x is the implemented Gate E scientific API and is declared through +Meson CMake discovery. The measured machine has 16 logical CPUs, `MemTotal=15597716 KiB`, `MemAvailable=8245288 KiB` at preflight, an 8 GiB swapfile, 6 GiB zram and zero memory/IO PSI averages at the probe start. Gate E uses one solver thread; future Gate G resource admission cannot change that @@ -860,8 +866,8 @@ globally aligned by Gate D. 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. +separate PASS / FROZEN Gate E post-processing stage; project/task orchestration +remains Gate F and resource/freeze integration remains Gate G. ### Canonical Gate D functional matrix diff --git a/docs/roadmap/roadmap.md b/docs/roadmap/roadmap.md index 39fec0e..56a52a3 100644 --- a/docs/roadmap/roadmap.md +++ b/docs/roadmap/roadmap.md @@ -52,6 +52,7 @@ Lardon3D suit une feuille de route ordonnée qui privilégie la stabilité et la - ✅ Track Model / Track Builder v1 - ✅ Sparse SfM : primitives géométriques Gate C et noyau incrémental Gate D implémentés +- ✅ Sparse SfM Gate E : Bundle Adjustment final par composante PASS / FROZEN ### Phase 5 : Reconstruction (PLANNED) - 📋 Orchestration de reconstruction incrémentale diff --git a/include/lardon3d/sparse_sfm_bundle_adjustment.h b/include/lardon3d/sparse_sfm_bundle_adjustment.h index 4f332c6..212216b 100644 --- a/include/lardon3d/sparse_sfm_bundle_adjustment.h +++ b/include/lardon3d/sparse_sfm_bundle_adjustment.h @@ -20,6 +20,16 @@ typedef enum { LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED } Lardon3DSparseBundleAdjustmentStatus; +/* Execution errors are separate from the scientific result status. An OK + * execution may produce a scientific FAILED result when no eligible component + * is accepted. */ +typedef enum { + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK = 0, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR +} Lardon3DSparseBundleAdjustmentExecutionStatus; + typedef enum { LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_NONE = 0, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X, @@ -39,6 +49,7 @@ typedef enum { LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INPUT, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE, @@ -106,6 +117,21 @@ typedef struct { Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostics; } Lardon3DSparseBundleAdjustmentResult; +/* Runs synchronous final per-component Bundle Adjustment. input remains + * caller-owned and immutable. result must be zero-initialized or previously + * destroyed. On EXECUTION_OK, result owns all non-null arrays and its + * scientific status may be COMPLETE, PARTIAL or FAILED. On every other + * execution status, result is left in its canonical zero state. */ +Lardon3DSparseBundleAdjustmentExecutionStatus +lardon3d_sparse_bundle_adjustment_run( + const Lardon3DSparseBundleAdjustmentInput *input, + Lardon3DSparseBundleAdjustmentResult *result); + +/* Releases every result-owned array and restores the canonical zero state. + * The operation is NULL-safe and repeat-safe; it never releases input data. */ +void lardon3d_sparse_bundle_adjustment_result_destroy( + Lardon3DSparseBundleAdjustmentResult *result); + #ifdef __cplusplus } #endif diff --git a/meson.build b/meson.build index d04019a..e895a55 100644 --- a/meson.build +++ b/meson.build @@ -24,6 +24,12 @@ ncursesw = dependency('ncursesw', required: true) threads = dependency('threads') sqlite3 = dependency('sqlite3', required: true) openssl = dependency('openssl', required: true) +ceres = dependency( + 'Ceres', + method: 'cmake', + version: ['>=2.2.0', '<2.3.0'], + required: true, +) opencv = dependency( 'opencv5', required: true, @@ -164,7 +170,7 @@ executable( ] + matcher_backend_sources, include_directories: include_directories('include'), c_args: ['-DLARDON3D_TRACK_BUILDER_TASK_AVAILABLE'], - dependencies: [ncursesw, threads, sqlite3, openssl, opencv, opencv_geometry] + dependencies: [ncursesw, threads, sqlite3, openssl, opencv, opencv_geometry, ceres] + matcher_backend_dependencies, ) @@ -555,6 +561,7 @@ sparse_sfm_bundle_adjustment_test = executable( ], cpp_args: ['-DLARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TESTING'], include_directories: include_directories('include'), + dependencies: [ceres], ) test('sparse-sfm-bundle-adjustment', sparse_sfm_bundle_adjustment_test, diff --git a/src/sparse_sfm_bundle_adjustment.cpp b/src/sparse_sfm_bundle_adjustment.cpp index 0708591..a24f011 100644 --- a/src/sparse_sfm_bundle_adjustment.cpp +++ b/src/sparse_sfm_bundle_adjustment.cpp @@ -1,8 +1,16 @@ #include +#include +#include + #include #include +#include +#include +#include +#include #include +#include #include #include @@ -14,7 +22,12 @@ constexpr size_t maximum_observations = 1000000; constexpr double depth_epsilon = 1e-9; constexpr double gauge_epsilon = 1e-9; -enum class PreparationStatus { prepared, invalid_argument, out_of_memory }; +enum class PreparationStatus { + prepared, + invalid_argument, + out_of_memory, + internal_error +}; enum class PrivateTermination { converged, no_convergence, failure }; struct ResolvedObservation { @@ -43,6 +56,74 @@ struct ObservationIndex { size_t index; }; +struct ComponentView { + std::vector cameras; + std::vector landmarks; + std::vector observations; +}; + +struct SolverCamera { + size_t source_index; + size_t image_index; + double quaternion[4]; + double center[3]; + double initial_center[3]; +}; + +struct SolverLandmark { + size_t source_index; + double point[3]; +}; + +struct SolverObservation { + size_t camera_index; + size_t landmark_index; + size_t resolved_index; +}; + +struct UnderconstraintResult { + bool valid; + uint32_t mask; +}; +enum class UnderconstraintAction { proceed, reject, internal_error }; + +struct CandidateDecision { + bool accepted; + bool has_metrics; + Lardon3DSparseBundleAdjustmentRejectionReason rejection_reason; +}; + +enum class ParameterKind : int { landmark = 0, camera_quaternion = 1, camera_center = 2 }; +struct ParameterRecord { + ParameterKind kind; + uint64_t identity; + int group; + bool constant; + int subset_axis; +}; + +struct DisjointSet { + std::vector parent; + + explicit DisjointSet(size_t count) : parent(count) { + std::iota(parent.begin(), parent.end(), 0); + } + + size_t find(size_t value) { + while (parent[value] != value) { + parent[value] = parent[parent[value]]; + value = parent[value]; + } + return value; + } + + void join(size_t a, size_t b) { + a = find(a); + b = find(b); + if (a != b) parent[std::max(a, b)] = std::min(a, b); + } +}; + bool finite_calibration(const Lardon3DSparseGeometryCalibration &value) { return value.width > 0 && value.height > 0 && std::isfinite(value.fx) && std::isfinite(value.fy) && value.fx > 0.0 && value.fy > 0.0 && @@ -94,6 +175,114 @@ bool camera_center(const Lardon3DSparseGeometryPose &pose, double center[3]) { return true; } +bool rotation_to_quaternion(const double rotation[9], double quaternion[4]) { + const double trace = rotation[0] + rotation[4] + rotation[8]; + if (trace > 0.0) { + const double scale = 2.0 * std::sqrt(trace + 1.0); + if (!std::isfinite(scale) || scale == 0.0) return false; + quaternion[0] = 0.25 * scale; + quaternion[1] = (rotation[7] - rotation[5]) / scale; + quaternion[2] = (rotation[2] - rotation[6]) / scale; + quaternion[3] = (rotation[3] - rotation[1]) / scale; + } else if (rotation[0] > rotation[4] && rotation[0] > rotation[8]) { + const double scale = 2.0 * std::sqrt(1.0 + rotation[0] - rotation[4] - rotation[8]); + if (!std::isfinite(scale) || scale == 0.0) return false; + quaternion[0] = (rotation[7] - rotation[5]) / scale; + quaternion[1] = 0.25 * scale; + quaternion[2] = (rotation[1] + rotation[3]) / scale; + quaternion[3] = (rotation[2] + rotation[6]) / scale; + } else if (rotation[4] > rotation[8]) { + const double scale = 2.0 * std::sqrt(1.0 + rotation[4] - rotation[0] - rotation[8]); + if (!std::isfinite(scale) || scale == 0.0) return false; + quaternion[0] = (rotation[2] - rotation[6]) / scale; + quaternion[1] = (rotation[1] + rotation[3]) / scale; + quaternion[2] = 0.25 * scale; + quaternion[3] = (rotation[5] + rotation[7]) / scale; + } else { + const double scale = 2.0 * std::sqrt(1.0 + rotation[8] - rotation[0] - rotation[4]); + if (!std::isfinite(scale) || scale == 0.0) return false; + quaternion[0] = (rotation[3] - rotation[1]) / scale; + quaternion[1] = (rotation[2] + rotation[6]) / scale; + quaternion[2] = (rotation[5] + rotation[7]) / scale; + quaternion[3] = 0.25 * scale; + } + double norm = 0.0; + for (size_t index = 0; index < 4; ++index) + norm += quaternion[index] * quaternion[index]; + norm = std::sqrt(norm); + if (!std::isfinite(norm) || norm == 0.0) return false; + for (size_t index = 0; index < 4; ++index) quaternion[index] /= norm; + if (quaternion[0] < 0.0 || + (quaternion[0] == 0.0 && + (quaternion[1] < 0.0 || + (quaternion[1] == 0.0 && + (quaternion[2] < 0.0 || + (quaternion[2] == 0.0 && quaternion[3] < 0.0)))))) + for (size_t index = 0; index < 4; ++index) quaternion[index] = -quaternion[index]; + return true; +} + +bool quaternion_to_pose(const double quaternion_source[4], const double center[3], + Lardon3DSparseGeometryPose *pose) { + if (!pose) return false; + double quaternion[4] = {quaternion_source[0], quaternion_source[1], + quaternion_source[2], quaternion_source[3]}; + double norm = 0.0; + for (double value : quaternion) norm += value * value; + norm = std::sqrt(norm); + if (!std::isfinite(norm) || norm == 0.0) return false; + for (double &value : quaternion) value /= norm; + if (quaternion[0] < 0.0) for (double &value : quaternion) value = -value; + const double w = quaternion[0]; + const double x = quaternion[1]; + const double y = quaternion[2]; + const double z = quaternion[3]; + double *r = pose->rotation_cw; + r[0] = 1.0 - 2.0 * (y * y + z * z); + r[1] = 2.0 * (x * y - z * w); + r[2] = 2.0 * (x * z + y * w); + r[3] = 2.0 * (x * y + z * w); + r[4] = 1.0 - 2.0 * (x * x + z * z); + r[5] = 2.0 * (y * z - x * w); + r[6] = 2.0 * (x * z - y * w); + r[7] = 2.0 * (y * z + x * w); + r[8] = 1.0 - 2.0 * (x * x + y * y); + for (size_t row = 0; row < 3; ++row) { + pose->translation_cw[row] = + -(r[row * 3] * center[0] + r[row * 3 + 1] * center[1] + + r[row * 3 + 2] * center[2]); + } + return finite_pose(*pose) && valid_rotation(*pose); +} + +struct ReprojectionResidual { + Lardon3DSparseGeometryCalibration calibration; + double observed_x; + double observed_y; + + template + bool operator()(const T *quaternion, const T *center, const T *point, + T *residual) const { + const T relative[3] = {point[0] - center[0], point[1] - center[1], + point[2] - center[2]}; + T camera[3]; + ceres::QuaternionRotatePoint(quaternion, relative, camera); + if (camera[2] <= T(depth_epsilon)) return false; + const T xn = camera[0] / camera[2]; + const T yn = camera[1] / camera[2]; + const T r2 = xn * xn + yn * yn; + const T radial = T(1.0) + T(calibration.k1) * r2 + + T(calibration.k2) * r2 * r2; + const T xd = xn * radial + T(2.0 * calibration.p1) * xn * yn + + T(calibration.p2) * (r2 + T(2.0) * xn * xn); + const T yd = yn * radial + T(calibration.p1) * (r2 + T(2.0) * yn * yn) + + T(2.0 * calibration.p2) * xn * yn; + residual[0] = T(calibration.fx) * xd + T(calibration.cx - observed_x); + residual[1] = T(calibration.fy) * yd + T(calibration.cy - observed_y); + return true; + } +}; + bool select_anchors(const std::vector &cameras, uint64_t component_key, Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic) { @@ -198,6 +387,160 @@ bool cost_acceptable(double initial_cost, double final_cost) { return std::isfinite(tolerance) && final_cost <= initial_cost + tolerance; } +bool build_component_views(const Preparation &preparation, + std::vector *views) { + if (!views) return false; + views->clear(); + views->resize(preparation.components.size()); + for (size_t index = 0; index < preparation.components.size(); ++index) { + (*views)[index].cameras.reserve( + static_cast(preparation.components[index].registered_image_count)); + (*views)[index].landmarks.reserve( + static_cast(preparation.components[index].landmark_count)); + (*views)[index].observations.reserve( + static_cast(preparation.diagnostics[index].observation_count)); + } + for (size_t index = 0; index < preparation.cameras.size(); ++index) { + auto component = std::lower_bound( + preparation.components.begin(), preparation.components.end(), + preparation.cameras[index].component_key, + [](const auto &value, uint64_t key) { return value.component_key < key; }); + if (component == preparation.components.end() || + component->component_key != preparation.cameras[index].component_key) + return false; + (*views)[static_cast(component - preparation.components.begin())] + .cameras.push_back(index); + } + for (size_t index = 0; index < preparation.landmarks.size(); ++index) { + auto component = std::lower_bound( + preparation.components.begin(), preparation.components.end(), + preparation.landmarks[index].component_key, + [](const auto &value, uint64_t key) { return value.component_key < key; }); + if (component == preparation.components.end() || + component->component_key != preparation.landmarks[index].component_key) + return false; + (*views)[static_cast(component - preparation.components.begin())] + .landmarks.push_back(index); + } + for (size_t index = 0; index < preparation.observations.size(); ++index) { + auto landmark = std::lower_bound( + preparation.landmarks.begin(), preparation.landmarks.end(), + preparation.observations[index].track_id, + [](const auto &value, uint64_t key) { return value.track_id < key; }); + if (landmark == preparation.landmarks.end() || + landmark->track_id != preparation.observations[index].track_id) + return false; + auto component = std::lower_bound( + preparation.components.begin(), preparation.components.end(), + landmark->component_key, + [](const auto &value, uint64_t key) { return value.component_key < key; }); + if (component == preparation.components.end() || + component->component_key != landmark->component_key) + return false; + (*views)[static_cast(component - preparation.components.begin())] + .observations.push_back(index); + } + return true; +} + +uint32_t structural_underconstraint_mask( + size_t camera_count, size_t landmark_count, size_t observation_count, + size_t pose_anchor, std::vector> edges) { + constexpr uint32_t uc1 = 1U << 0; + constexpr uint32_t uc2 = 1U << 1; + constexpr uint32_t uc3 = 1U << 2; + constexpr uint32_t uc4 = 1U << 3; + uint32_t mask = 0; + const uint64_t cameras = camera_count; + const uint64_t landmarks = landmark_count; + const uint64_t observations = observation_count; + if (cameras > (UINT64_MAX - 7) / 6 || landmarks > UINT64_MAX / 3 || + observations > UINT64_MAX / 2) { + mask |= uc1; + } else { + const uint64_t camera_dof = 6 * cameras; + const uint64_t landmark_dof = 3 * landmarks; + if (camera_dof > UINT64_MAX - landmark_dof || + camera_dof + landmark_dof < 7 || + 2 * observations < camera_dof + landmark_dof - 7) + mask |= uc1; + } + std::sort(edges.begin(), edges.end()); + edges.erase(std::unique(edges.begin(), edges.end()), edges.end()); + std::vector camera_support(camera_count, 0); + std::vector landmark_support(landmark_count, 0); + DisjointSet graph(camera_count + landmark_count); + for (const auto &[camera, landmark] : edges) { + if (camera >= camera_count || landmark >= landmark_count) + return uc1 | uc2 | uc3 | uc4; + ++camera_support[camera]; + ++landmark_support[landmark]; + graph.join(camera, camera_count + landmark); + } + for (size_t support : landmark_support) + if (support < 2) mask |= uc2; + if (pose_anchor >= camera_count) return uc1 | uc2 | uc3 | uc4; + for (size_t index = 0; index < camera_support.size(); ++index) + if (index != pose_anchor && camera_support[index] < 3) mask |= uc3; + const size_t root = graph.find(pose_anchor); + for (size_t index = 0; index < graph.parent.size(); ++index) + if (graph.find(index) != root) mask |= uc4; + return mask; +} + +UnderconstraintResult underconstraint_result( + const Preparation &preparation, const ComponentView &view, + const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic) { + std::vector> edges; + edges.reserve(view.observations.size()); + for (size_t observation_index : view.observations) { + const auto &observation = preparation.observations[observation_index]; + auto camera = std::lower_bound( + view.cameras.begin(), view.cameras.end(), observation.image_id, + [&](size_t index, uint64_t key) { + return preparation.cameras[index].image_id < key; + }); + auto landmark = std::lower_bound( + view.landmarks.begin(), view.landmarks.end(), observation.track_id, + [&](size_t index, uint64_t key) { + return preparation.landmarks[index].track_id < key; + }); + if (camera == view.cameras.end() || landmark == view.landmarks.end() || + preparation.cameras[*camera].image_id != observation.image_id || + preparation.landmarks[*landmark].track_id != observation.track_id) + return {false, 0}; + edges.emplace_back(static_cast(camera - view.cameras.begin()), + static_cast(landmark - view.landmarks.begin())); + } + size_t pose_anchor = view.cameras.size(); + for (size_t index = 0; index < view.cameras.size(); ++index) + if (preparation.cameras[view.cameras[index]].image_id == + diagnostic.pose_anchor_image_id) + pose_anchor = index; + if (pose_anchor == view.cameras.size()) return {false, 0}; + return {true, structural_underconstraint_mask( + view.cameras.size(), view.landmarks.size(), + view.observations.size(), pose_anchor, std::move(edges))}; +} + +UnderconstraintAction classify_underconstraint(const UnderconstraintResult &result) { + if (!result.valid) return UnderconstraintAction::internal_error; + return result.mask == 0 ? UnderconstraintAction::proceed + : UnderconstraintAction::reject; +} + +bool public_counts_valid(size_t image_count, size_t camera_count, + size_t landmark_count, size_t result_observation_count, + size_t input_observation_count) { + if (image_count > maximum_images || camera_count > maximum_images || + landmark_count > maximum_landmarks || + result_observation_count > maximum_observations || + input_observation_count > maximum_observations) + return false; + return result_observation_count <= SIZE_MAX / 2 && + result_observation_count <= SIZE_MAX / sizeof(double) / 2; +} + template void copy_view(std::vector *destination, const T *source, size_t count) { destination->clear(); @@ -211,10 +554,9 @@ PreparationStatus prepare(const Lardon3DSparseBundleAdjustmentInput &input, const auto &result = *input.incremental_result; if (result.status < LARDON3D_SPARSE_INCREMENTAL_COMPLETE || result.status > LARDON3D_SPARSE_INCREMENTAL_FAILED || - input.image_count > maximum_images || result.camera_count > maximum_images || - result.landmark_count > maximum_landmarks || - result.observation_count > maximum_observations || - input.observation_count > maximum_observations || + !public_counts_valid(input.image_count, result.camera_count, + result.landmark_count, result.observation_count, + input.observation_count) || (input.image_count && !input.images) || (input.observation_count && !input.observations) || (result.component_count && !result.components) || @@ -403,12 +745,411 @@ PreparationStatus prepare(const Lardon3DSparseBundleAdjustmentInput &input, } catch (const std::bad_alloc &) { return PreparationStatus::out_of_memory; } catch (...) { - return PreparationStatus::invalid_argument; + return PreparationStatus::internal_error; } } +bool component_metrics( + const Preparation &preparation, const ComponentView &view, + const std::vector &cameras, + const std::vector &landmarks, + double *rmse, double *cost) { + std::vector residuals; + if (view.observations.size() > SIZE_MAX / 2) return false; + residuals.resize(view.observations.size() * 2); + for (size_t local = 0; local < view.observations.size(); ++local) { + const size_t observation_index = view.observations[local]; + const auto &observation = preparation.observations[observation_index]; + auto camera = std::lower_bound( + cameras.begin(), cameras.end(), observation.image_id, + [](const auto &value, uint64_t key) { return value.image_id < key; }); + auto landmark = std::lower_bound( + landmarks.begin(), landmarks.end(), observation.track_id, + [](const auto &value, uint64_t key) { return value.track_id < key; }); + if (camera == cameras.end() || camera->image_id != observation.image_id || + landmark == landmarks.end() || landmark->track_id != observation.track_id) + return false; + const auto &resolved = preparation.resolved_observations[observation_index]; + Lardon3DSparseGeometryPoint2 predicted = {}; + if (!project(preparation.images[resolved.image_index].calibration, + camera->pose_cw, landmark->point, &predicted)) + return false; + residuals[local * 2] = predicted.x - resolved.source.x; + residuals[local * 2 + 1] = predicted.y - resolved.source.y; + } + return residual_metrics(residuals.data(), view.observations.size(), rmse, cost); +} + +bool build_public_candidate( + const Preparation &preparation, + const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic, + const std::vector &cameras, + const std::vector &landmarks, + std::vector *candidate_cameras, + std::vector *candidate_landmarks) { + if (!candidate_cameras || !candidate_landmarks) return false; + *candidate_cameras = preparation.cameras; + *candidate_landmarks = preparation.landmarks; + for (size_t index = 0; index < cameras.size(); ++index) { + const size_t source_index = cameras[index].source_index; + if (preparation.cameras[source_index].image_id == + diagnostic.pose_anchor_image_id) + continue; + if (!quaternion_to_pose( + cameras[index].quaternion, cameras[index].center, + &(*candidate_cameras)[source_index].pose_cw)) + return false; + } + for (const auto &landmark : landmarks) { + const Lardon3DSparseGeometryPoint3 point = { + landmark.point[0], landmark.point[1], landmark.point[2]}; + if (!finite_point(point)) return false; + (*candidate_landmarks)[landmark.source_index].point = point; + } + return true; +} + +PrivateTermination translate_termination(ceres::TerminationType value) { + if (value == ceres::CONVERGENCE) return PrivateTermination::converged; + if (value == ceres::NO_CONVERGENCE) return PrivateTermination::no_convergence; + return PrivateTermination::failure; +} + +Lardon3DSparseBundleAdjustmentTermination public_termination( + PrivateTermination value) { + if (value == PrivateTermination::converged) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED; + if (value == PrivateTermination::no_convergence) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE; + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_FAILURE; +} + +CandidateDecision validate_candidate(PrivateTermination termination, + bool final_metrics_valid, + bool gauge_valid, + double initial_cost, + double final_cost) { + if (!termination_accepted(termination)) { + return {false, false, + termination == PrivateTermination::no_convergence + ? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE + : LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE}; + } + if (!final_metrics_valid || !gauge_valid) + return {false, false, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE}; + if (!cost_acceptable(initial_cost, final_cost)) + return {false, true, + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION}; + return {true, true, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE}; +} + +void apply_component_decision( + const CandidateDecision &decision, const ComponentView &view, + const std::vector &candidate_cameras, + const std::vector &candidate_landmarks, + std::vector *published_cameras, + std::vector *published_landmarks) { + if (!decision.accepted) return; + for (size_t source_index : view.cameras) + (*published_cameras)[source_index] = candidate_cameras[source_index]; + for (size_t source_index : view.landmarks) + (*published_landmarks)[source_index] = candidate_landmarks[source_index]; +} + +bool configure_parameter_blocks( + ceres::Problem *problem, ceres::ParameterBlockOrdering *ordering, + const Preparation &preparation, + const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic, + std::vector *cameras, + std::vector *landmarks, + std::vector *records) { + if (!problem || !ordering || !cameras || !landmarks) return false; + if (records) records->clear(); + for (auto &landmark : *landmarks) { + problem->AddParameterBlock(landmark.point, 3); + ordering->AddElementToGroup(landmark.point, 0); + if (records) + records->push_back({ParameterKind::landmark, + preparation.landmarks[landmark.source_index].track_id, + 0, false, -1}); + } + for (auto &camera : *cameras) { + const uint64_t image_id = preparation.cameras[camera.source_index].image_id; + problem->AddParameterBlock(camera.quaternion, 4, + new ceres::QuaternionManifold); + problem->AddParameterBlock(camera.center, 3); + ordering->AddElementToGroup(camera.quaternion, 1); + ordering->AddElementToGroup(camera.center, 1); + if (records) { + records->push_back({ParameterKind::camera_quaternion, image_id, 1, + image_id == diagnostic.pose_anchor_image_id, -1}); + records->push_back({ParameterKind::camera_center, image_id, 1, + image_id == diagnostic.pose_anchor_image_id, + image_id == diagnostic.scale_anchor_image_id + ? static_cast(diagnostic.scale_axis) - 1 + : -1}); + } + } + auto pose_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) { + return preparation.cameras[camera.source_index].image_id == + diagnostic.pose_anchor_image_id; + }); + auto scale_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) { + return preparation.cameras[camera.source_index].image_id == + diagnostic.scale_anchor_image_id; + }); + if (pose_anchor == cameras->end() || scale_anchor == cameras->end()) return false; + problem->SetParameterBlockConstant(pose_anchor->quaternion); + problem->SetParameterBlockConstant(pose_anchor->center); + const int scale_axis = static_cast(diagnostic.scale_axis) - 1; + problem->SetManifold(scale_anchor->center, + new ceres::SubsetManifold(3, {scale_axis})); + return true; +} + +bool solve_component( + const Preparation &preparation, const ComponentView &view, + Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic, + std::vector *published_cameras, + std::vector *published_landmarks) { + if (!diagnostic || !published_cameras || !published_landmarks) return false; + std::vector cameras; + std::vector landmarks; + std::vector observations; + cameras.reserve(view.cameras.size()); + landmarks.reserve(view.landmarks.size()); + observations.reserve(view.observations.size()); + + for (size_t source_index : view.cameras) { + const auto &source = preparation.cameras[source_index]; + auto image = std::lower_bound( + preparation.images.begin(), preparation.images.end(), source.image_id, + [](const auto &value, uint64_t key) { return value.image_id < key; }); + SolverCamera camera = {}; + camera.source_index = source_index; + camera.image_index = static_cast(image - preparation.images.begin()); + if (image == preparation.images.end() || image->image_id != source.image_id || + !rotation_to_quaternion(source.pose_cw.rotation_cw, camera.quaternion) || + !camera_center(source.pose_cw, camera.center)) + return false; + std::copy(camera.center, camera.center + 3, camera.initial_center); + cameras.push_back(camera); + } + for (size_t source_index : view.landmarks) { + const auto &source = preparation.landmarks[source_index]; + landmarks.push_back( + {source_index, {source.point.x, source.point.y, source.point.z}}); + } + for (size_t resolved_index : view.observations) { + const auto &source = preparation.observations[resolved_index]; + auto camera = std::lower_bound( + cameras.begin(), cameras.end(), source.image_id, + [&](const auto &value, uint64_t key) { + return preparation.cameras[value.source_index].image_id < key; + }); + auto landmark = std::lower_bound( + landmarks.begin(), landmarks.end(), source.track_id, + [&](const auto &value, uint64_t key) { + return preparation.landmarks[value.source_index].track_id < key; + }); + if (camera == cameras.end() || landmark == landmarks.end()) return false; + observations.push_back({static_cast(camera - cameras.begin()), + static_cast(landmark - landmarks.begin()), + resolved_index}); + } + + if (!component_metrics(preparation, view, preparation.cameras, + preparation.landmarks, + &diagnostic->initial_reprojection_rmse_px, + &diagnostic->initial_robust_cost)) { + diagnostic->rejection_reason = + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE; + return true; + } + ceres::Problem problem; + auto ordering = std::make_shared(); + if (!configure_parameter_blocks(&problem, ordering.get(), preparation, + *diagnostic, &cameras, &landmarks, nullptr)) + return false; + + auto *loss = new ceres::HuberLoss(2.0); + for (const auto &observation : observations) { + const auto &resolved = preparation.resolved_observations[observation.resolved_index]; + const auto &calibration = preparation.images[cameras[observation.camera_index] + .image_index].calibration; + auto *cost = new ceres::AutoDiffCostFunction( + new ReprojectionResidual{calibration, resolved.source.x, resolved.source.y}); + problem.AddResidualBlock(cost, loss, + cameras[observation.camera_index].quaternion, + cameras[observation.camera_index].center, + landmarks[observation.landmark_index].point); + } + + auto pose_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) { + return preparation.cameras[camera.source_index].image_id == + diagnostic->pose_anchor_image_id; + }); + auto scale_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) { + return preparation.cameras[camera.source_index].image_id == + diagnostic->scale_anchor_image_id; + }); + if (pose_anchor == cameras.end() || scale_anchor == cameras.end()) return false; + const int scale_axis = static_cast(diagnostic->scale_axis) - 1; + + ceres::Solver::Options options; + options.minimizer_type = ceres::TRUST_REGION; + options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; + options.linear_solver_type = ceres::ITERATIVE_SCHUR; + options.preconditioner_type = ceres::SCHUR_JACOBI; + options.num_threads = 1; + options.max_num_iterations = 50; + options.function_tolerance = 1e-6; + options.gradient_tolerance = 1e-10; + options.parameter_tolerance = 1e-8; + options.linear_solver_ordering = std::move(ordering); + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + const PrivateTermination termination = translate_termination(summary.termination_type); + diagnostic->termination = public_termination(termination); + diagnostic->iteration_count = summary.iterations.size() > UINT32_MAX + ? UINT32_MAX + : static_cast(summary.iterations.size()); + std::vector candidate_cameras; + std::vector candidate_landmarks; + bool candidate_valid = false; + if (termination_accepted(termination) && + build_public_candidate(preparation, *diagnostic, cameras, landmarks, + &candidate_cameras, &candidate_landmarks)) { + candidate_valid = component_metrics( + preparation, view, candidate_cameras, candidate_landmarks, + &diagnostic->final_reprojection_rmse_px, + &diagnostic->final_robust_cost); + } + const bool gauge_valid = + scale_anchor->center[scale_axis] == scale_anchor->initial_center[scale_axis]; + const CandidateDecision decision = validate_candidate( + termination, candidate_valid, gauge_valid, diagnostic->initial_robust_cost, + diagnostic->final_robust_cost); + diagnostic->has_costs = decision.has_metrics; + diagnostic->has_rmse = decision.has_metrics; + diagnostic->accepted = decision.accepted; + diagnostic->rejection_reason = decision.rejection_reason; + apply_component_decision(decision, view, candidate_cameras, + candidate_landmarks, published_cameras, + published_landmarks); + return true; +} + +template +bool allocate_copy(const std::vector &source, T **destination) { + *destination = nullptr; + if (source.empty()) return true; + if (source.size() > SIZE_MAX / sizeof(T)) return false; + *destination = static_cast(std::malloc(source.size() * sizeof(T))); + if (!*destination) return false; + std::copy(source.begin(), source.end(), *destination); + return true; +} + +void destroy_result(Lardon3DSparseBundleAdjustmentResult *result) { + if (!result) return; + std::free(result->components); + std::free(result->cameras); + std::free(result->landmarks); + std::free(result->observations); + std::free(result->diagnostics); + *result = {}; +} + } // namespace lardon3d::sparse_bundle_adjustment +extern "C" Lardon3DSparseBundleAdjustmentExecutionStatus +lardon3d_sparse_bundle_adjustment_run( + const Lardon3DSparseBundleAdjustmentInput *input, + Lardon3DSparseBundleAdjustmentResult *result) { + using namespace lardon3d::sparse_bundle_adjustment; + if (!result) return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT; + destroy_result(result); + if (!input) return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT; + try { + Preparation preparation; + const PreparationStatus preparation_status = prepare(*input, &preparation); + if (preparation_status == PreparationStatus::invalid_argument) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT; + if (preparation_status == PreparationStatus::out_of_memory) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY; + if (preparation_status == PreparationStatus::internal_error) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR; + + std::vector views; + if (!build_component_views(preparation, &views)) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR; + std::vector published_cameras = + preparation.cameras; + std::vector published_landmarks = + preparation.landmarks; + size_t accepted = 0; + size_t rejected_eligible = 0; + for (size_t index = 0; index < views.size(); ++index) { + auto &diagnostic = preparation.diagnostics[index]; + if (!diagnostic.eligible) continue; + const UnderconstraintResult underconstraint = + underconstraint_result(preparation, views[index], diagnostic); + const UnderconstraintAction underconstraint_action = + classify_underconstraint(underconstraint); + if (underconstraint_action == UnderconstraintAction::internal_error) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR; + if (underconstraint_action == UnderconstraintAction::reject) { + diagnostic.eligible = false; + diagnostic.rejection_reason = + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED; + continue; + } + if (!solve_component(preparation, views[index], &diagnostic, + &published_cameras, &published_landmarks)) + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR; + if (diagnostic.accepted) + ++accepted; + else + ++rejected_eligible; + } + + Lardon3DSparseBundleAdjustmentResult candidate = {}; + candidate.status = accepted == 0 + ? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED + : rejected_eligible == 0 + ? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE + : LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL; + candidate.component_count = preparation.components.size(); + candidate.camera_count = published_cameras.size(); + candidate.landmark_count = published_landmarks.size(); + candidate.observation_count = preparation.observations.size(); + if (!allocate_copy(preparation.components, &candidate.components) || + !allocate_copy(published_cameras, &candidate.cameras) || + !allocate_copy(published_landmarks, &candidate.landmarks) || + !allocate_copy(preparation.observations, &candidate.observations) || + !allocate_copy(preparation.diagnostics, &candidate.diagnostics)) { + destroy_result(&candidate); + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY; + } + *result = candidate; + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK; + } catch (const std::bad_alloc &) { + destroy_result(result); + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY; + } catch (...) { + destroy_result(result); + return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR; + } +} + +extern "C" void lardon3d_sparse_bundle_adjustment_result_destroy( + Lardon3DSparseBundleAdjustmentResult *result) { + lardon3d::sparse_bundle_adjustment::destroy_result(result); +} + #ifdef LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TESTING extern "C" int lardon3d_sparse_bundle_adjustment_test_prepare( const Lardon3DSparseBundleAdjustmentInput *input, @@ -460,4 +1201,193 @@ extern "C" bool lardon3d_sparse_bundle_adjustment_test_termination_accepted( if (value < 0 || value > 2) return false; return termination_accepted(static_cast(value)); } + +extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_underconstraint( + const Lardon3DSparseBundleAdjustmentInput *input, size_t component_index) { + using namespace lardon3d::sparse_bundle_adjustment; + if (!input) return UINT32_MAX; + Preparation preparation; + if (prepare(*input, &preparation) != PreparationStatus::prepared || + component_index >= preparation.components.size()) + return UINT32_MAX; + std::vector views; + if (!build_component_views(preparation, &views)) return UINT32_MAX; + const UnderconstraintResult result = underconstraint_result( + preparation, views[component_index], preparation.diagnostics[component_index]); + return result.valid ? result.mask : UINT32_MAX; +} + +extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_structural_mask( + size_t camera_count, size_t landmark_count, size_t observation_count, + size_t pose_anchor, const size_t *camera_indices, + const size_t *landmark_indices, size_t edge_count) { + using namespace lardon3d::sparse_bundle_adjustment; + if (edge_count != 0 && (!camera_indices || !landmark_indices)) return UINT32_MAX; + std::vector> edges; + edges.reserve(edge_count); + for (size_t index = 0; index < edge_count; ++index) + edges.emplace_back(camera_indices[index], landmark_indices[index]); + return structural_underconstraint_mask(camera_count, landmark_count, + observation_count, pose_anchor, + std::move(edges)); +} + +extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_decision( + int termination, bool final_metrics_valid, bool gauge_valid, + double initial_cost, double final_cost, bool *has_metrics, + Lardon3DSparseBundleAdjustmentRejectionReason *reason) { + using namespace lardon3d::sparse_bundle_adjustment; + if (termination < 0 || termination > 2 || !has_metrics || !reason) return false; + const CandidateDecision decision = validate_candidate( + static_cast(termination), final_metrics_valid, + gauge_valid, initial_cost, final_cost); + *has_metrics = decision.has_metrics; + *reason = decision.rejection_reason; + return decision.accepted; +} + +extern "C" bool lardon3d_sparse_bundle_adjustment_test_counts_valid( + size_t image_count, size_t camera_count, size_t landmark_count, + size_t result_observation_count, size_t input_observation_count) { + return lardon3d::sparse_bundle_adjustment::public_counts_valid( + image_count, camera_count, landmark_count, result_observation_count, + input_observation_count); +} + +extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_publication( + bool final_metrics_valid, double initial_cost, double final_cost, + double original_x, double candidate_x, double *published_x, + bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) { + using namespace lardon3d::sparse_bundle_adjustment; + if (!published_x || !has_metrics || !reason) return false; + const CandidateDecision decision = validate_candidate( + PrivateTermination::converged, final_metrics_valid, true, initial_cost, + final_cost); + ComponentView view; + view.landmarks.push_back(0); + std::vector cameras; + std::vector published(1); + std::vector candidate(1); + published[0].point.x = original_x; + candidate[0].point.x = candidate_x; + apply_component_decision(decision, view, cameras, candidate, &cameras, + &published); + *published_x = published[0].point.x; + *has_metrics = decision.has_metrics; + *reason = decision.rejection_reason; + return decision.accepted; +} + +extern "C" int lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() { + using namespace lardon3d::sparse_bundle_adjustment; + Preparation preparation; + preparation.cameras.resize(1); + preparation.cameras[0].image_id = 10; + preparation.landmarks.resize(1); + preparation.landmarks[0].track_id = 20; + preparation.observations.resize(1); + preparation.observations[0].image_id = 99; + preparation.observations[0].track_id = 20; + ComponentView view; + view.cameras.push_back(0); + view.landmarks.push_back(0); + view.observations.push_back(0); + Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostic = {}; + diagnostic.pose_anchor_image_id = 10; + const UnderconstraintAction action = classify_underconstraint( + underconstraint_result(preparation, view, diagnostic)); + return action == UnderconstraintAction::internal_error + ? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR + : LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK; +} + +extern "C" bool lardon3d_sparse_bundle_adjustment_test_parameter_ordering( + const Lardon3DSparseBundleAdjustmentInput *input, int *kinds, + uint64_t *identities, int *groups, bool *constants, int *subset_axes, + size_t capacity, size_t *count) { + using namespace lardon3d::sparse_bundle_adjustment; + if (!input || !count) return false; + Preparation preparation; + if (prepare(*input, &preparation) != PreparationStatus::prepared || + preparation.components.size() != 1) + return false; + std::vector views; + if (!build_component_views(preparation, &views)) return false; + std::vector cameras; + std::vector landmarks; + for (size_t source_index : views[0].cameras) { + SolverCamera camera = {}; + camera.source_index = source_index; + if (!rotation_to_quaternion(preparation.cameras[source_index].pose_cw.rotation_cw, + camera.quaternion) || + !camera_center(preparation.cameras[source_index].pose_cw, camera.center)) + return false; + cameras.push_back(camera); + } + for (size_t source_index : views[0].landmarks) { + const auto &point = preparation.landmarks[source_index].point; + landmarks.push_back({source_index, {point.x, point.y, point.z}}); + } + ceres::Problem problem; + ceres::ParameterBlockOrdering ordering; + std::vector records; + if (!configure_parameter_blocks(&problem, &ordering, preparation, + preparation.diagnostics[0], &cameras, + &landmarks, &records)) + return false; + *count = records.size(); + if (records.size() > capacity || + (!records.empty() && + (!kinds || !identities || !groups || !constants || !subset_axes))) + return false; + for (size_t index = 0; index < records.size(); ++index) { + kinds[index] = static_cast(records[index].kind); + identities[index] = records[index].identity; + groups[index] = records[index].group; + constants[index] = records[index].constant; + subset_axes[index] = records[index].subset_axis; + } + return true; +} + +extern "C" bool lardon3d_sparse_bundle_adjustment_test_invalid_candidate( + const Lardon3DSparseBundleAdjustmentInput *input, double *published_z, + bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) { + using namespace lardon3d::sparse_bundle_adjustment; + if (!input || !published_z || !has_metrics || !reason) return false; + Preparation preparation; + if (prepare(*input, &preparation) != PreparationStatus::prepared || + preparation.components.size() != 1 || preparation.landmarks.empty()) + return false; + std::vector views; + if (!build_component_views(preparation, &views)) return false; + std::vector candidate_cameras = + preparation.cameras; + std::vector candidate_landmarks = + preparation.landmarks; + candidate_landmarks[views[0].landmarks[0]].point.z = -1.0; + double initial_rmse = 0.0; + double initial_cost = 0.0; + double final_rmse = 0.0; + double final_cost = 0.0; + if (!component_metrics(preparation, views[0], preparation.cameras, + preparation.landmarks, &initial_rmse, &initial_cost)) + return false; + const bool final_valid = component_metrics( + preparation, views[0], candidate_cameras, candidate_landmarks, + &final_rmse, &final_cost); + const CandidateDecision decision = validate_candidate( + PrivateTermination::converged, final_valid, true, initial_cost, final_cost); + std::vector published_cameras = + preparation.cameras; + std::vector published_landmarks = + preparation.landmarks; + apply_component_decision(decision, views[0], candidate_cameras, + candidate_landmarks, &published_cameras, + &published_landmarks); + *published_z = published_landmarks[views[0].landmarks[0]].point.z; + *has_metrics = decision.has_metrics; + *reason = decision.rejection_reason; + return !decision.accepted && !final_valid; +} #endif diff --git a/tests/test_sparse_sfm_bundle_adjustment.cpp b/tests/test_sparse_sfm_bundle_adjustment.cpp index 2b4a381..24fea96 100644 --- a/tests/test_sparse_sfm_bundle_adjustment.cpp +++ b/tests/test_sparse_sfm_bundle_adjustment.cpp @@ -1,9 +1,12 @@ #include +#include #include +#include #include #include #include +#include #define CHECK(value) \ do { \ @@ -30,11 +33,132 @@ extern "C" bool lardon3d_sparse_bundle_adjustment_test_metrics( const double *residuals, size_t count, double *rmse, double *huber_cost); extern "C" bool lardon3d_sparse_bundle_adjustment_test_cost_acceptable( double initial_cost, double final_cost); +extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_structural_mask( + size_t camera_count, size_t landmark_count, size_t observation_count, + size_t pose_anchor, const size_t *camera_indices, + const size_t *landmark_indices, size_t edge_count); +extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_decision( + int termination, bool final_metrics_valid, bool gauge_valid, + double initial_cost, double final_cost, bool *has_metrics, + Lardon3DSparseBundleAdjustmentRejectionReason *reason); +extern "C" bool lardon3d_sparse_bundle_adjustment_test_counts_valid( + size_t image_count, size_t camera_count, size_t landmark_count, + size_t result_observation_count, size_t input_observation_count); +extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_publication( + bool final_metrics_valid, double initial_cost, double final_cost, + double original_x, double candidate_x, double *published_x, + bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason); +extern "C" int lardon3d_sparse_bundle_adjustment_test_internal_underconstraint(); +extern "C" bool lardon3d_sparse_bundle_adjustment_test_parameter_ordering( + const Lardon3DSparseBundleAdjustmentInput *input, int *kinds, + uint64_t *identities, int *groups, bool *constants, int *subset_axes, + size_t capacity, size_t *count); +extern "C" bool lardon3d_sparse_bundle_adjustment_test_invalid_candidate( + const Lardon3DSparseBundleAdjustmentInput *input, double *published_z, + bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason); static Lardon3DSparseGeometryCalibration calibration() { return {1280, 960, 800.0, 810.0, 640.0, 480.0, 0.0, 0.0, 0.0, 0.0}; } +static double camera_center_x(const Lardon3DSparseGeometryPose &pose) { + return -(pose.rotation_cw[0] * pose.translation_cw[0] + + pose.rotation_cw[3] * pose.translation_cw[1] + + pose.rotation_cw[6] * pose.translation_cw[2]); +} + +static bool close_scalar(double a, double b) { + return std::isfinite(a) && std::isfinite(b) && + std::abs(a - b) <= + 1e-12 * std::max({1.0, std::abs(a), std::abs(b)}); +} + +static bool close_rotation(const double a[9], const double b[9]) { + bool identical = true; + for (size_t index = 0; index < 9; ++index) + identical = identical && a[index] == b[index]; + if (identical) return true; + double trace = 0.0; + for (size_t row = 0; row < 3; ++row) + for (size_t column = 0; column < 3; ++column) + trace += a[row * 3 + column] * b[row * 3 + column]; + const double cosine = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5)); + return std::acos(cosine) <= 1e-12; +} + +static bool equal_results(const Lardon3DSparseBundleAdjustmentResult &a, + const Lardon3DSparseBundleAdjustmentResult &b) { + if (a.status != b.status || a.component_count != b.component_count || + a.camera_count != b.camera_count || a.landmark_count != b.landmark_count || + a.observation_count != b.observation_count) + return false; + for (size_t index = 0; index < a.component_count; ++index) { + const auto &ac = a.components[index]; + const auto &bc = b.components[index]; + if (ac.component_key != bc.component_key || ac.image_count != bc.image_count || + ac.registered_image_count != bc.registered_image_count || + ac.landmark_count != bc.landmark_count) + return false; + const auto &ad = a.diagnostics[index]; + const auto &bd = b.diagnostics[index]; + if (ad.component_key != bd.component_key || ad.camera_count != bd.camera_count || + ad.landmark_count != bd.landmark_count || + ad.observation_count != bd.observation_count || ad.eligible != bd.eligible || + ad.has_anchors != bd.has_anchors || + ad.pose_anchor_image_id != bd.pose_anchor_image_id || + ad.scale_anchor_image_id != bd.scale_anchor_image_id || + ad.scale_axis != bd.scale_axis || ad.has_costs != bd.has_costs || + ad.has_rmse != bd.has_rmse || ad.iteration_count != bd.iteration_count || + ad.termination != bd.termination || ad.accepted != bd.accepted || + ad.rejection_reason != bd.rejection_reason) + return false; + if (ad.has_costs && + (!close_scalar(ad.initial_robust_cost, bd.initial_robust_cost) || + !close_scalar(ad.final_robust_cost, bd.final_robust_cost))) + return false; + if (ad.has_rmse && + (!close_scalar(ad.initial_reprojection_rmse_px, + bd.initial_reprojection_rmse_px) || + !close_scalar(ad.final_reprojection_rmse_px, + bd.final_reprojection_rmse_px))) + return false; + } + for (size_t index = 0; index < a.camera_count; ++index) { + if (a.cameras[index].image_id != b.cameras[index].image_id || + a.cameras[index].component_key != b.cameras[index].component_key || + !close_rotation(a.cameras[index].pose_cw.rotation_cw, + b.cameras[index].pose_cw.rotation_cw)) + return false; + for (size_t item = 0; item < 3; ++item) + if (!close_scalar(a.cameras[index].pose_cw.translation_cw[item], + b.cameras[index].pose_cw.translation_cw[item])) + return false; + } + for (size_t index = 0; index < a.landmark_count; ++index) { + const auto &al = a.landmarks[index]; + const auto &bl = b.landmarks[index]; + if (al.landmark_id != bl.landmark_id || al.track_id != bl.track_id || + al.component_key != bl.component_key || + al.observation_count != bl.observation_count || + !close_scalar(al.point.x, bl.point.x) || + !close_scalar(al.point.y, bl.point.y) || + !close_scalar(al.point.z, bl.point.z) || + !close_scalar(al.reprojection_rmse_px, bl.reprojection_rmse_px) || + !close_scalar(al.reprojection_median_px, bl.reprojection_median_px)) + return false; + } + for (size_t index = 0; index < a.observation_count; ++index) { + const auto &ao = a.observations[index]; + const auto &bo = b.observations[index]; + if (ao.landmark_id != bo.landmark_id || ao.track_id != bo.track_id || + ao.image_id != bo.image_id || ao.feature_set_id != bo.feature_set_id || + ao.feature_index != bo.feature_index || + ao.position_in_track != bo.position_in_track) + return false; + } + return true; +} + static int test_helpers() { const Lardon3DSparseGeometryPose pose = { {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, {0.0, 0.0, 0.0}}; @@ -100,6 +224,59 @@ static int test_helpers() { CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(1)); CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(2)); CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(3)); + + bool has_metrics = false; + Lardon3DSparseBundleAdjustmentRejectionReason reason = + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE; + CHECK(lardon3d_sparse_bundle_adjustment_test_candidate_decision( + 0, true, true, 10.0, 9.0, &has_metrics, &reason)); + CHECK(has_metrics && reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE); + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision( + 1, false, true, 10.0, 0.0, &has_metrics, &reason)); + CHECK(!has_metrics && reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE); + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision( + 2, false, true, 10.0, 0.0, &has_metrics, &reason)); + CHECK(!has_metrics && reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE); + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision( + 0, false, true, 10.0, 0.0, &has_metrics, &reason)); + CHECK(!has_metrics && reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE); + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_decision( + 0, true, true, 10.0, 11.0, &has_metrics, &reason)); + CHECK(has_metrics && reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION); + double published_x = 0.0; + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_publication( + false, 10.0, 0.0, 3.0, 9.0, &published_x, &has_metrics, &reason)); + CHECK(published_x == 3.0 && !has_metrics && + reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE); + CHECK(!lardon3d_sparse_bundle_adjustment_test_candidate_publication( + true, 10.0, 11.0, 3.0, 9.0, &published_x, &has_metrics, &reason)); + CHECK(published_x == 3.0 && has_metrics && + reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION); + + CHECK(lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4095, 4095, 249999, 999999, 999999)); + CHECK(lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4096, 4096, 250000, 1000000, 1000000)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4097, 4096, 250000, 1000000, 1000000)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4096, 4097, 250000, 1000000, 1000000)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4096, 4096, 250001, 1000000, 1000000)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4096, 4096, 250000, 1000001, 1000000)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 4096, 4096, 250000, 1000000, 1000001)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + 0, 0, 0, std::numeric_limits::max(), 0)); + CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid( + std::numeric_limits::max(), 0, 0, 0, 0)); + CHECK(lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR); return 0; } @@ -236,6 +413,22 @@ static int test_preparation() { CHECK(!diagnostics[0].eligible); CHECK(diagnostics[0].rejection_reason == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE); + + value = fixture(); + bind_fixture(&value); + value.cameras[1].pose_cw.translation_cw[0] = + -std::nextafter(1e-9, 1.0); + value.cameras[1].pose_cw.translation_cw[1] = 0.0; + value.cameras[2].pose_cw.translation_cw[0] = + std::nextafter(1e-9, 1.0); + value.cameras[2].pose_cw.translation_cw[1] = 0.0; + CHECK(lardon3d_sparse_bundle_adjustment_test_prepare( + &value.input, diagnostics, 1, resolved, 3, &diagnostic_count, + &resolved_count) == 0); + CHECK(diagnostics[0].eligible); + CHECK(diagnostics[0].scale_anchor_image_id == 20); + CHECK(diagnostics[0].scale_axis == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X); return 0; } @@ -348,9 +541,649 @@ static int test_multiple_components() { return 0; } -int main() { +static int test_underconstraint_rules() { + std::vector cameras; + std::vector landmarks; + auto complete_edges = [&](size_t camera_count, size_t landmark_count) { + cameras.clear(); + landmarks.clear(); + for (size_t camera = 0; camera < camera_count; ++camera) { + for (size_t landmark = 0; landmark < landmark_count; ++landmark) { + cameras.push_back(camera); + landmarks.push_back(landmark); + } + } + }; + + complete_edges(4, 3); + CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask( + 4, 3, cameras.size(), 0, cameras.data(), landmarks.data(), + cameras.size()) == 1U); + + complete_edges(3, 6); + for (size_t index = cameras.size(); index-- > 0;) { + if (landmarks[index] == 0 && cameras[index] != 0) { + cameras.erase(cameras.begin() + static_cast(index)); + landmarks.erase(landmarks.begin() + static_cast(index)); + } + } + CHECK((lardon3d_sparse_bundle_adjustment_test_structural_mask( + 3, 6, cameras.size(), 0, cameras.data(), landmarks.data(), + cameras.size()) & 2U) != 0); + + complete_edges(3, 10); + for (size_t index = cameras.size(); index-- > 0;) { + if (cameras[index] == 1 && landmarks[index] >= 2) { + cameras.erase(cameras.begin() + static_cast(index)); + landmarks.erase(landmarks.begin() + static_cast(index)); + } + } + CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask( + 3, 10, cameras.size(), 0, cameras.data(), landmarks.data(), + cameras.size()) == 4U); + + cameras.clear(); + landmarks.clear(); + for (size_t group = 0; group < 2; ++group) { + for (size_t camera = group * 3; camera < group * 3 + 3; ++camera) { + for (size_t landmark = group * 6; landmark < group * 6 + 6; ++landmark) { + cameras.push_back(camera); + landmarks.push_back(landmark); + } + } + } + CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask( + 6, 12, cameras.size(), 0, cameras.data(), landmarks.data(), + cameras.size()) == 8U); + return 0; +} + +struct ScientificFixture { + std::vector images; + std::vector input_observations; + std::vector components; + std::vector cameras; + std::vector landmarks; + std::vector observations; + Lardon3DSparseIncrementalResult incremental = {}; + Lardon3DSparseBundleAdjustmentInput input = {}; +}; + +static ScientificFixture scientific_fixture(bool perturb_camera, + bool perturb_landmarks, + size_t landmark_count = 5, + bool nontrivial_anchor = false) { + ScientificFixture value; + const auto intrinsic = calibration(); + const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + const double anchor_angle = 0.2; + const double anchor_rotation[9] = { + std::cos(anchor_angle), -std::sin(anchor_angle), 0.0, + std::sin(anchor_angle), std::cos(anchor_angle), 0.0, + 0.0, 0.0, 1.0}; + for (size_t camera = 0; camera < 4; ++camera) { + const uint64_t image_id = 10 + camera * 10; + value.images.push_back({image_id, intrinsic}); + Lardon3DSparseIncrementalCamera output = {}; + output.image_id = image_id; + output.component_key = 10; + std::memcpy(output.pose_cw.rotation_cw, + camera == 0 && nontrivial_anchor ? anchor_rotation : identity, + sizeof(identity)); + output.pose_cw.translation_cw[0] = -static_cast(camera); + value.cameras.push_back(output); + } + if (perturb_camera) value.cameras[1].pose_cw.translation_cw[1] = 0.08; + + for (size_t landmark = 0; landmark < landmark_count; ++landmark) { + const uint64_t track_id = landmark + 1; + const Lardon3DSparseGeometryPoint3 truth = { + -0.6 + 0.25 * static_cast(landmark), + -0.3 + 0.12 * static_cast(landmark % 3), + 4.5 + 0.3 * static_cast(landmark)}; + auto initial = truth; + if (perturb_landmarks) { + initial.x += 0.06; + initial.y -= 0.04; + } + value.landmarks.push_back( + {track_id, track_id, 10, initial, 0.0, 0.0, 4}); + for (size_t camera = 0; camera < 4; ++camera) { + Lardon3DSparseGeometryPose truth_pose = {}; + std::memcpy(truth_pose.rotation_cw, + camera == 0 && nontrivial_anchor ? anchor_rotation : identity, + sizeof(identity)); + truth_pose.translation_cw[0] = -static_cast(camera); + Lardon3DSparseGeometryPoint2 pixel = {}; + if (!lardon3d_sparse_bundle_adjustment_test_project( + &intrinsic, &truth_pose, &truth, &pixel)) + std::abort(); + const uint64_t image_id = 10 + camera * 10; + const uint64_t feature_set_id = 100 + camera; + value.input_observations.push_back( + {track_id, image_id, feature_set_id, + static_cast(landmark), 64, pixel.x, pixel.y}); + value.observations.push_back( + {track_id, track_id, image_id, feature_set_id, + static_cast(landmark), static_cast(camera)}); + } + } + value.components.push_back({10, 4, 4, landmark_count}); + value.incremental.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE; + value.incremental.track_set_id = 77; + value.incremental.calibration_scope_id = 88; + return value; +} + +static void bind_scientific_fixture(ScientificFixture *value) { + value->incremental.components = value->components.data(); + value->incremental.component_count = value->components.size(); + value->incremental.cameras = value->cameras.data(); + value->incremental.camera_count = value->cameras.size(); + value->incremental.landmarks = value->landmarks.data(); + value->incremental.landmark_count = value->landmarks.size(); + value->incremental.observations = value->observations.data(); + value->incremental.observation_count = value->observations.size(); + value->input = {&value->incremental, value->images.data(), value->images.size(), + value->input_observations.data(), + value->input_observations.size()}; +} + +static bool published_metrics(const ScientificFixture &fixture, + const Lardon3DSparseBundleAdjustmentResult &result, + double *rmse, double *cost) { + std::vector residuals(result.observation_count * 2); + for (size_t index = 0; index < result.observation_count; ++index) { + const auto &association = result.observations[index]; + auto camera = std::lower_bound( + result.cameras, result.cameras + result.camera_count, association.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + auto landmark = std::lower_bound( + result.landmarks, result.landmarks + result.landmark_count, + association.track_id, + [](const auto &item, uint64_t key) { return item.track_id < key; }); + auto source = std::find_if( + fixture.input_observations.begin(), fixture.input_observations.end(), + [&](const auto &item) { + return item.feature_set_id == association.feature_set_id && + item.feature_index == association.feature_index; + }); + auto image = std::lower_bound( + fixture.images.begin(), fixture.images.end(), association.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + if (camera == result.cameras + result.camera_count || + landmark == result.landmarks + result.landmark_count || + source == fixture.input_observations.end() || image == fixture.images.end()) + return false; + Lardon3DSparseGeometryPoint2 pixel = {}; + if (!lardon3d_sparse_bundle_adjustment_test_project( + &image->calibration, &camera->pose_cw, &landmark->point, &pixel)) + return false; + residuals[index * 2] = pixel.x - source->x; + residuals[index * 2 + 1] = pixel.y - source->y; + } + return lardon3d_sparse_bundle_adjustment_test_metrics( + residuals.data(), result.observation_count, rmse, cost); +} + +static ScientificFixture two_component_fixture(bool reject_second) { + ScientificFixture first = scientific_fixture(true, true); + ScientificFixture second = scientific_fixture(true, true); + for (auto &image : second.images) image.image_id += 100; + for (auto &observation : second.input_observations) { + observation.track_id += 100; + observation.image_id += 100; + observation.feature_set_id += 1000; + } + for (auto &component : second.components) component.component_key += 100; + for (auto &camera : second.cameras) { + camera.image_id += 100; + camera.component_key += 100; + } + for (auto &landmark : second.landmarks) { + landmark.landmark_id += 100; + landmark.track_id += 100; + landmark.component_key += 100; + } + if (reject_second) second.landmarks[0].point.z = -1.0; + for (auto &observation : second.observations) { + observation.landmark_id += 100; + observation.track_id += 100; + observation.image_id += 100; + observation.feature_set_id += 1000; + } + first.images.insert(first.images.end(), second.images.begin(), second.images.end()); + first.input_observations.insert(first.input_observations.end(), + second.input_observations.begin(), + second.input_observations.end()); + first.components.insert(first.components.end(), second.components.begin(), + second.components.end()); + first.cameras.insert(first.cameras.end(), second.cameras.begin(), + second.cameras.end()); + first.landmarks.insert(first.landmarks.end(), second.landmarks.begin(), + second.landmarks.end()); + first.observations.insert(first.observations.end(), second.observations.begin(), + second.observations.end()); + return first; +} + +static int run_scientific_case(bool perturb_camera, bool perturb_landmarks, + double noise_px, size_t outlier_percent) { + auto value = scientific_fixture(perturb_camera, perturb_landmarks, + noise_px == 0.0 && outlier_percent == 0 ? 5 : 10); + if (noise_px != 0.0) { + for (auto &landmark : value.landmarks) landmark.point.z = 5.0; + for (auto &observation : value.input_observations) { + const auto camera = std::lower_bound( + value.cameras.begin(), value.cameras.end(), observation.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + const auto landmark = std::lower_bound( + value.landmarks.begin(), value.landmarks.end(), observation.track_id, + [](const auto &item, uint64_t key) { return item.track_id < key; }); + const auto image = std::lower_bound( + value.images.begin(), value.images.end(), observation.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + CHECK(camera != value.cameras.end()); + CHECK(landmark != value.landmarks.end()); + CHECK(image != value.images.end()); + Lardon3DSparseGeometryPoint2 noiseless = {}; + CHECK(lardon3d_sparse_bundle_adjustment_test_project( + &image->calibration, &camera->pose_cw, &landmark->point, &noiseless)); + observation.x = noiseless.x; + observation.y = noiseless.y; + } + for (size_t index = 0; index < value.input_observations.size(); ++index) { + auto &observation = value.input_observations[index]; + const auto camera = std::lower_bound( + value.cameras.begin(), value.cameras.end(), observation.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + const auto landmark = std::lower_bound( + value.landmarks.begin(), value.landmarks.end(), observation.track_id, + [](const auto &item, uint64_t key) { return item.track_id < key; }); + const auto image = std::lower_bound( + value.images.begin(), value.images.end(), observation.image_id, + [](const auto &item, uint64_t key) { return item.image_id < key; }); + CHECK(camera != value.cameras.end()); + CHECK(landmark != value.landmarks.end()); + CHECK(image != value.images.end()); + Lardon3DSparseGeometryPoint2 noiseless = {}; + CHECK(lardon3d_sparse_bundle_adjustment_test_project( + &image->calibration, &camera->pose_cw, &landmark->point, &noiseless)); + CHECK(close_scalar(observation.x, noiseless.x)); + CHECK(close_scalar(observation.y, noiseless.y)); + observation.y += observation.track_id % 2 == 0 ? noise_px : -noise_px; + CHECK(std::hypot(observation.x - noiseless.x, + observation.y - noiseless.y) == noise_px); + } + } + const size_t outlier_count = + value.input_observations.size() * outlier_percent / 100; + CHECK(outlier_count * 100 == + value.input_observations.size() * outlier_percent); + if (outlier_percent == 10) CHECK(outlier_count == 4); + if (outlier_percent == 20) CHECK(outlier_count == 8); + if (outlier_percent == 40) CHECK(outlier_count == 16); + for (size_t index = 0; index < outlier_count; ++index) { + value.input_observations[index].x += index % 2 == 0 ? 2.000001 : -2.000001; + } + bind_scientific_fixture(&value); + Lardon3DSparseBundleAdjustmentResult result = {}; + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + if (outlier_percent != 0 && + result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED) { + CHECK(!result.diagnostics[0].accepted); + CHECK(result.diagnostics[0].termination == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE); + CHECK(result.diagnostics[0].rejection_reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE); + CHECK(!result.diagnostics[0].has_costs && !result.diagnostics[0].has_rmse); + CHECK(std::memcmp(result.cameras, value.cameras.data(), + value.cameras.size() * sizeof(value.cameras[0])) == 0); + CHECK(std::memcmp(result.landmarks, value.landmarks.data(), + value.landmarks.size() * sizeof(value.landmarks[0])) == 0); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + return 0; + } + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE); + CHECK(result.diagnostics[0].accepted); + CHECK(result.diagnostics[0].termination == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED); + CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse); + CHECK(std::isfinite(result.diagnostics[0].initial_robust_cost)); + CHECK(std::isfinite(result.diagnostics[0].final_robust_cost)); + CHECK(result.diagnostics[0].final_robust_cost <= + result.diagnostics[0].initial_robust_cost + + 1e-12 * std::max(1.0, + std::abs(result.diagnostics[0].initial_robust_cost))); + if (perturb_camera || perturb_landmarks) + CHECK(result.diagnostics[0].final_robust_cost < + result.diagnostics[0].initial_robust_cost); + CHECK(std::memcmp(&result.cameras[0], &value.cameras[0], + sizeof(value.cameras[0])) == 0); + double published_rmse = 0.0; + double published_cost = 0.0; + CHECK(published_metrics(value, result, &published_rmse, &published_cost)); + CHECK(close_scalar(published_rmse, + result.diagnostics[0].final_reprojection_rmse_px)); + CHECK(close_scalar(published_cost, result.diagnostics[0].final_robust_cost)); + const size_t scale_index = value.cameras.size() - 1; + const double scale_before = camera_center_x(value.cameras[scale_index].pose_cw); + const double scale_after = camera_center_x(result.cameras[scale_index].pose_cw); + CHECK(std::abs(scale_after - scale_before) <= + 1e-12 * std::max({1.0, std::abs(scale_before), std::abs(scale_after)})); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + return 0; +} + +static int test_execution_and_solver() { + Lardon3DSparseBundleAdjustmentResult result = {}; + CHECK(lardon3d_sparse_bundle_adjustment_run(nullptr, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT); + CHECK(result.components == nullptr && result.component_count == 0); + + auto value = scientific_fixture(false, false); + bind_scientific_fixture(&value); + const auto original_cameras = value.cameras; + const auto original_landmarks = value.landmarks; + const auto original_images = value.images; + const auto original_observations = value.input_observations; + const auto original_components = value.components; + const auto original_result_observations = value.observations; + const auto original_incremental = value.incremental; + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE); + CHECK(result.diagnostics[0].eligible && result.diagnostics[0].accepted); + CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse); + CHECK(result.diagnostics[0].termination == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED); + CHECK(std::memcmp(value.cameras.data(), original_cameras.data(), + value.cameras.size() * sizeof(value.cameras[0])) == 0); + CHECK(std::memcmp(value.landmarks.data(), original_landmarks.data(), + value.landmarks.size() * sizeof(value.landmarks[0])) == 0); + CHECK(std::memcmp(value.input_observations.data(), original_observations.data(), + value.input_observations.size() * + sizeof(value.input_observations[0])) == 0); + CHECK(std::memcmp(value.images.data(), original_images.data(), + value.images.size() * sizeof(value.images[0])) == 0); + CHECK(std::memcmp(value.components.data(), original_components.data(), + value.components.size() * sizeof(value.components[0])) == 0); + CHECK(std::memcmp(value.observations.data(), original_result_observations.data(), + value.observations.size() * sizeof(value.observations[0])) == 0); + CHECK(std::memcmp(&value.incremental, &original_incremental, + sizeof(value.incremental)) == 0); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + lardon3d_sparse_bundle_adjustment_result_destroy(nullptr); + + auto anchor_value = scientific_fixture(false, false, 5, true); + bind_scientific_fixture(&anchor_value); + CHECK(lardon3d_sparse_bundle_adjustment_run(&anchor_value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE); + CHECK(std::memcmp(&result.cameras[0], &anchor_value.cameras[0], + sizeof(result.cameras[0])) == 0); + double anchor_rmse = 0.0; + double anchor_cost = 0.0; + CHECK(published_metrics(anchor_value, result, &anchor_rmse, &anchor_cost)); + CHECK(close_scalar(anchor_rmse, + result.diagnostics[0].final_reprojection_rmse_px)); + CHECK(close_scalar(anchor_cost, result.diagnostics[0].final_robust_cost)); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + Lardon3DSparseIncrementalImage lone_image = {10, calibration()}; + Lardon3DSparseIncrementalComponent lone_component = {10, 1, 1, 0}; + Lardon3DSparseIncrementalCamera lone_camera = {}; + lone_camera.image_id = 10; + lone_camera.component_key = 10; + const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + std::memcpy(lone_camera.pose_cw.rotation_cw, identity, sizeof(identity)); + Lardon3DSparseIncrementalResult lone_incremental = {}; + lone_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED; + lone_incremental.components = &lone_component; + lone_incremental.component_count = 1; + lone_incremental.cameras = &lone_camera; + lone_incremental.camera_count = 1; + Lardon3DSparseBundleAdjustmentInput lone_input = { + &lone_incremental, &lone_image, 1, nullptr, 0}; + CHECK(lardon3d_sparse_bundle_adjustment_run(&lone_input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED); + CHECK(!result.diagnostics[0].eligible); + CHECK(result.diagnostics[0].rejection_reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + Lardon3DSparseIncrementalResult empty_incremental = {}; + empty_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED; + Lardon3DSparseBundleAdjustmentInput empty_input = { + &empty_incremental, nullptr, 0, nullptr, 0}; + CHECK(lardon3d_sparse_bundle_adjustment_run(&empty_input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED); + CHECK(result.component_count == 0 && result.camera_count == 0 && + result.landmark_count == 0 && result.observation_count == 0); + CHECK(result.components == nullptr && result.cameras == nullptr && + result.landmarks == nullptr && result.observations == nullptr && + result.diagnostics == nullptr); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + value = scientific_fixture(true, true); + bind_scientific_fixture(&value); + int kinds[13] = {}; + uint64_t identities[13] = {}; + int groups[13] = {}; + bool constants[13] = {}; + int subset_axes[13] = {}; + size_t parameter_count = 0; + CHECK(lardon3d_sparse_bundle_adjustment_test_parameter_ordering( + &value.input, kinds, identities, groups, constants, subset_axes, 13, + ¶meter_count)); + CHECK(parameter_count == 13); + for (size_t index = 0; index < 5; ++index) + CHECK(kinds[index] == 0 && identities[index] == index + 1 && groups[index] == 0); + for (size_t camera = 0; camera < 4; ++camera) { + const size_t quaternion = 5 + camera * 2; + const size_t center = quaternion + 1; + CHECK(kinds[quaternion] == 1 && kinds[center] == 2); + CHECK(identities[quaternion] == 10 + camera * 10 && + identities[center] == identities[quaternion]); + CHECK(groups[quaternion] == 1 && groups[center] == 1); + CHECK(constants[quaternion] == (camera == 0)); + CHECK(constants[center] == (camera == 0)); + CHECK(subset_axes[center] == (camera == 3 ? 0 : -1)); + } + double published_z = 0.0; + bool candidate_has_metrics = true; + Lardon3DSparseBundleAdjustmentRejectionReason candidate_reason = + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE; + CHECK(lardon3d_sparse_bundle_adjustment_test_invalid_candidate( + &value.input, &published_z, &candidate_has_metrics, &candidate_reason)); + CHECK(published_z == value.landmarks[0].point.z); + CHECK(!candidate_has_metrics && + candidate_reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE); + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE); + CHECK(result.diagnostics[0].accepted); + CHECK(result.diagnostics[0].final_robust_cost <= + result.diagnostics[0].initial_robust_cost); + CHECK(std::memcmp(&result.cameras[0], &value.cameras[0], + sizeof(value.cameras[0])) == 0); + CHECK(std::abs(camera_center_x(result.cameras[3].pose_cw) - + camera_center_x(value.cameras[3].pose_cw)) < 1e-12); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + value = two_component_fixture(false); + bind_scientific_fixture(&value); + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE); + CHECK(result.component_count == 2 && result.diagnostics[0].accepted && + result.diagnostics[1].accepted); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + value = two_component_fixture(true); + bind_scientific_fixture(&value); + const auto rejected_cameras = value.cameras; + const auto rejected_landmarks = value.landmarks; + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL); + CHECK(result.diagnostics[0].accepted && !result.diagnostics[1].accepted); + CHECK(result.diagnostics[1].rejection_reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE); + CHECK(std::memcmp(result.cameras + 4, rejected_cameras.data() + 4, + 4 * sizeof(result.cameras[0])) == 0); + CHECK(std::memcmp(result.landmarks + 5, rejected_landmarks.data() + 5, + 5 * sizeof(result.landmarks[0])) == 0); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + CHECK(run_scientific_case(true, false, 0.0, 0) == 0); + CHECK(run_scientific_case(false, true, 0.0, 0) == 0); + CHECK(run_scientific_case(true, true, 0.0, 0) == 0); + CHECK(run_scientific_case(false, false, 0.5, 0) == 0); + CHECK(run_scientific_case(false, false, 1.0, 0) == 0); + CHECK(run_scientific_case(false, false, 2.0, 0) == 0); + CHECK(run_scientific_case(false, false, 0.0, 10) == 0); + CHECK(run_scientific_case(false, false, 0.0, 20) == 0); + CHECK(run_scientific_case(false, false, 0.0, 40) == 0); + + value = scientific_fixture(true, true); + bind_scientific_fixture(&value); + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + for (size_t run = 1; run < 5; ++run) { + Lardon3DSparseBundleAdjustmentResult repeated = {}; + CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &repeated) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(equal_results(result, repeated)); + lardon3d_sparse_bundle_adjustment_result_destroy(&repeated); + } + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + + Fixture sparse = fixture(); + bind_fixture(&sparse); + CHECK(lardon3d_sparse_bundle_adjustment_run(&sparse.input, &result) == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK); + CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED); + CHECK(!result.diagnostics[0].eligible); + CHECK(result.diagnostics[0].rejection_reason == + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED); + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + return 0; +} + +template +static bool write_items(FILE *file, const T *items, size_t count) { + return count == 0 || std::fwrite(items, sizeof(T), count, file) == count; +} + +template +static bool read_items(FILE *file, std::vector *items, size_t count) { + items->resize(count); + return count == 0 || std::fread(items->data(), sizeof(T), count, file) == count; +} + +struct SnapshotStorage { + Lardon3DSparseBundleAdjustmentResult result = {}; + std::vector components; + std::vector cameras; + std::vector landmarks; + std::vector observations; + std::vector diagnostics; +}; + +static bool write_snapshot(const char *path, + const Lardon3DSparseBundleAdjustmentResult &result) { + FILE *file = std::fopen(path, "wb"); + if (!file) return false; + const char header[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'}; + const bool ok = write_items(file, header, sizeof(header)) && + write_items(file, &result.status, 1) && + write_items(file, &result.component_count, 1) && + write_items(file, &result.camera_count, 1) && + write_items(file, &result.landmark_count, 1) && + write_items(file, &result.observation_count, 1) && + write_items(file, result.components, result.component_count) && + write_items(file, result.cameras, result.camera_count) && + write_items(file, result.landmarks, result.landmark_count) && + write_items(file, result.observations, result.observation_count) && + write_items(file, result.diagnostics, result.component_count); + return std::fclose(file) == 0 && ok; +} + +static bool read_snapshot(const char *path, SnapshotStorage *snapshot) { + FILE *file = std::fopen(path, "rb"); + if (!file) return false; + char header[8] = {}; + const char expected[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'}; + bool ok = read_items(file, &snapshot->components, 0) && + std::fread(header, 1, sizeof(header), file) == sizeof(header) && + std::memcmp(header, expected, sizeof(header)) == 0 && + std::fread(&snapshot->result.status, sizeof(snapshot->result.status), 1, + file) == 1 && + std::fread(&snapshot->result.component_count, + sizeof(snapshot->result.component_count), 1, file) == 1 && + std::fread(&snapshot->result.camera_count, + sizeof(snapshot->result.camera_count), 1, file) == 1 && + std::fread(&snapshot->result.landmark_count, + sizeof(snapshot->result.landmark_count), 1, file) == 1 && + std::fread(&snapshot->result.observation_count, + sizeof(snapshot->result.observation_count), 1, file) == 1; + if (ok && (snapshot->result.component_count > 4096 || + snapshot->result.camera_count > 4096 || + snapshot->result.landmark_count > 250000 || + snapshot->result.observation_count > 1000000)) + ok = false; + if (ok) + ok = read_items(file, &snapshot->components, snapshot->result.component_count) && + read_items(file, &snapshot->cameras, snapshot->result.camera_count) && + read_items(file, &snapshot->landmarks, snapshot->result.landmark_count) && + read_items(file, &snapshot->observations, + snapshot->result.observation_count) && + read_items(file, &snapshot->diagnostics, snapshot->result.component_count); + if (ok) { + snapshot->result.components = snapshot->components.data(); + snapshot->result.cameras = snapshot->cameras.data(); + snapshot->result.landmarks = snapshot->landmarks.data(); + snapshot->result.observations = snapshot->observations.data(); + snapshot->result.diagnostics = snapshot->diagnostics.data(); + ok = std::fgetc(file) == EOF; + } + return std::fclose(file) == 0 && ok; +} + +static int snapshot_mode(const char *write_path, const char *compare_path) { + auto value = scientific_fixture(true, true); + bind_scientific_fixture(&value); + Lardon3DSparseBundleAdjustmentResult result = {}; + if (lardon3d_sparse_bundle_adjustment_run(&value.input, &result) != + LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK) + return 1; + bool ok = false; + if (write_path) { + ok = write_snapshot(write_path, result); + } else { + SnapshotStorage reference; + ok = read_snapshot(compare_path, &reference) && + equal_results(reference.result, result); + } + lardon3d_sparse_bundle_adjustment_result_destroy(&result); + return ok ? 0 : 1; +} + +int main(int argc, char **argv) { + if (argc == 3 && std::strcmp(argv[1], "--write-snapshot") == 0) + return snapshot_mode(argv[2], nullptr); + if (argc == 3 && std::strcmp(argv[1], "--compare-snapshot") == 0) + return snapshot_mode(nullptr, argv[2]); if (test_helpers() != 0) return 1; if (test_preparation() != 0) return 1; if (test_invalid() != 0) return 1; - return test_multiple_components(); + if (test_multiple_components() != 0) return 1; + if (test_underconstraint_rules() != 0) return 1; + return test_execution_and_solver(); }