secure commit
This commit is contained in:
fy59
parent
af7ecdf466
commit
fa58c8a225
10 changed files with 1854 additions and 35 deletions
11
README.md
11
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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é,
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -1,8 +1,16 @@
|
|||
#include <lardon3d/sparse_sfm_bundle_adjustment.h>
|
||||
|
||||
#include <ceres/ceres.h>
|
||||
#include <ceres/rotation.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <new>
|
||||
#include <numeric>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -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<size_t> cameras;
|
||||
std::vector<size_t> landmarks;
|
||||
std::vector<size_t> observations;
|
||||
};
|
||||
|
||||
struct SolverCamera {
|
||||
size_t source_index;
|
||||
size_t image_index;
|
||||
double quaternion[4];
|
||||
double center[3];
|
||||
double initial_center[3];
|
||||
};
|
||||
|
||||
struct SolverLandmark {
|
||||
size_t source_index;
|
||||
double point[3];
|
||||
};
|
||||
|
||||
struct SolverObservation {
|
||||
size_t camera_index;
|
||||
size_t landmark_index;
|
||||
size_t resolved_index;
|
||||
};
|
||||
|
||||
struct UnderconstraintResult {
|
||||
bool valid;
|
||||
uint32_t mask;
|
||||
};
|
||||
enum class UnderconstraintAction { proceed, reject, internal_error };
|
||||
|
||||
struct CandidateDecision {
|
||||
bool accepted;
|
||||
bool has_metrics;
|
||||
Lardon3DSparseBundleAdjustmentRejectionReason rejection_reason;
|
||||
};
|
||||
|
||||
enum class ParameterKind : int { landmark = 0, camera_quaternion = 1, camera_center = 2 };
|
||||
struct ParameterRecord {
|
||||
ParameterKind kind;
|
||||
uint64_t identity;
|
||||
int group;
|
||||
bool constant;
|
||||
int subset_axis;
|
||||
};
|
||||
|
||||
struct DisjointSet {
|
||||
std::vector<size_t> parent;
|
||||
|
||||
explicit DisjointSet(size_t count) : parent(count) {
|
||||
std::iota(parent.begin(), parent.end(), 0);
|
||||
}
|
||||
|
||||
size_t find(size_t value) {
|
||||
while (parent[value] != value) {
|
||||
parent[value] = parent[parent[value]];
|
||||
value = parent[value];
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
void join(size_t a, size_t b) {
|
||||
a = find(a);
|
||||
b = find(b);
|
||||
if (a != b) parent[std::max(a, b)] = std::min(a, b);
|
||||
}
|
||||
};
|
||||
|
||||
bool finite_calibration(const Lardon3DSparseGeometryCalibration &value) {
|
||||
return value.width > 0 && value.height > 0 && std::isfinite(value.fx) &&
|
||||
std::isfinite(value.fy) && value.fx > 0.0 && value.fy > 0.0 &&
|
||||
|
|
@ -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 <typename T>
|
||||
bool operator()(const T *quaternion, const T *center, const T *point,
|
||||
T *residual) const {
|
||||
const T relative[3] = {point[0] - center[0], point[1] - center[1],
|
||||
point[2] - center[2]};
|
||||
T camera[3];
|
||||
ceres::QuaternionRotatePoint(quaternion, relative, camera);
|
||||
if (camera[2] <= T(depth_epsilon)) return false;
|
||||
const T xn = camera[0] / camera[2];
|
||||
const T yn = camera[1] / camera[2];
|
||||
const T r2 = xn * xn + yn * yn;
|
||||
const T radial = T(1.0) + T(calibration.k1) * r2 +
|
||||
T(calibration.k2) * r2 * r2;
|
||||
const T xd = xn * radial + T(2.0 * calibration.p1) * xn * yn +
|
||||
T(calibration.p2) * (r2 + T(2.0) * xn * xn);
|
||||
const T yd = yn * radial + T(calibration.p1) * (r2 + T(2.0) * yn * yn) +
|
||||
T(2.0 * calibration.p2) * xn * yn;
|
||||
residual[0] = T(calibration.fx) * xd + T(calibration.cx - observed_x);
|
||||
residual[1] = T(calibration.fy) * yd + T(calibration.cy - observed_y);
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
bool select_anchors(const std::vector<Lardon3DSparseIncrementalCamera> &cameras,
|
||||
uint64_t component_key,
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic) {
|
||||
|
|
@ -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<ComponentView> *views) {
|
||||
if (!views) return false;
|
||||
views->clear();
|
||||
views->resize(preparation.components.size());
|
||||
for (size_t index = 0; index < preparation.components.size(); ++index) {
|
||||
(*views)[index].cameras.reserve(
|
||||
static_cast<size_t>(preparation.components[index].registered_image_count));
|
||||
(*views)[index].landmarks.reserve(
|
||||
static_cast<size_t>(preparation.components[index].landmark_count));
|
||||
(*views)[index].observations.reserve(
|
||||
static_cast<size_t>(preparation.diagnostics[index].observation_count));
|
||||
}
|
||||
for (size_t index = 0; index < preparation.cameras.size(); ++index) {
|
||||
auto component = std::lower_bound(
|
||||
preparation.components.begin(), preparation.components.end(),
|
||||
preparation.cameras[index].component_key,
|
||||
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
||||
if (component == preparation.components.end() ||
|
||||
component->component_key != preparation.cameras[index].component_key)
|
||||
return false;
|
||||
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
||||
.cameras.push_back(index);
|
||||
}
|
||||
for (size_t index = 0; index < preparation.landmarks.size(); ++index) {
|
||||
auto component = std::lower_bound(
|
||||
preparation.components.begin(), preparation.components.end(),
|
||||
preparation.landmarks[index].component_key,
|
||||
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
||||
if (component == preparation.components.end() ||
|
||||
component->component_key != preparation.landmarks[index].component_key)
|
||||
return false;
|
||||
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
||||
.landmarks.push_back(index);
|
||||
}
|
||||
for (size_t index = 0; index < preparation.observations.size(); ++index) {
|
||||
auto landmark = std::lower_bound(
|
||||
preparation.landmarks.begin(), preparation.landmarks.end(),
|
||||
preparation.observations[index].track_id,
|
||||
[](const auto &value, uint64_t key) { return value.track_id < key; });
|
||||
if (landmark == preparation.landmarks.end() ||
|
||||
landmark->track_id != preparation.observations[index].track_id)
|
||||
return false;
|
||||
auto component = std::lower_bound(
|
||||
preparation.components.begin(), preparation.components.end(),
|
||||
landmark->component_key,
|
||||
[](const auto &value, uint64_t key) { return value.component_key < key; });
|
||||
if (component == preparation.components.end() ||
|
||||
component->component_key != landmark->component_key)
|
||||
return false;
|
||||
(*views)[static_cast<size_t>(component - preparation.components.begin())]
|
||||
.observations.push_back(index);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t structural_underconstraint_mask(
|
||||
size_t camera_count, size_t landmark_count, size_t observation_count,
|
||||
size_t pose_anchor, std::vector<std::pair<size_t, size_t>> edges) {
|
||||
constexpr uint32_t uc1 = 1U << 0;
|
||||
constexpr uint32_t uc2 = 1U << 1;
|
||||
constexpr uint32_t uc3 = 1U << 2;
|
||||
constexpr uint32_t uc4 = 1U << 3;
|
||||
uint32_t mask = 0;
|
||||
const uint64_t cameras = camera_count;
|
||||
const uint64_t landmarks = landmark_count;
|
||||
const uint64_t observations = observation_count;
|
||||
if (cameras > (UINT64_MAX - 7) / 6 || landmarks > UINT64_MAX / 3 ||
|
||||
observations > UINT64_MAX / 2) {
|
||||
mask |= uc1;
|
||||
} else {
|
||||
const uint64_t camera_dof = 6 * cameras;
|
||||
const uint64_t landmark_dof = 3 * landmarks;
|
||||
if (camera_dof > UINT64_MAX - landmark_dof ||
|
||||
camera_dof + landmark_dof < 7 ||
|
||||
2 * observations < camera_dof + landmark_dof - 7)
|
||||
mask |= uc1;
|
||||
}
|
||||
std::sort(edges.begin(), edges.end());
|
||||
edges.erase(std::unique(edges.begin(), edges.end()), edges.end());
|
||||
std::vector<size_t> camera_support(camera_count, 0);
|
||||
std::vector<size_t> landmark_support(landmark_count, 0);
|
||||
DisjointSet graph(camera_count + landmark_count);
|
||||
for (const auto &[camera, landmark] : edges) {
|
||||
if (camera >= camera_count || landmark >= landmark_count)
|
||||
return uc1 | uc2 | uc3 | uc4;
|
||||
++camera_support[camera];
|
||||
++landmark_support[landmark];
|
||||
graph.join(camera, camera_count + landmark);
|
||||
}
|
||||
for (size_t support : landmark_support)
|
||||
if (support < 2) mask |= uc2;
|
||||
if (pose_anchor >= camera_count) return uc1 | uc2 | uc3 | uc4;
|
||||
for (size_t index = 0; index < camera_support.size(); ++index)
|
||||
if (index != pose_anchor && camera_support[index] < 3) mask |= uc3;
|
||||
const size_t root = graph.find(pose_anchor);
|
||||
for (size_t index = 0; index < graph.parent.size(); ++index)
|
||||
if (graph.find(index) != root) mask |= uc4;
|
||||
return mask;
|
||||
}
|
||||
|
||||
UnderconstraintResult underconstraint_result(
|
||||
const Preparation &preparation, const ComponentView &view,
|
||||
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic) {
|
||||
std::vector<std::pair<size_t, size_t>> edges;
|
||||
edges.reserve(view.observations.size());
|
||||
for (size_t observation_index : view.observations) {
|
||||
const auto &observation = preparation.observations[observation_index];
|
||||
auto camera = std::lower_bound(
|
||||
view.cameras.begin(), view.cameras.end(), observation.image_id,
|
||||
[&](size_t index, uint64_t key) {
|
||||
return preparation.cameras[index].image_id < key;
|
||||
});
|
||||
auto landmark = std::lower_bound(
|
||||
view.landmarks.begin(), view.landmarks.end(), observation.track_id,
|
||||
[&](size_t index, uint64_t key) {
|
||||
return preparation.landmarks[index].track_id < key;
|
||||
});
|
||||
if (camera == view.cameras.end() || landmark == view.landmarks.end() ||
|
||||
preparation.cameras[*camera].image_id != observation.image_id ||
|
||||
preparation.landmarks[*landmark].track_id != observation.track_id)
|
||||
return {false, 0};
|
||||
edges.emplace_back(static_cast<size_t>(camera - view.cameras.begin()),
|
||||
static_cast<size_t>(landmark - view.landmarks.begin()));
|
||||
}
|
||||
size_t pose_anchor = view.cameras.size();
|
||||
for (size_t index = 0; index < view.cameras.size(); ++index)
|
||||
if (preparation.cameras[view.cameras[index]].image_id ==
|
||||
diagnostic.pose_anchor_image_id)
|
||||
pose_anchor = index;
|
||||
if (pose_anchor == view.cameras.size()) return {false, 0};
|
||||
return {true, structural_underconstraint_mask(
|
||||
view.cameras.size(), view.landmarks.size(),
|
||||
view.observations.size(), pose_anchor, std::move(edges))};
|
||||
}
|
||||
|
||||
UnderconstraintAction classify_underconstraint(const UnderconstraintResult &result) {
|
||||
if (!result.valid) return UnderconstraintAction::internal_error;
|
||||
return result.mask == 0 ? UnderconstraintAction::proceed
|
||||
: UnderconstraintAction::reject;
|
||||
}
|
||||
|
||||
bool public_counts_valid(size_t image_count, size_t camera_count,
|
||||
size_t landmark_count, size_t result_observation_count,
|
||||
size_t input_observation_count) {
|
||||
if (image_count > maximum_images || camera_count > maximum_images ||
|
||||
landmark_count > maximum_landmarks ||
|
||||
result_observation_count > maximum_observations ||
|
||||
input_observation_count > maximum_observations)
|
||||
return false;
|
||||
return result_observation_count <= SIZE_MAX / 2 &&
|
||||
result_observation_count <= SIZE_MAX / sizeof(double) / 2;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void copy_view(std::vector<T> *destination, const T *source, size_t count) {
|
||||
destination->clear();
|
||||
|
|
@ -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<Lardon3DSparseIncrementalCamera> &cameras,
|
||||
const std::vector<Lardon3DSparseIncrementalLandmark> &landmarks,
|
||||
double *rmse, double *cost) {
|
||||
std::vector<double> residuals;
|
||||
if (view.observations.size() > SIZE_MAX / 2) return false;
|
||||
residuals.resize(view.observations.size() * 2);
|
||||
for (size_t local = 0; local < view.observations.size(); ++local) {
|
||||
const size_t observation_index = view.observations[local];
|
||||
const auto &observation = preparation.observations[observation_index];
|
||||
auto camera = std::lower_bound(
|
||||
cameras.begin(), cameras.end(), observation.image_id,
|
||||
[](const auto &value, uint64_t key) { return value.image_id < key; });
|
||||
auto landmark = std::lower_bound(
|
||||
landmarks.begin(), landmarks.end(), observation.track_id,
|
||||
[](const auto &value, uint64_t key) { return value.track_id < key; });
|
||||
if (camera == cameras.end() || camera->image_id != observation.image_id ||
|
||||
landmark == landmarks.end() || landmark->track_id != observation.track_id)
|
||||
return false;
|
||||
const auto &resolved = preparation.resolved_observations[observation_index];
|
||||
Lardon3DSparseGeometryPoint2 predicted = {};
|
||||
if (!project(preparation.images[resolved.image_index].calibration,
|
||||
camera->pose_cw, landmark->point, &predicted))
|
||||
return false;
|
||||
residuals[local * 2] = predicted.x - resolved.source.x;
|
||||
residuals[local * 2 + 1] = predicted.y - resolved.source.y;
|
||||
}
|
||||
return residual_metrics(residuals.data(), view.observations.size(), rmse, cost);
|
||||
}
|
||||
|
||||
bool build_public_candidate(
|
||||
const Preparation &preparation,
|
||||
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic,
|
||||
const std::vector<SolverCamera> &cameras,
|
||||
const std::vector<SolverLandmark> &landmarks,
|
||||
std::vector<Lardon3DSparseIncrementalCamera> *candidate_cameras,
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> *candidate_landmarks) {
|
||||
if (!candidate_cameras || !candidate_landmarks) return false;
|
||||
*candidate_cameras = preparation.cameras;
|
||||
*candidate_landmarks = preparation.landmarks;
|
||||
for (size_t index = 0; index < cameras.size(); ++index) {
|
||||
const size_t source_index = cameras[index].source_index;
|
||||
if (preparation.cameras[source_index].image_id ==
|
||||
diagnostic.pose_anchor_image_id)
|
||||
continue;
|
||||
if (!quaternion_to_pose(
|
||||
cameras[index].quaternion, cameras[index].center,
|
||||
&(*candidate_cameras)[source_index].pose_cw))
|
||||
return false;
|
||||
}
|
||||
for (const auto &landmark : landmarks) {
|
||||
const Lardon3DSparseGeometryPoint3 point = {
|
||||
landmark.point[0], landmark.point[1], landmark.point[2]};
|
||||
if (!finite_point(point)) return false;
|
||||
(*candidate_landmarks)[landmark.source_index].point = point;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
PrivateTermination translate_termination(ceres::TerminationType value) {
|
||||
if (value == ceres::CONVERGENCE) return PrivateTermination::converged;
|
||||
if (value == ceres::NO_CONVERGENCE) return PrivateTermination::no_convergence;
|
||||
return PrivateTermination::failure;
|
||||
}
|
||||
|
||||
Lardon3DSparseBundleAdjustmentTermination public_termination(
|
||||
PrivateTermination value) {
|
||||
if (value == PrivateTermination::converged)
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED;
|
||||
if (value == PrivateTermination::no_convergence)
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE;
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_FAILURE;
|
||||
}
|
||||
|
||||
CandidateDecision validate_candidate(PrivateTermination termination,
|
||||
bool final_metrics_valid,
|
||||
bool gauge_valid,
|
||||
double initial_cost,
|
||||
double final_cost) {
|
||||
if (!termination_accepted(termination)) {
|
||||
return {false, false,
|
||||
termination == PrivateTermination::no_convergence
|
||||
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE
|
||||
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE};
|
||||
}
|
||||
if (!final_metrics_valid || !gauge_valid)
|
||||
return {false, false,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE};
|
||||
if (!cost_acceptable(initial_cost, final_cost))
|
||||
return {false, true,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION};
|
||||
return {true, true, LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE};
|
||||
}
|
||||
|
||||
void apply_component_decision(
|
||||
const CandidateDecision &decision, const ComponentView &view,
|
||||
const std::vector<Lardon3DSparseIncrementalCamera> &candidate_cameras,
|
||||
const std::vector<Lardon3DSparseIncrementalLandmark> &candidate_landmarks,
|
||||
std::vector<Lardon3DSparseIncrementalCamera> *published_cameras,
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> *published_landmarks) {
|
||||
if (!decision.accepted) return;
|
||||
for (size_t source_index : view.cameras)
|
||||
(*published_cameras)[source_index] = candidate_cameras[source_index];
|
||||
for (size_t source_index : view.landmarks)
|
||||
(*published_landmarks)[source_index] = candidate_landmarks[source_index];
|
||||
}
|
||||
|
||||
bool configure_parameter_blocks(
|
||||
ceres::Problem *problem, ceres::ParameterBlockOrdering *ordering,
|
||||
const Preparation &preparation,
|
||||
const Lardon3DSparseBundleAdjustmentComponentDiagnostic &diagnostic,
|
||||
std::vector<SolverCamera> *cameras,
|
||||
std::vector<SolverLandmark> *landmarks,
|
||||
std::vector<ParameterRecord> *records) {
|
||||
if (!problem || !ordering || !cameras || !landmarks) return false;
|
||||
if (records) records->clear();
|
||||
for (auto &landmark : *landmarks) {
|
||||
problem->AddParameterBlock(landmark.point, 3);
|
||||
ordering->AddElementToGroup(landmark.point, 0);
|
||||
if (records)
|
||||
records->push_back({ParameterKind::landmark,
|
||||
preparation.landmarks[landmark.source_index].track_id,
|
||||
0, false, -1});
|
||||
}
|
||||
for (auto &camera : *cameras) {
|
||||
const uint64_t image_id = preparation.cameras[camera.source_index].image_id;
|
||||
problem->AddParameterBlock(camera.quaternion, 4,
|
||||
new ceres::QuaternionManifold);
|
||||
problem->AddParameterBlock(camera.center, 3);
|
||||
ordering->AddElementToGroup(camera.quaternion, 1);
|
||||
ordering->AddElementToGroup(camera.center, 1);
|
||||
if (records) {
|
||||
records->push_back({ParameterKind::camera_quaternion, image_id, 1,
|
||||
image_id == diagnostic.pose_anchor_image_id, -1});
|
||||
records->push_back({ParameterKind::camera_center, image_id, 1,
|
||||
image_id == diagnostic.pose_anchor_image_id,
|
||||
image_id == diagnostic.scale_anchor_image_id
|
||||
? static_cast<int>(diagnostic.scale_axis) - 1
|
||||
: -1});
|
||||
}
|
||||
}
|
||||
auto pose_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) {
|
||||
return preparation.cameras[camera.source_index].image_id ==
|
||||
diagnostic.pose_anchor_image_id;
|
||||
});
|
||||
auto scale_anchor = std::find_if(cameras->begin(), cameras->end(), [&](const auto &camera) {
|
||||
return preparation.cameras[camera.source_index].image_id ==
|
||||
diagnostic.scale_anchor_image_id;
|
||||
});
|
||||
if (pose_anchor == cameras->end() || scale_anchor == cameras->end()) return false;
|
||||
problem->SetParameterBlockConstant(pose_anchor->quaternion);
|
||||
problem->SetParameterBlockConstant(pose_anchor->center);
|
||||
const int scale_axis = static_cast<int>(diagnostic.scale_axis) - 1;
|
||||
problem->SetManifold(scale_anchor->center,
|
||||
new ceres::SubsetManifold(3, {scale_axis}));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool solve_component(
|
||||
const Preparation &preparation, const ComponentView &view,
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic,
|
||||
std::vector<Lardon3DSparseIncrementalCamera> *published_cameras,
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> *published_landmarks) {
|
||||
if (!diagnostic || !published_cameras || !published_landmarks) return false;
|
||||
std::vector<SolverCamera> cameras;
|
||||
std::vector<SolverLandmark> landmarks;
|
||||
std::vector<SolverObservation> observations;
|
||||
cameras.reserve(view.cameras.size());
|
||||
landmarks.reserve(view.landmarks.size());
|
||||
observations.reserve(view.observations.size());
|
||||
|
||||
for (size_t source_index : view.cameras) {
|
||||
const auto &source = preparation.cameras[source_index];
|
||||
auto image = std::lower_bound(
|
||||
preparation.images.begin(), preparation.images.end(), source.image_id,
|
||||
[](const auto &value, uint64_t key) { return value.image_id < key; });
|
||||
SolverCamera camera = {};
|
||||
camera.source_index = source_index;
|
||||
camera.image_index = static_cast<size_t>(image - preparation.images.begin());
|
||||
if (image == preparation.images.end() || image->image_id != source.image_id ||
|
||||
!rotation_to_quaternion(source.pose_cw.rotation_cw, camera.quaternion) ||
|
||||
!camera_center(source.pose_cw, camera.center))
|
||||
return false;
|
||||
std::copy(camera.center, camera.center + 3, camera.initial_center);
|
||||
cameras.push_back(camera);
|
||||
}
|
||||
for (size_t source_index : view.landmarks) {
|
||||
const auto &source = preparation.landmarks[source_index];
|
||||
landmarks.push_back(
|
||||
{source_index, {source.point.x, source.point.y, source.point.z}});
|
||||
}
|
||||
for (size_t resolved_index : view.observations) {
|
||||
const auto &source = preparation.observations[resolved_index];
|
||||
auto camera = std::lower_bound(
|
||||
cameras.begin(), cameras.end(), source.image_id,
|
||||
[&](const auto &value, uint64_t key) {
|
||||
return preparation.cameras[value.source_index].image_id < key;
|
||||
});
|
||||
auto landmark = std::lower_bound(
|
||||
landmarks.begin(), landmarks.end(), source.track_id,
|
||||
[&](const auto &value, uint64_t key) {
|
||||
return preparation.landmarks[value.source_index].track_id < key;
|
||||
});
|
||||
if (camera == cameras.end() || landmark == landmarks.end()) return false;
|
||||
observations.push_back({static_cast<size_t>(camera - cameras.begin()),
|
||||
static_cast<size_t>(landmark - landmarks.begin()),
|
||||
resolved_index});
|
||||
}
|
||||
|
||||
if (!component_metrics(preparation, view, preparation.cameras,
|
||||
preparation.landmarks,
|
||||
&diagnostic->initial_reprojection_rmse_px,
|
||||
&diagnostic->initial_robust_cost)) {
|
||||
diagnostic->rejection_reason =
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE;
|
||||
return true;
|
||||
}
|
||||
ceres::Problem problem;
|
||||
auto ordering = std::make_shared<ceres::ParameterBlockOrdering>();
|
||||
if (!configure_parameter_blocks(&problem, ordering.get(), preparation,
|
||||
*diagnostic, &cameras, &landmarks, nullptr))
|
||||
return false;
|
||||
|
||||
auto *loss = new ceres::HuberLoss(2.0);
|
||||
for (const auto &observation : observations) {
|
||||
const auto &resolved = preparation.resolved_observations[observation.resolved_index];
|
||||
const auto &calibration = preparation.images[cameras[observation.camera_index]
|
||||
.image_index].calibration;
|
||||
auto *cost = new ceres::AutoDiffCostFunction<ReprojectionResidual, 2, 4, 3, 3>(
|
||||
new ReprojectionResidual{calibration, resolved.source.x, resolved.source.y});
|
||||
problem.AddResidualBlock(cost, loss,
|
||||
cameras[observation.camera_index].quaternion,
|
||||
cameras[observation.camera_index].center,
|
||||
landmarks[observation.landmark_index].point);
|
||||
}
|
||||
|
||||
auto pose_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) {
|
||||
return preparation.cameras[camera.source_index].image_id ==
|
||||
diagnostic->pose_anchor_image_id;
|
||||
});
|
||||
auto scale_anchor = std::find_if(cameras.begin(), cameras.end(), [&](const auto &camera) {
|
||||
return preparation.cameras[camera.source_index].image_id ==
|
||||
diagnostic->scale_anchor_image_id;
|
||||
});
|
||||
if (pose_anchor == cameras.end() || scale_anchor == cameras.end()) return false;
|
||||
const int scale_axis = static_cast<int>(diagnostic->scale_axis) - 1;
|
||||
|
||||
ceres::Solver::Options options;
|
||||
options.minimizer_type = ceres::TRUST_REGION;
|
||||
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
|
||||
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
|
||||
options.preconditioner_type = ceres::SCHUR_JACOBI;
|
||||
options.num_threads = 1;
|
||||
options.max_num_iterations = 50;
|
||||
options.function_tolerance = 1e-6;
|
||||
options.gradient_tolerance = 1e-10;
|
||||
options.parameter_tolerance = 1e-8;
|
||||
options.linear_solver_ordering = std::move(ordering);
|
||||
ceres::Solver::Summary summary;
|
||||
ceres::Solve(options, &problem, &summary);
|
||||
|
||||
const PrivateTermination termination = translate_termination(summary.termination_type);
|
||||
diagnostic->termination = public_termination(termination);
|
||||
diagnostic->iteration_count = summary.iterations.size() > UINT32_MAX
|
||||
? UINT32_MAX
|
||||
: static_cast<uint32_t>(summary.iterations.size());
|
||||
std::vector<Lardon3DSparseIncrementalCamera> candidate_cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> candidate_landmarks;
|
||||
bool candidate_valid = false;
|
||||
if (termination_accepted(termination) &&
|
||||
build_public_candidate(preparation, *diagnostic, cameras, landmarks,
|
||||
&candidate_cameras, &candidate_landmarks)) {
|
||||
candidate_valid = component_metrics(
|
||||
preparation, view, candidate_cameras, candidate_landmarks,
|
||||
&diagnostic->final_reprojection_rmse_px,
|
||||
&diagnostic->final_robust_cost);
|
||||
}
|
||||
const bool gauge_valid =
|
||||
scale_anchor->center[scale_axis] == scale_anchor->initial_center[scale_axis];
|
||||
const CandidateDecision decision = validate_candidate(
|
||||
termination, candidate_valid, gauge_valid, diagnostic->initial_robust_cost,
|
||||
diagnostic->final_robust_cost);
|
||||
diagnostic->has_costs = decision.has_metrics;
|
||||
diagnostic->has_rmse = decision.has_metrics;
|
||||
diagnostic->accepted = decision.accepted;
|
||||
diagnostic->rejection_reason = decision.rejection_reason;
|
||||
apply_component_decision(decision, view, candidate_cameras,
|
||||
candidate_landmarks, published_cameras,
|
||||
published_landmarks);
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool allocate_copy(const std::vector<T> &source, T **destination) {
|
||||
*destination = nullptr;
|
||||
if (source.empty()) return true;
|
||||
if (source.size() > SIZE_MAX / sizeof(T)) return false;
|
||||
*destination = static_cast<T *>(std::malloc(source.size() * sizeof(T)));
|
||||
if (!*destination) return false;
|
||||
std::copy(source.begin(), source.end(), *destination);
|
||||
return true;
|
||||
}
|
||||
|
||||
void destroy_result(Lardon3DSparseBundleAdjustmentResult *result) {
|
||||
if (!result) return;
|
||||
std::free(result->components);
|
||||
std::free(result->cameras);
|
||||
std::free(result->landmarks);
|
||||
std::free(result->observations);
|
||||
std::free(result->diagnostics);
|
||||
*result = {};
|
||||
}
|
||||
|
||||
} // namespace lardon3d::sparse_bundle_adjustment
|
||||
|
||||
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<ComponentView> views;
|
||||
if (!build_component_views(preparation, &views))
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
||||
std::vector<Lardon3DSparseIncrementalCamera> published_cameras =
|
||||
preparation.cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> published_landmarks =
|
||||
preparation.landmarks;
|
||||
size_t accepted = 0;
|
||||
size_t rejected_eligible = 0;
|
||||
for (size_t index = 0; index < views.size(); ++index) {
|
||||
auto &diagnostic = preparation.diagnostics[index];
|
||||
if (!diagnostic.eligible) continue;
|
||||
const UnderconstraintResult underconstraint =
|
||||
underconstraint_result(preparation, views[index], diagnostic);
|
||||
const UnderconstraintAction underconstraint_action =
|
||||
classify_underconstraint(underconstraint);
|
||||
if (underconstraint_action == UnderconstraintAction::internal_error)
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
||||
if (underconstraint_action == UnderconstraintAction::reject) {
|
||||
diagnostic.eligible = false;
|
||||
diagnostic.rejection_reason =
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_UNDERCONSTRAINED;
|
||||
continue;
|
||||
}
|
||||
if (!solve_component(preparation, views[index], &diagnostic,
|
||||
&published_cameras, &published_landmarks))
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
||||
if (diagnostic.accepted)
|
||||
++accepted;
|
||||
else
|
||||
++rejected_eligible;
|
||||
}
|
||||
|
||||
Lardon3DSparseBundleAdjustmentResult candidate = {};
|
||||
candidate.status = accepted == 0
|
||||
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED
|
||||
: rejected_eligible == 0
|
||||
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE
|
||||
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL;
|
||||
candidate.component_count = preparation.components.size();
|
||||
candidate.camera_count = published_cameras.size();
|
||||
candidate.landmark_count = published_landmarks.size();
|
||||
candidate.observation_count = preparation.observations.size();
|
||||
if (!allocate_copy(preparation.components, &candidate.components) ||
|
||||
!allocate_copy(published_cameras, &candidate.cameras) ||
|
||||
!allocate_copy(published_landmarks, &candidate.landmarks) ||
|
||||
!allocate_copy(preparation.observations, &candidate.observations) ||
|
||||
!allocate_copy(preparation.diagnostics, &candidate.diagnostics)) {
|
||||
destroy_result(&candidate);
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY;
|
||||
}
|
||||
*result = candidate;
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK;
|
||||
} catch (const std::bad_alloc &) {
|
||||
destroy_result(result);
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OUT_OF_MEMORY;
|
||||
} catch (...) {
|
||||
destroy_result(result);
|
||||
return LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" 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<PrivateTermination>(value));
|
||||
}
|
||||
|
||||
extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_underconstraint(
|
||||
const Lardon3DSparseBundleAdjustmentInput *input, size_t component_index) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (!input) return UINT32_MAX;
|
||||
Preparation preparation;
|
||||
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
||||
component_index >= preparation.components.size())
|
||||
return UINT32_MAX;
|
||||
std::vector<ComponentView> views;
|
||||
if (!build_component_views(preparation, &views)) return UINT32_MAX;
|
||||
const UnderconstraintResult result = underconstraint_result(
|
||||
preparation, views[component_index], preparation.diagnostics[component_index]);
|
||||
return result.valid ? result.mask : UINT32_MAX;
|
||||
}
|
||||
|
||||
extern "C" uint32_t lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
||||
size_t camera_count, size_t landmark_count, size_t observation_count,
|
||||
size_t pose_anchor, const size_t *camera_indices,
|
||||
const size_t *landmark_indices, size_t edge_count) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (edge_count != 0 && (!camera_indices || !landmark_indices)) return UINT32_MAX;
|
||||
std::vector<std::pair<size_t, size_t>> edges;
|
||||
edges.reserve(edge_count);
|
||||
for (size_t index = 0; index < edge_count; ++index)
|
||||
edges.emplace_back(camera_indices[index], landmark_indices[index]);
|
||||
return structural_underconstraint_mask(camera_count, landmark_count,
|
||||
observation_count, pose_anchor,
|
||||
std::move(edges));
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_decision(
|
||||
int termination, bool final_metrics_valid, bool gauge_valid,
|
||||
double initial_cost, double final_cost, bool *has_metrics,
|
||||
Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (termination < 0 || termination > 2 || !has_metrics || !reason) return false;
|
||||
const CandidateDecision decision = validate_candidate(
|
||||
static_cast<PrivateTermination>(termination), final_metrics_valid,
|
||||
gauge_valid, initial_cost, final_cost);
|
||||
*has_metrics = decision.has_metrics;
|
||||
*reason = decision.rejection_reason;
|
||||
return decision.accepted;
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_counts_valid(
|
||||
size_t image_count, size_t camera_count, size_t landmark_count,
|
||||
size_t result_observation_count, size_t input_observation_count) {
|
||||
return lardon3d::sparse_bundle_adjustment::public_counts_valid(
|
||||
image_count, camera_count, landmark_count, result_observation_count,
|
||||
input_observation_count);
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_candidate_publication(
|
||||
bool final_metrics_valid, double initial_cost, double final_cost,
|
||||
double original_x, double candidate_x, double *published_x,
|
||||
bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (!published_x || !has_metrics || !reason) return false;
|
||||
const CandidateDecision decision = validate_candidate(
|
||||
PrivateTermination::converged, final_metrics_valid, true, initial_cost,
|
||||
final_cost);
|
||||
ComponentView view;
|
||||
view.landmarks.push_back(0);
|
||||
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> published(1);
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> candidate(1);
|
||||
published[0].point.x = original_x;
|
||||
candidate[0].point.x = candidate_x;
|
||||
apply_component_decision(decision, view, cameras, candidate, &cameras,
|
||||
&published);
|
||||
*published_x = published[0].point.x;
|
||||
*has_metrics = decision.has_metrics;
|
||||
*reason = decision.rejection_reason;
|
||||
return decision.accepted;
|
||||
}
|
||||
|
||||
extern "C" int lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
Preparation preparation;
|
||||
preparation.cameras.resize(1);
|
||||
preparation.cameras[0].image_id = 10;
|
||||
preparation.landmarks.resize(1);
|
||||
preparation.landmarks[0].track_id = 20;
|
||||
preparation.observations.resize(1);
|
||||
preparation.observations[0].image_id = 99;
|
||||
preparation.observations[0].track_id = 20;
|
||||
ComponentView view;
|
||||
view.cameras.push_back(0);
|
||||
view.landmarks.push_back(0);
|
||||
view.observations.push_back(0);
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostic = {};
|
||||
diagnostic.pose_anchor_image_id = 10;
|
||||
const UnderconstraintAction action = classify_underconstraint(
|
||||
underconstraint_result(preparation, view, diagnostic));
|
||||
return action == UnderconstraintAction::internal_error
|
||||
? LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR
|
||||
: LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK;
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_parameter_ordering(
|
||||
const Lardon3DSparseBundleAdjustmentInput *input, int *kinds,
|
||||
uint64_t *identities, int *groups, bool *constants, int *subset_axes,
|
||||
size_t capacity, size_t *count) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (!input || !count) return false;
|
||||
Preparation preparation;
|
||||
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
||||
preparation.components.size() != 1)
|
||||
return false;
|
||||
std::vector<ComponentView> views;
|
||||
if (!build_component_views(preparation, &views)) return false;
|
||||
std::vector<SolverCamera> cameras;
|
||||
std::vector<SolverLandmark> landmarks;
|
||||
for (size_t source_index : views[0].cameras) {
|
||||
SolverCamera camera = {};
|
||||
camera.source_index = source_index;
|
||||
if (!rotation_to_quaternion(preparation.cameras[source_index].pose_cw.rotation_cw,
|
||||
camera.quaternion) ||
|
||||
!camera_center(preparation.cameras[source_index].pose_cw, camera.center))
|
||||
return false;
|
||||
cameras.push_back(camera);
|
||||
}
|
||||
for (size_t source_index : views[0].landmarks) {
|
||||
const auto &point = preparation.landmarks[source_index].point;
|
||||
landmarks.push_back({source_index, {point.x, point.y, point.z}});
|
||||
}
|
||||
ceres::Problem problem;
|
||||
ceres::ParameterBlockOrdering ordering;
|
||||
std::vector<ParameterRecord> records;
|
||||
if (!configure_parameter_blocks(&problem, &ordering, preparation,
|
||||
preparation.diagnostics[0], &cameras,
|
||||
&landmarks, &records))
|
||||
return false;
|
||||
*count = records.size();
|
||||
if (records.size() > capacity ||
|
||||
(!records.empty() &&
|
||||
(!kinds || !identities || !groups || !constants || !subset_axes)))
|
||||
return false;
|
||||
for (size_t index = 0; index < records.size(); ++index) {
|
||||
kinds[index] = static_cast<int>(records[index].kind);
|
||||
identities[index] = records[index].identity;
|
||||
groups[index] = records[index].group;
|
||||
constants[index] = records[index].constant;
|
||||
subset_axes[index] = records[index].subset_axis;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_invalid_candidate(
|
||||
const Lardon3DSparseBundleAdjustmentInput *input, double *published_z,
|
||||
bool *has_metrics, Lardon3DSparseBundleAdjustmentRejectionReason *reason) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (!input || !published_z || !has_metrics || !reason) return false;
|
||||
Preparation preparation;
|
||||
if (prepare(*input, &preparation) != PreparationStatus::prepared ||
|
||||
preparation.components.size() != 1 || preparation.landmarks.empty())
|
||||
return false;
|
||||
std::vector<ComponentView> views;
|
||||
if (!build_component_views(preparation, &views)) return false;
|
||||
std::vector<Lardon3DSparseIncrementalCamera> candidate_cameras =
|
||||
preparation.cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> candidate_landmarks =
|
||||
preparation.landmarks;
|
||||
candidate_landmarks[views[0].landmarks[0]].point.z = -1.0;
|
||||
double initial_rmse = 0.0;
|
||||
double initial_cost = 0.0;
|
||||
double final_rmse = 0.0;
|
||||
double final_cost = 0.0;
|
||||
if (!component_metrics(preparation, views[0], preparation.cameras,
|
||||
preparation.landmarks, &initial_rmse, &initial_cost))
|
||||
return false;
|
||||
const bool final_valid = component_metrics(
|
||||
preparation, views[0], candidate_cameras, candidate_landmarks,
|
||||
&final_rmse, &final_cost);
|
||||
const CandidateDecision decision = validate_candidate(
|
||||
PrivateTermination::converged, final_valid, true, initial_cost, final_cost);
|
||||
std::vector<Lardon3DSparseIncrementalCamera> published_cameras =
|
||||
preparation.cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> published_landmarks =
|
||||
preparation.landmarks;
|
||||
apply_component_decision(decision, views[0], candidate_cameras,
|
||||
candidate_landmarks, &published_cameras,
|
||||
&published_landmarks);
|
||||
*published_z = published_landmarks[views[0].landmarks[0]].point.z;
|
||||
*has_metrics = decision.has_metrics;
|
||||
*reason = decision.rejection_reason;
|
||||
return !decision.accepted && !final_valid;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,9 +1,12 @@
|
|||
#include <lardon3d/sparse_sfm_bundle_adjustment.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
#define CHECK(value) \
|
||||
do { \
|
||||
|
|
@ -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<size_t>::max(), 0));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_counts_valid(
|
||||
std::numeric_limits<size_t>::max(), 0, 0, 0, 0));
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_internal_underconstraint() ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INTERNAL_ERROR);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -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<size_t> cameras;
|
||||
std::vector<size_t> landmarks;
|
||||
auto complete_edges = [&](size_t camera_count, size_t landmark_count) {
|
||||
cameras.clear();
|
||||
landmarks.clear();
|
||||
for (size_t camera = 0; camera < camera_count; ++camera) {
|
||||
for (size_t landmark = 0; landmark < landmark_count; ++landmark) {
|
||||
cameras.push_back(camera);
|
||||
landmarks.push_back(landmark);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
complete_edges(4, 3);
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
||||
4, 3, cameras.size(), 0, cameras.data(), landmarks.data(),
|
||||
cameras.size()) == 1U);
|
||||
|
||||
complete_edges(3, 6);
|
||||
for (size_t index = cameras.size(); index-- > 0;) {
|
||||
if (landmarks[index] == 0 && cameras[index] != 0) {
|
||||
cameras.erase(cameras.begin() + static_cast<ptrdiff_t>(index));
|
||||
landmarks.erase(landmarks.begin() + static_cast<ptrdiff_t>(index));
|
||||
}
|
||||
}
|
||||
CHECK((lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
||||
3, 6, cameras.size(), 0, cameras.data(), landmarks.data(),
|
||||
cameras.size()) & 2U) != 0);
|
||||
|
||||
complete_edges(3, 10);
|
||||
for (size_t index = cameras.size(); index-- > 0;) {
|
||||
if (cameras[index] == 1 && landmarks[index] >= 2) {
|
||||
cameras.erase(cameras.begin() + static_cast<ptrdiff_t>(index));
|
||||
landmarks.erase(landmarks.begin() + static_cast<ptrdiff_t>(index));
|
||||
}
|
||||
}
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
||||
3, 10, cameras.size(), 0, cameras.data(), landmarks.data(),
|
||||
cameras.size()) == 4U);
|
||||
|
||||
cameras.clear();
|
||||
landmarks.clear();
|
||||
for (size_t group = 0; group < 2; ++group) {
|
||||
for (size_t camera = group * 3; camera < group * 3 + 3; ++camera) {
|
||||
for (size_t landmark = group * 6; landmark < group * 6 + 6; ++landmark) {
|
||||
cameras.push_back(camera);
|
||||
landmarks.push_back(landmark);
|
||||
}
|
||||
}
|
||||
}
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_structural_mask(
|
||||
6, 12, cameras.size(), 0, cameras.data(), landmarks.data(),
|
||||
cameras.size()) == 8U);
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct ScientificFixture {
|
||||
std::vector<Lardon3DSparseIncrementalImage> images;
|
||||
std::vector<Lardon3DSparseIncrementalObservation> input_observations;
|
||||
std::vector<Lardon3DSparseIncrementalComponent> components;
|
||||
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
|
||||
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
|
||||
Lardon3DSparseIncrementalResult incremental = {};
|
||||
Lardon3DSparseBundleAdjustmentInput input = {};
|
||||
};
|
||||
|
||||
static ScientificFixture scientific_fixture(bool perturb_camera,
|
||||
bool perturb_landmarks,
|
||||
size_t landmark_count = 5,
|
||||
bool nontrivial_anchor = false) {
|
||||
ScientificFixture value;
|
||||
const auto intrinsic = calibration();
|
||||
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
const double anchor_angle = 0.2;
|
||||
const double anchor_rotation[9] = {
|
||||
std::cos(anchor_angle), -std::sin(anchor_angle), 0.0,
|
||||
std::sin(anchor_angle), std::cos(anchor_angle), 0.0,
|
||||
0.0, 0.0, 1.0};
|
||||
for (size_t camera = 0; camera < 4; ++camera) {
|
||||
const uint64_t image_id = 10 + camera * 10;
|
||||
value.images.push_back({image_id, intrinsic});
|
||||
Lardon3DSparseIncrementalCamera output = {};
|
||||
output.image_id = image_id;
|
||||
output.component_key = 10;
|
||||
std::memcpy(output.pose_cw.rotation_cw,
|
||||
camera == 0 && nontrivial_anchor ? anchor_rotation : identity,
|
||||
sizeof(identity));
|
||||
output.pose_cw.translation_cw[0] = -static_cast<double>(camera);
|
||||
value.cameras.push_back(output);
|
||||
}
|
||||
if (perturb_camera) value.cameras[1].pose_cw.translation_cw[1] = 0.08;
|
||||
|
||||
for (size_t landmark = 0; landmark < landmark_count; ++landmark) {
|
||||
const uint64_t track_id = landmark + 1;
|
||||
const Lardon3DSparseGeometryPoint3 truth = {
|
||||
-0.6 + 0.25 * static_cast<double>(landmark),
|
||||
-0.3 + 0.12 * static_cast<double>(landmark % 3),
|
||||
4.5 + 0.3 * static_cast<double>(landmark)};
|
||||
auto initial = truth;
|
||||
if (perturb_landmarks) {
|
||||
initial.x += 0.06;
|
||||
initial.y -= 0.04;
|
||||
}
|
||||
value.landmarks.push_back(
|
||||
{track_id, track_id, 10, initial, 0.0, 0.0, 4});
|
||||
for (size_t camera = 0; camera < 4; ++camera) {
|
||||
Lardon3DSparseGeometryPose truth_pose = {};
|
||||
std::memcpy(truth_pose.rotation_cw,
|
||||
camera == 0 && nontrivial_anchor ? anchor_rotation : identity,
|
||||
sizeof(identity));
|
||||
truth_pose.translation_cw[0] = -static_cast<double>(camera);
|
||||
Lardon3DSparseGeometryPoint2 pixel = {};
|
||||
if (!lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&intrinsic, &truth_pose, &truth, &pixel))
|
||||
std::abort();
|
||||
const uint64_t image_id = 10 + camera * 10;
|
||||
const uint64_t feature_set_id = 100 + camera;
|
||||
value.input_observations.push_back(
|
||||
{track_id, image_id, feature_set_id,
|
||||
static_cast<uint32_t>(landmark), 64, pixel.x, pixel.y});
|
||||
value.observations.push_back(
|
||||
{track_id, track_id, image_id, feature_set_id,
|
||||
static_cast<uint32_t>(landmark), static_cast<uint32_t>(camera)});
|
||||
}
|
||||
}
|
||||
value.components.push_back({10, 4, 4, landmark_count});
|
||||
value.incremental.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
|
||||
value.incremental.track_set_id = 77;
|
||||
value.incremental.calibration_scope_id = 88;
|
||||
return value;
|
||||
}
|
||||
|
||||
static void bind_scientific_fixture(ScientificFixture *value) {
|
||||
value->incremental.components = value->components.data();
|
||||
value->incremental.component_count = value->components.size();
|
||||
value->incremental.cameras = value->cameras.data();
|
||||
value->incremental.camera_count = value->cameras.size();
|
||||
value->incremental.landmarks = value->landmarks.data();
|
||||
value->incremental.landmark_count = value->landmarks.size();
|
||||
value->incremental.observations = value->observations.data();
|
||||
value->incremental.observation_count = value->observations.size();
|
||||
value->input = {&value->incremental, value->images.data(), value->images.size(),
|
||||
value->input_observations.data(),
|
||||
value->input_observations.size()};
|
||||
}
|
||||
|
||||
static bool published_metrics(const ScientificFixture &fixture,
|
||||
const Lardon3DSparseBundleAdjustmentResult &result,
|
||||
double *rmse, double *cost) {
|
||||
std::vector<double> residuals(result.observation_count * 2);
|
||||
for (size_t index = 0; index < result.observation_count; ++index) {
|
||||
const auto &association = result.observations[index];
|
||||
auto camera = std::lower_bound(
|
||||
result.cameras, result.cameras + result.camera_count, association.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
auto landmark = std::lower_bound(
|
||||
result.landmarks, result.landmarks + result.landmark_count,
|
||||
association.track_id,
|
||||
[](const auto &item, uint64_t key) { return item.track_id < key; });
|
||||
auto source = std::find_if(
|
||||
fixture.input_observations.begin(), fixture.input_observations.end(),
|
||||
[&](const auto &item) {
|
||||
return item.feature_set_id == association.feature_set_id &&
|
||||
item.feature_index == association.feature_index;
|
||||
});
|
||||
auto image = std::lower_bound(
|
||||
fixture.images.begin(), fixture.images.end(), association.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
if (camera == result.cameras + result.camera_count ||
|
||||
landmark == result.landmarks + result.landmark_count ||
|
||||
source == fixture.input_observations.end() || image == fixture.images.end())
|
||||
return false;
|
||||
Lardon3DSparseGeometryPoint2 pixel = {};
|
||||
if (!lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&image->calibration, &camera->pose_cw, &landmark->point, &pixel))
|
||||
return false;
|
||||
residuals[index * 2] = pixel.x - source->x;
|
||||
residuals[index * 2 + 1] = pixel.y - source->y;
|
||||
}
|
||||
return lardon3d_sparse_bundle_adjustment_test_metrics(
|
||||
residuals.data(), result.observation_count, rmse, cost);
|
||||
}
|
||||
|
||||
static ScientificFixture two_component_fixture(bool reject_second) {
|
||||
ScientificFixture first = scientific_fixture(true, true);
|
||||
ScientificFixture second = scientific_fixture(true, true);
|
||||
for (auto &image : second.images) image.image_id += 100;
|
||||
for (auto &observation : second.input_observations) {
|
||||
observation.track_id += 100;
|
||||
observation.image_id += 100;
|
||||
observation.feature_set_id += 1000;
|
||||
}
|
||||
for (auto &component : second.components) component.component_key += 100;
|
||||
for (auto &camera : second.cameras) {
|
||||
camera.image_id += 100;
|
||||
camera.component_key += 100;
|
||||
}
|
||||
for (auto &landmark : second.landmarks) {
|
||||
landmark.landmark_id += 100;
|
||||
landmark.track_id += 100;
|
||||
landmark.component_key += 100;
|
||||
}
|
||||
if (reject_second) second.landmarks[0].point.z = -1.0;
|
||||
for (auto &observation : second.observations) {
|
||||
observation.landmark_id += 100;
|
||||
observation.track_id += 100;
|
||||
observation.image_id += 100;
|
||||
observation.feature_set_id += 1000;
|
||||
}
|
||||
first.images.insert(first.images.end(), second.images.begin(), second.images.end());
|
||||
first.input_observations.insert(first.input_observations.end(),
|
||||
second.input_observations.begin(),
|
||||
second.input_observations.end());
|
||||
first.components.insert(first.components.end(), second.components.begin(),
|
||||
second.components.end());
|
||||
first.cameras.insert(first.cameras.end(), second.cameras.begin(),
|
||||
second.cameras.end());
|
||||
first.landmarks.insert(first.landmarks.end(), second.landmarks.begin(),
|
||||
second.landmarks.end());
|
||||
first.observations.insert(first.observations.end(), second.observations.begin(),
|
||||
second.observations.end());
|
||||
return first;
|
||||
}
|
||||
|
||||
static int run_scientific_case(bool perturb_camera, bool perturb_landmarks,
|
||||
double noise_px, size_t outlier_percent) {
|
||||
auto value = scientific_fixture(perturb_camera, perturb_landmarks,
|
||||
noise_px == 0.0 && outlier_percent == 0 ? 5 : 10);
|
||||
if (noise_px != 0.0) {
|
||||
for (auto &landmark : value.landmarks) landmark.point.z = 5.0;
|
||||
for (auto &observation : value.input_observations) {
|
||||
const auto camera = std::lower_bound(
|
||||
value.cameras.begin(), value.cameras.end(), observation.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
const auto landmark = std::lower_bound(
|
||||
value.landmarks.begin(), value.landmarks.end(), observation.track_id,
|
||||
[](const auto &item, uint64_t key) { return item.track_id < key; });
|
||||
const auto image = std::lower_bound(
|
||||
value.images.begin(), value.images.end(), observation.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
CHECK(camera != value.cameras.end());
|
||||
CHECK(landmark != value.landmarks.end());
|
||||
CHECK(image != value.images.end());
|
||||
Lardon3DSparseGeometryPoint2 noiseless = {};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&image->calibration, &camera->pose_cw, &landmark->point, &noiseless));
|
||||
observation.x = noiseless.x;
|
||||
observation.y = noiseless.y;
|
||||
}
|
||||
for (size_t index = 0; index < value.input_observations.size(); ++index) {
|
||||
auto &observation = value.input_observations[index];
|
||||
const auto camera = std::lower_bound(
|
||||
value.cameras.begin(), value.cameras.end(), observation.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
const auto landmark = std::lower_bound(
|
||||
value.landmarks.begin(), value.landmarks.end(), observation.track_id,
|
||||
[](const auto &item, uint64_t key) { return item.track_id < key; });
|
||||
const auto image = std::lower_bound(
|
||||
value.images.begin(), value.images.end(), observation.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
CHECK(camera != value.cameras.end());
|
||||
CHECK(landmark != value.landmarks.end());
|
||||
CHECK(image != value.images.end());
|
||||
Lardon3DSparseGeometryPoint2 noiseless = {};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&image->calibration, &camera->pose_cw, &landmark->point, &noiseless));
|
||||
CHECK(close_scalar(observation.x, noiseless.x));
|
||||
CHECK(close_scalar(observation.y, noiseless.y));
|
||||
observation.y += observation.track_id % 2 == 0 ? noise_px : -noise_px;
|
||||
CHECK(std::hypot(observation.x - noiseless.x,
|
||||
observation.y - noiseless.y) == noise_px);
|
||||
}
|
||||
}
|
||||
const size_t outlier_count =
|
||||
value.input_observations.size() * outlier_percent / 100;
|
||||
CHECK(outlier_count * 100 ==
|
||||
value.input_observations.size() * outlier_percent);
|
||||
if (outlier_percent == 10) CHECK(outlier_count == 4);
|
||||
if (outlier_percent == 20) CHECK(outlier_count == 8);
|
||||
if (outlier_percent == 40) CHECK(outlier_count == 16);
|
||||
for (size_t index = 0; index < outlier_count; ++index) {
|
||||
value.input_observations[index].x += index % 2 == 0 ? 2.000001 : -2.000001;
|
||||
}
|
||||
bind_scientific_fixture(&value);
|
||||
Lardon3DSparseBundleAdjustmentResult result = {};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
|
||||
if (outlier_percent != 0 &&
|
||||
result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED) {
|
||||
CHECK(!result.diagnostics[0].accepted);
|
||||
CHECK(result.diagnostics[0].termination ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE);
|
||||
CHECK(result.diagnostics[0].rejection_reason ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE);
|
||||
CHECK(!result.diagnostics[0].has_costs && !result.diagnostics[0].has_rmse);
|
||||
CHECK(std::memcmp(result.cameras, value.cameras.data(),
|
||||
value.cameras.size() * sizeof(value.cameras[0])) == 0);
|
||||
CHECK(std::memcmp(result.landmarks, value.landmarks.data(),
|
||||
value.landmarks.size() * sizeof(value.landmarks[0])) == 0);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
return 0;
|
||||
}
|
||||
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
|
||||
CHECK(result.diagnostics[0].accepted);
|
||||
CHECK(result.diagnostics[0].termination ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED);
|
||||
CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse);
|
||||
CHECK(std::isfinite(result.diagnostics[0].initial_robust_cost));
|
||||
CHECK(std::isfinite(result.diagnostics[0].final_robust_cost));
|
||||
CHECK(result.diagnostics[0].final_robust_cost <=
|
||||
result.diagnostics[0].initial_robust_cost +
|
||||
1e-12 * std::max(1.0,
|
||||
std::abs(result.diagnostics[0].initial_robust_cost)));
|
||||
if (perturb_camera || perturb_landmarks)
|
||||
CHECK(result.diagnostics[0].final_robust_cost <
|
||||
result.diagnostics[0].initial_robust_cost);
|
||||
CHECK(std::memcmp(&result.cameras[0], &value.cameras[0],
|
||||
sizeof(value.cameras[0])) == 0);
|
||||
double published_rmse = 0.0;
|
||||
double published_cost = 0.0;
|
||||
CHECK(published_metrics(value, result, &published_rmse, &published_cost));
|
||||
CHECK(close_scalar(published_rmse,
|
||||
result.diagnostics[0].final_reprojection_rmse_px));
|
||||
CHECK(close_scalar(published_cost, result.diagnostics[0].final_robust_cost));
|
||||
const size_t scale_index = value.cameras.size() - 1;
|
||||
const double scale_before = camera_center_x(value.cameras[scale_index].pose_cw);
|
||||
const double scale_after = camera_center_x(result.cameras[scale_index].pose_cw);
|
||||
CHECK(std::abs(scale_after - scale_before) <=
|
||||
1e-12 * std::max({1.0, std::abs(scale_before), std::abs(scale_after)}));
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int test_execution_and_solver() {
|
||||
Lardon3DSparseBundleAdjustmentResult result = {};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(nullptr, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_INVALID_ARGUMENT);
|
||||
CHECK(result.components == nullptr && result.component_count == 0);
|
||||
|
||||
auto value = scientific_fixture(false, false);
|
||||
bind_scientific_fixture(&value);
|
||||
const auto original_cameras = value.cameras;
|
||||
const auto original_landmarks = value.landmarks;
|
||||
const auto original_images = value.images;
|
||||
const auto original_observations = value.input_observations;
|
||||
const auto original_components = value.components;
|
||||
const auto original_result_observations = value.observations;
|
||||
const auto original_incremental = value.incremental;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(&value.input, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
|
||||
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
|
||||
CHECK(result.diagnostics[0].eligible && result.diagnostics[0].accepted);
|
||||
CHECK(result.diagnostics[0].has_costs && result.diagnostics[0].has_rmse);
|
||||
CHECK(result.diagnostics[0].termination ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED);
|
||||
CHECK(std::memcmp(value.cameras.data(), original_cameras.data(),
|
||||
value.cameras.size() * sizeof(value.cameras[0])) == 0);
|
||||
CHECK(std::memcmp(value.landmarks.data(), original_landmarks.data(),
|
||||
value.landmarks.size() * sizeof(value.landmarks[0])) == 0);
|
||||
CHECK(std::memcmp(value.input_observations.data(), original_observations.data(),
|
||||
value.input_observations.size() *
|
||||
sizeof(value.input_observations[0])) == 0);
|
||||
CHECK(std::memcmp(value.images.data(), original_images.data(),
|
||||
value.images.size() * sizeof(value.images[0])) == 0);
|
||||
CHECK(std::memcmp(value.components.data(), original_components.data(),
|
||||
value.components.size() * sizeof(value.components[0])) == 0);
|
||||
CHECK(std::memcmp(value.observations.data(), original_result_observations.data(),
|
||||
value.observations.size() * sizeof(value.observations[0])) == 0);
|
||||
CHECK(std::memcmp(&value.incremental, &original_incremental,
|
||||
sizeof(value.incremental)) == 0);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(nullptr);
|
||||
|
||||
auto anchor_value = scientific_fixture(false, false, 5, true);
|
||||
bind_scientific_fixture(&anchor_value);
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(&anchor_value.input, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
|
||||
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE);
|
||||
CHECK(std::memcmp(&result.cameras[0], &anchor_value.cameras[0],
|
||||
sizeof(result.cameras[0])) == 0);
|
||||
double anchor_rmse = 0.0;
|
||||
double anchor_cost = 0.0;
|
||||
CHECK(published_metrics(anchor_value, result, &anchor_rmse, &anchor_cost));
|
||||
CHECK(close_scalar(anchor_rmse,
|
||||
result.diagnostics[0].final_reprojection_rmse_px));
|
||||
CHECK(close_scalar(anchor_cost, result.diagnostics[0].final_robust_cost));
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
|
||||
Lardon3DSparseIncrementalImage lone_image = {10, calibration()};
|
||||
Lardon3DSparseIncrementalComponent lone_component = {10, 1, 1, 0};
|
||||
Lardon3DSparseIncrementalCamera lone_camera = {};
|
||||
lone_camera.image_id = 10;
|
||||
lone_camera.component_key = 10;
|
||||
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
std::memcpy(lone_camera.pose_cw.rotation_cw, identity, sizeof(identity));
|
||||
Lardon3DSparseIncrementalResult lone_incremental = {};
|
||||
lone_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
|
||||
lone_incremental.components = &lone_component;
|
||||
lone_incremental.component_count = 1;
|
||||
lone_incremental.cameras = &lone_camera;
|
||||
lone_incremental.camera_count = 1;
|
||||
Lardon3DSparseBundleAdjustmentInput lone_input = {
|
||||
&lone_incremental, &lone_image, 1, nullptr, 0};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(&lone_input, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
|
||||
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED);
|
||||
CHECK(!result.diagnostics[0].eligible);
|
||||
CHECK(result.diagnostics[0].rejection_reason ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
|
||||
Lardon3DSparseIncrementalResult empty_incremental = {};
|
||||
empty_incremental.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
|
||||
Lardon3DSparseBundleAdjustmentInput empty_input = {
|
||||
&empty_incremental, nullptr, 0, nullptr, 0};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_run(&empty_input, &result) ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK);
|
||||
CHECK(result.status == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED);
|
||||
CHECK(result.component_count == 0 && result.camera_count == 0 &&
|
||||
result.landmark_count == 0 && result.observation_count == 0);
|
||||
CHECK(result.components == nullptr && result.cameras == nullptr &&
|
||||
result.landmarks == nullptr && result.observations == nullptr &&
|
||||
result.diagnostics == nullptr);
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
|
||||
value = scientific_fixture(true, true);
|
||||
bind_scientific_fixture(&value);
|
||||
int kinds[13] = {};
|
||||
uint64_t identities[13] = {};
|
||||
int groups[13] = {};
|
||||
bool constants[13] = {};
|
||||
int subset_axes[13] = {};
|
||||
size_t parameter_count = 0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_parameter_ordering(
|
||||
&value.input, kinds, identities, groups, constants, subset_axes, 13,
|
||||
¶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 <typename T>
|
||||
static bool write_items(FILE *file, const T *items, size_t count) {
|
||||
return count == 0 || std::fwrite(items, sizeof(T), count, file) == count;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
static bool read_items(FILE *file, std::vector<T> *items, size_t count) {
|
||||
items->resize(count);
|
||||
return count == 0 || std::fread(items->data(), sizeof(T), count, file) == count;
|
||||
}
|
||||
|
||||
struct SnapshotStorage {
|
||||
Lardon3DSparseBundleAdjustmentResult result = {};
|
||||
std::vector<Lardon3DSparseIncrementalComponent> components;
|
||||
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
|
||||
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
|
||||
std::vector<Lardon3DSparseBundleAdjustmentComponentDiagnostic> diagnostics;
|
||||
};
|
||||
|
||||
static bool write_snapshot(const char *path,
|
||||
const Lardon3DSparseBundleAdjustmentResult &result) {
|
||||
FILE *file = std::fopen(path, "wb");
|
||||
if (!file) return false;
|
||||
const char header[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'};
|
||||
const bool ok = write_items(file, header, sizeof(header)) &&
|
||||
write_items(file, &result.status, 1) &&
|
||||
write_items(file, &result.component_count, 1) &&
|
||||
write_items(file, &result.camera_count, 1) &&
|
||||
write_items(file, &result.landmark_count, 1) &&
|
||||
write_items(file, &result.observation_count, 1) &&
|
||||
write_items(file, result.components, result.component_count) &&
|
||||
write_items(file, result.cameras, result.camera_count) &&
|
||||
write_items(file, result.landmarks, result.landmark_count) &&
|
||||
write_items(file, result.observations, result.observation_count) &&
|
||||
write_items(file, result.diagnostics, result.component_count);
|
||||
return std::fclose(file) == 0 && ok;
|
||||
}
|
||||
|
||||
static bool read_snapshot(const char *path, SnapshotStorage *snapshot) {
|
||||
FILE *file = std::fopen(path, "rb");
|
||||
if (!file) return false;
|
||||
char header[8] = {};
|
||||
const char expected[8] = {'L', '3', 'D', 'B', 'A', 'E', '2', '7'};
|
||||
bool ok = read_items(file, &snapshot->components, 0) &&
|
||||
std::fread(header, 1, sizeof(header), file) == sizeof(header) &&
|
||||
std::memcmp(header, expected, sizeof(header)) == 0 &&
|
||||
std::fread(&snapshot->result.status, sizeof(snapshot->result.status), 1,
|
||||
file) == 1 &&
|
||||
std::fread(&snapshot->result.component_count,
|
||||
sizeof(snapshot->result.component_count), 1, file) == 1 &&
|
||||
std::fread(&snapshot->result.camera_count,
|
||||
sizeof(snapshot->result.camera_count), 1, file) == 1 &&
|
||||
std::fread(&snapshot->result.landmark_count,
|
||||
sizeof(snapshot->result.landmark_count), 1, file) == 1 &&
|
||||
std::fread(&snapshot->result.observation_count,
|
||||
sizeof(snapshot->result.observation_count), 1, file) == 1;
|
||||
if (ok && (snapshot->result.component_count > 4096 ||
|
||||
snapshot->result.camera_count > 4096 ||
|
||||
snapshot->result.landmark_count > 250000 ||
|
||||
snapshot->result.observation_count > 1000000))
|
||||
ok = false;
|
||||
if (ok)
|
||||
ok = read_items(file, &snapshot->components, snapshot->result.component_count) &&
|
||||
read_items(file, &snapshot->cameras, snapshot->result.camera_count) &&
|
||||
read_items(file, &snapshot->landmarks, snapshot->result.landmark_count) &&
|
||||
read_items(file, &snapshot->observations,
|
||||
snapshot->result.observation_count) &&
|
||||
read_items(file, &snapshot->diagnostics, snapshot->result.component_count);
|
||||
if (ok) {
|
||||
snapshot->result.components = snapshot->components.data();
|
||||
snapshot->result.cameras = snapshot->cameras.data();
|
||||
snapshot->result.landmarks = snapshot->landmarks.data();
|
||||
snapshot->result.observations = snapshot->observations.data();
|
||||
snapshot->result.diagnostics = snapshot->diagnostics.data();
|
||||
ok = std::fgetc(file) == EOF;
|
||||
}
|
||||
return std::fclose(file) == 0 && ok;
|
||||
}
|
||||
|
||||
static int snapshot_mode(const char *write_path, const char *compare_path) {
|
||||
auto value = scientific_fixture(true, true);
|
||||
bind_scientific_fixture(&value);
|
||||
Lardon3DSparseBundleAdjustmentResult result = {};
|
||||
if (lardon3d_sparse_bundle_adjustment_run(&value.input, &result) !=
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_EXECUTION_OK)
|
||||
return 1;
|
||||
bool ok = false;
|
||||
if (write_path) {
|
||||
ok = write_snapshot(write_path, result);
|
||||
} else {
|
||||
SnapshotStorage reference;
|
||||
ok = read_snapshot(compare_path, &reference) &&
|
||||
equal_results(reference.result, result);
|
||||
}
|
||||
lardon3d_sparse_bundle_adjustment_result_destroy(&result);
|
||||
return ok ? 0 : 1;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
if (argc == 3 && std::strcmp(argv[1], "--write-snapshot") == 0)
|
||||
return snapshot_mode(argv[2], nullptr);
|
||||
if (argc == 3 && std::strcmp(argv[1], "--compare-snapshot") == 0)
|
||||
return snapshot_mode(nullptr, argv[2]);
|
||||
if (test_helpers() != 0) return 1;
|
||||
if (test_preparation() != 0) return 1;
|
||||
if (test_invalid() != 0) return 1;
|
||||
return test_multiple_components();
|
||||
if (test_multiple_components() != 0) return 1;
|
||||
if (test_underconstraint_rules() != 0) return 1;
|
||||
return test_execution_and_solver();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue