fy59/lardon3d
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

sparse-sfm: complete Gate D incremental core

Browse source
This commit is contained in:
fy59 2026-08-10 17:05:20 +02:00
parent 7872bc82ae
commit e4675cd60b
7 changed files with 2564 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
docs/architecture/reconstruction_pipeline.md
View file

@ -252,9 +252,10 @@ Image Catalog (B) ──► Feature Store (C)
## Statut du pipeline ## Statut du pipeline
Import, Image Catalog, Feature Extraction, Feature Store, Visual Index, Import, Image Catalog, Feature Extraction, Feature Store, Visual Index,
Candidate Pair, Matching v1 et Geometric Verification Model v1 sont Candidate Pair, Matching v1, Geometric Verification, Track Model/Builder v1
**IMPLEMENTED**. Geometric Verifier, and Sparse SfM Gate C geometry are **IMPLEMENTED**. The synchronous in-memory
Tracks et SfM sont **PLANNED**. incremental Sparse SfM Gate D core is **IMPLEMENTED / PASS**. BA,
orchestration, MVS, mesh, texturing and viewer remain **PLANNED**.
Ce document décrit la vision architecturale cible du pipeline de Ce document décrit la vision architecturale cible du pipeline de
reconstruction. Les modules listés ici ne sont pas tous implémentés. reconstruction. Les modules listés ici ne sont pas tous implémentés.

153
docs/architecture/sparse_sfm.md
View file

@ -437,6 +437,159 @@ Degeneracy checks use finite values, positive depth, rotation SO(3) residual
collinearity covariance determinant `1e-10`. These are pure-geometry collinearity covariance determinant `1e-10`. These are pure-geometry
parameters and do not alter Project DB identity. parameters and do not alter Project DB identity.
## Gate D — incremental Sparse SfM core
**GATE D — PASS.** Gate D is the first executable link
between the immutable Track/Calibration contracts and the Gate C primitives.
The reference implementation is synchronous, deterministic, CPU-only,
in-memory, bounded and independent of Project DB, Task Runtime, Resource
Governor and persistence publication.
### Inputs
Gate D consumes exactly one immutable Track Set, one immutable calibration
scope, finite calibration values for participating images, bounded keypoint
coordinates addressed by `(feature_set_id, feature_index)`, and explicit
fingerprinted parameters. The Track Set is never mutated.
### Algorithm
The core sorts image and Track identities, builds sparse connected components,
orders seed candidates by shared Track count and image IDs, and tries a bounded
number of seeds. Each candidate uses the Gate C relative-pose, cheirality,
parallax and two-view triangulation contracts. A valid seed establishes a
component-local unit gauge.
Unregistered images are then ordered by visible accepted-landmark count and
image ID. Gate C calibrated PnP registers at most one selected image per
bounded round. Failed registration leaves the image explicitly unregistered.
New landmarks use all currently registered observations, Gate C multi-view DLT
and bounded point-only refinement. A landmark is accepted or rejected as a
whole; Track observations are never dropped or rewritten.
After each successful camera registration, an existing landmark whose Track
has gained registered observations is reconsidered in canonical image-ID
order. Gate C multi-view triangulation and point refinement use the complete
eligible observation set. The replacement is published in memory only after
finite-value, positive-depth and reprojection validation; otherwise the prior
valid landmark and its observations remain unchanged.
The Gate D reference bounds are 4096 input images, 250,000 Tracks, 1,000,000
observations, 32 seed candidates, 32 registration rounds and 4096 new
landmarks per growth round. The defaults use 1.5 px relative-pose/PnP robust
thresholds, a 2.0 px landmark reprojection threshold, 0.5 minimum inlier
ratios, `1e-4` rad minimum parallax, 6 minimum seed/PnP inliers and 30
point-refinement iterations. These are Gate D policy defaults; changing them
changes the explicit parameter configuration.
### Output and failure semantics
The in-memory result contains deterministic components, registered cameras,
accepted landmarks, landmark observations, reprojection diagnostics and
explicit unregistered images. Results are `COMPLETE`, `PARTIAL` or `FAILED`.
Invalid input fails before computation. A rejected seed, camera or landmark
does not corrupt an accepted model. No partial result is persisted.
Growth stops immediately when a complete registration round cannot register
an image. It also stops exactly at the configured registration-round bound.
Both paths retain valid cameras and landmarks, list every remaining image as
unregistered and produce `PARTIAL` when usable geometry exists.
Components with fewer than two registered cameras are not valid 3D components.
Disconnected valid components retain independent unit gauges and are never
globally aligned by Gate D.
### Gate D limits
Gate D does not implement BA, persistence adapters, Task Runtime, checkpoints,
Governor integration, a Resource System, GPU execution, dense reconstruction,
metric alignment, viewer integration or any Project DB change. BA remains the
later Gate E; project/task orchestration remains Gate F; resource/freeze
integration remains Gate G.
### Canonical Gate D functional matrix
This table freezes the complete numbered validation contract. Evidence is the
minimum dedicated observation required; an earlier rejection never substitutes
for the named path.
| Case | Purpose | Required path and evidence | Expected result |
|---|---|---|---|
| 01 Minimal two-view | Smallest valid reconstruction | One seed, two cameras, finite landmarks | `COMPLETE` |
| 02 Deterministic seed | Canonical seed identity | Same selected pair and pose on repeat | `COMPLETE` |
| 03 Multiple seed candidates | Candidate ordering | Multiple eligible pairs, canonical first pair | `COMPLETE` |
| 04 Rejected first seed / later seed | Seed fallback | At least two attempts, later pair selected | `COMPLETE` |
| 05 Camera-addition order | Registration ordering | Highest support then image ID, one per round | `COMPLETE` |
| 06 Clean PnP | Nominal registration | PnP attempted and succeeds with clean support | `COMPLETE` |
| 07 Noisy PnP | Bounded noise | PnP succeeds with finite pose | `COMPLETE` |
| 08 Deterministic PnP outliers | Robust registration | Stable inlier count and pose | `COMPLETE` |
| 09 Failed PnP | Registration rejection | Failure counted and image listed | `PARTIAL` |
| 10 Insufficient PnP support | Eligibility bound | Solver not called and image listed | `PARTIAL` |
| 11 Low-parallax rejection | Seed guard | Gate C low-parallax/degenerate status | `FAILED` |
| 12 Pure rotation | Translation degeneracy | Relative pose rejected, no camera | `FAILED` |
| 13 Planar degeneracy | Ambiguous seed | Gate C degeneracy, no camera | `FAILED` |
| 14 Far scene | Finite distant geometry | Seed and finite landmarks accepted | `COMPLETE` |
| 15 Disconnected graph | Component discovery | Valid component plus explicit singleton | `PARTIAL` |
| 16 Multiple valid components | Isolation | Two reconstructed components | `COMPLETE` |
| 17 Independent gauges | Per-component gauge | Each seed camera is identity | `COMPLETE` |
| 18 Unregistered images | Explicit output | Remaining image and component key listed | `PARTIAL` |
| 19 Behind-camera landmark | Cheirality | Exact Gate C status and distinct counter | `COMPLETE` model |
| 20 High reprojection error | Residual policy | Finite triangulation then residual rejection | `COMPLETE` model |
| 21 Failed triangulation | Geometry failure | Finite input calls triangulation and fails | `COMPLETE` model |
| 22 Repeated observations | Track coherence | Duplicate image or feature reference rejected | `INVALID_ARGUMENT` |
| 23 Many-camera Track | Landmark lifecycle | One landmark, six ordered observations | `COMPLETE` |
| 24 New landmark after registration | Incremental growth | Ineligible Track accepted after PnP | `COMPLETE` |
| 25 Multi-view growth | All eligible views | New landmark uses at least three views | `COMPLETE` |
| 26 Point refinement | Bounded refinement | Attempt and finite accepted point | `COMPLETE` |
| 27 No-growth termination | Progress bound | One zero-progress round and diagnostic | `PARTIAL` |
| 28 All-images termination | Natural completion | All images registered, no stop diagnostic | `COMPLETE` |
| 29 Seed exhaustion | Candidate bound | Every available candidate attempted | `FAILED` |
| 30 Registration-round exhaustion | Round bound | Exact rounds and remaining images | `PARTIAL` |
| 31 Component ordering | Canonical components | Increasing component keys | success |
| 32 Camera ordering | Canonical cameras | Increasing image IDs | success |
| 33 Landmark ordering | Canonical landmarks | Increasing `(component_key, track_id)` | success |
| 34 In-process repeatability | Local determinism | Complete scientific result equality | same status |
| 35 Fresh-process repeatability | Process determinism | 20 runs emit one signature | same status |
### Gate D validation responsibility
`GATE_D_REQUIRED` covers pointer/count coherence, identities carried by this
API, finite calibration/keypoints, feature-index bounds, Track observation
coherence, geometry failures, atomic result ownership and cleanup. Store-level
Feature Set/File existence is `UPSTREAM_RESPONSIBILITY`: Gate D receives
flattened validated coordinates and never opens a store. Two separate Track
objects with the same ID are `UNREPRESENTABLE_BY_API` because rows are grouped
by `track_id`; duplicate image observations and feature references remain
representable and are rejected. Allocation-failure injection is
`NOT_APPLICABLE_WITH_PROOF`: no allocator injection boundary exists, production
catches allocation failure at the C ABI, and global test allocator state would
violate the architecture.
| Condition | Classification |
|---|---|
| Null parameters, missing arrays, empty input | `GATE_D_REQUIRED` |
| Zero Track/calibration/image/feature identity | `GATE_D_REQUIRED` |
| Missing per-image calibration coverage | `GATE_D_REQUIRED` |
| Zero/non-finite focal or distortion, invalid principal point | `GATE_D_REQUIRED` |
| Invalid feature index or non-finite keypoint | `GATE_D_REQUIRED` |
| Duplicate image/feature observation or singleton Track | `GATE_D_REQUIRED` |
| Seed/PnP/landmark failures and update rollback | `GATE_D_REQUIRED` |
| Missing Feature Set/File in persistent storage | `UPSTREAM_RESPONSIBILITY` |
| Two distinct Track objects sharing one ID | `UNREPRESENTABLE_BY_API` |
| Deterministic allocation-failure injection | `NOT_APPLICABLE_WITH_PROOF` |
The caller retains all input allocations for the synchronous call. The result
owns its arrays; `lardon3d_sparse_incremental_result_destroy()` releases them
and accepts an empty result or null pointer. No C++ exception crosses the C17
boundary.
Count-limit validation uses structurally sufficient fixtures at a lowered
explicit configured limit and proves `LIMIT-1`, `LIMIT`, and `LIMIT+1` without
materializing the public hard maxima. Scientific scale is validated separately
by the small, medium and large resource workloads. Policy tests prove exact
seed-candidate, registration-round and new-landmark-per-round admission; no
policy loop performs a `limit + 1` attempt.
## Out of scope ## Out of scope
No production Sparse SfM, triangulator, camera solver, BA, Project DB v16, No production Sparse SfM, triangulator, camera solver, BA, Project DB v16,

155
include/lardon3d/sparse_sfm_incremental.h Normal file
View file

@ -0,0 +1,155 @@
#ifndef LARDON3D_SPARSE_SFM_INCREMENTAL_H
#define LARDON3D_SPARSE_SFM_INCREMENTAL_H
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <lardon3d/sparse_sfm_geometry.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
LARDON3D_SPARSE_INCREMENTAL_COMPLETE = 0,
LARDON3D_SPARSE_INCREMENTAL_PARTIAL,
LARDON3D_SPARSE_INCREMENTAL_FAILED,
LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT,
LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY
} Lardon3DSparseIncrementalStatus;
typedef struct {
uint64_t image_id;
Lardon3DSparseGeometryCalibration calibration;
} Lardon3DSparseIncrementalImage;
typedef struct {
uint64_t track_id;
uint64_t image_id;
uint64_t feature_set_id;
uint32_t feature_index;
uint32_t feature_count;
double x;
double y;
} Lardon3DSparseIncrementalObservation;
typedef struct {
uint64_t track_set_id;
uint64_t calibration_scope_id;
const Lardon3DSparseIncrementalImage *images;
size_t image_count;
const Lardon3DSparseIncrementalObservation *observations;
size_t observation_count;
} Lardon3DSparseIncrementalInput;
typedef struct {
uint32_t minimum_seed_tracks;
uint32_t minimum_seed_landmarks;
uint32_t minimum_pnp_correspondences;
uint32_t maximum_seed_candidates;
uint32_t maximum_registration_rounds;
uint32_t maximum_landmarks_per_round;
uint32_t maximum_images;
uint64_t maximum_observations;
uint64_t maximum_tracks;
double reprojection_threshold_px;
double minimum_track_parallax_rad;
Lardon3DSparseGeometryRelativePoseParameters relative_pose;
Lardon3DSparseGeometryPnPParameters pnp;
Lardon3DSparseGeometryPointRefinementParameters refinement;
} Lardon3DSparseIncrementalParameters;
typedef struct {
uint64_t image_id;
uint64_t component_key;
Lardon3DSparseGeometryPose pose_cw;
} Lardon3DSparseIncrementalCamera;
typedef struct {
uint64_t landmark_id;
uint64_t track_id;
uint64_t component_key;
Lardon3DSparseGeometryPoint3 point;
double reprojection_rmse_px;
double reprojection_median_px;
uint64_t observation_count;
} Lardon3DSparseIncrementalLandmark;
typedef struct {
uint64_t landmark_id;
uint64_t track_id;
uint64_t image_id;
uint64_t feature_set_id;
uint32_t feature_index;
uint32_t position_in_track;
} Lardon3DSparseIncrementalLandmarkObservation;
typedef struct {
uint64_t component_key;
uint64_t image_count;
uint64_t registered_image_count;
uint64_t landmark_count;
} Lardon3DSparseIncrementalComponent;
typedef struct {
uint64_t image_id;
uint64_t component_key;
} Lardon3DSparseIncrementalUnregisteredImage;
typedef struct {
Lardon3DSparseIncrementalStatus status;
uint64_t track_set_id;
uint64_t calibration_scope_id;
Lardon3DSparseIncrementalComponent *components;
size_t component_count;
Lardon3DSparseIncrementalCamera *cameras;
size_t camera_count;
Lardon3DSparseIncrementalLandmark *landmarks;
size_t landmark_count;
Lardon3DSparseIncrementalLandmarkObservation *observations;
size_t observation_count;
Lardon3DSparseIncrementalUnregisteredImage *unregistered_images;
size_t unregistered_image_count;
uint64_t seed_candidates_considered;
uint64_t seed_candidates_available;
uint64_t seed_image_a;
uint64_t seed_image_b;
int32_t last_seed_geometry_status;
double last_seed_parallax_rad;
uint64_t registration_rounds;
uint64_t registration_attempts;
uint64_t registration_successes;
uint64_t registration_failures;
uint32_t last_pnp_inlier_count;
uint64_t triangulation_attempts;
uint64_t triangulation_failures;
uint64_t rejected_behind_camera;
uint64_t rejected_reprojection;
uint64_t rejected_landmarks;
int32_t last_triangulation_status;
uint64_t landmark_update_attempts;
uint64_t landmark_update_successes;
uint64_t landmark_update_failures;
uint64_t no_growth_terminations;
uint64_t round_limit_terminations;
uint64_t point_refinement_attempts;
uint64_t point_refinement_successes;
} Lardon3DSparseIncrementalResult;
bool lardon3d_sparse_incremental_parameters_default(
Lardon3DSparseIncrementalParameters *parameters);
Lardon3DSparseIncrementalStatus lardon3d_sparse_incremental_run(
const Lardon3DSparseIncrementalInput *input,
const Lardon3DSparseIncrementalParameters *parameters,
Lardon3DSparseIncrementalResult *result);
void lardon3d_sparse_incremental_result_destroy(
Lardon3DSparseIncrementalResult *result);
#ifdef __cplusplus
}
#endif
#endif

23
meson.build
View file

@ -145,6 +145,7 @@ executable(
'src/project.c', 'src/project.c',
'src/project_db.c', 'src/project_db_sparse_sfm.c', 'src/project_db.c', 'src/project_db_sparse_sfm.c',
'src/sparse_sfm_geometry.cpp', 'src/sparse_sfm_geometry.cpp',
'src/sparse_sfm_incremental.cpp',
'src/task.c', 'src/task.c',
'src/task_checkpoint.c', 'src/task_checkpoint.c',
'src/task_kind_registry.c', 'src/task_kind_registry.c',
@ -533,6 +534,28 @@ sparse_sfm_geometry_test = executable(
test('sparse-sfm-geometry', sparse_sfm_geometry_test, timeout: 60) test('sparse-sfm-geometry', sparse_sfm_geometry_test, timeout: 60)
sparse_sfm_incremental_test = executable(
'test-sparse-sfm-incremental',
sources: [
'tests/test_sparse_sfm_incremental.cpp',
'src/sparse_sfm_geometry.cpp', 'src/sparse_sfm_incremental.cpp',
],
include_directories: include_directories('include'),
dependencies: [opencv_geometry],
)
test('sparse-sfm-incremental', sparse_sfm_incremental_test, timeout: 60)
executable(
'benchmark-sparse-sfm-incremental',
sources: [
'tests/benchmark_sparse_sfm_incremental.cpp',
'src/sparse_sfm_geometry.cpp', 'src/sparse_sfm_incremental.cpp',
],
include_directories: include_directories('include'),
dependencies: [opencv_geometry],
)
sparse_sfm_resource_test = executable( sparse_sfm_resource_test = executable(
'test-sparse-sfm-resource', 'test-sparse-sfm-resource',
sources: [ sources: [

774
src/sparse_sfm_incremental.cpp Normal file
View file

@ -0,0 +1,774 @@
#include <lardon3d/sparse_sfm_incremental.h>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <map>
#include <new>
#include <set>
#include <utility>
#include <vector>
namespace {
struct Observation {
uint64_t track_id;
uint64_t image_id;
uint64_t feature_set_id;
uint32_t feature_index;
Lardon3DSparseGeometryPoint2 pixel;
};
struct Track {
uint64_t id;
std::vector<Observation> observations;
};
struct Image {
uint64_t id;
Lardon3DSparseGeometryCalibration calibration;
size_t component;
};
struct Pose {
uint64_t image_id;
Lardon3DSparseGeometryPose value;
};
struct Landmark {
uint64_t track_id;
uint64_t component_key;
Lardon3DSparseGeometryPoint3 point;
std::vector<Observation> observations;
double rmse;
double median;
};
struct Component {
uint64_t key;
std::vector<size_t> image_indices;
};
struct UnionFind {
std::vector<size_t> parent;
explicit UnionFind(size_t count) : parent(count) {
for (size_t index = 0; index < count; ++index) parent[index] = index;
}
size_t find(size_t value) {
size_t root = value;
while (parent[root] != root) root = parent[root];
while (parent[value] != value) {
size_t next = parent[value];
parent[value] = root;
value = next;
}
return root;
}
void unite(size_t left, size_t right) {
left = find(left);
right = find(right);
if (left == right) return;
if (left < right)
parent[right] = left;
else
parent[left] = right;
}
};
bool finite_calibration(const Lardon3DSparseGeometryCalibration &calibration) {
return calibration.width > 0 && calibration.height > 0 &&
std::isfinite(calibration.fx) && std::isfinite(calibration.fy) &&
std::isfinite(calibration.cx) && std::isfinite(calibration.cy) &&
std::isfinite(calibration.k1) && std::isfinite(calibration.k2) &&
std::isfinite(calibration.p1) && std::isfinite(calibration.p2) &&
calibration.fx > 0.0 && calibration.fy > 0.0 &&
calibration.cx >= 0.0 && calibration.cx < calibration.width &&
calibration.cy >= 0.0 && calibration.cy < calibration.height;
}
bool finite_pose(const Lardon3DSparseGeometryPose &pose) {
for (double value : pose.rotation_cw)
if (!std::isfinite(value)) return false;
for (double value : pose.translation_cw)
if (!std::isfinite(value)) return false;
return true;
}
Lardon3DSparseGeometryPose identity_pose() {
return {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0},
{0.0, 0.0, 0.0}};
}
bool project(const Image &image, const Pose &pose,
const Lardon3DSparseGeometryPoint3 &point,
Lardon3DSparseGeometryPoint2 *pixel) {
const double *r = pose.value.rotation_cw;
const double *t = pose.value.translation_cw;
const double x = r[0] * point.x + r[1] * point.y + r[2] * point.z + t[0];
const double y = r[3] * point.x + r[4] * point.y + r[5] * point.z + t[1];
const double z = r[6] * point.x + r[7] * point.y + r[8] * point.z + t[2];
if (!pixel || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) ||
z <= 1e-9)
return false;
pixel->x = image.calibration.fx * x / z + image.calibration.cx;
pixel->y = image.calibration.fy * y / z + image.calibration.cy;
return std::isfinite(pixel->x) && std::isfinite(pixel->y);
}
double reprojection_error(const Image &image, const Pose &pose,
const Landmark &landmark,
const Observation &observation) {
Lardon3DSparseGeometryPoint2 projected;
if (!project(image, pose, landmark.point, &projected)) return INFINITY;
const double dx = projected.x - observation.pixel.x;
const double dy = projected.y - observation.pixel.y;
return std::sqrt(dx * dx + dy * dy);
}
bool validate_parameters(const Lardon3DSparseIncrementalParameters &p) {
return p.minimum_seed_tracks >= 2 && p.minimum_seed_landmarks >= 1 &&
p.minimum_pnp_correspondences >= 4 && p.maximum_seed_candidates > 0 &&
p.maximum_registration_rounds > 0 && p.maximum_landmarks_per_round > 0 &&
p.maximum_images >= 2 && p.maximum_observations >= p.maximum_tracks &&
std::isfinite(p.reprojection_threshold_px) &&
p.reprojection_threshold_px > 0.0 &&
std::isfinite(p.minimum_track_parallax_rad) &&
p.minimum_track_parallax_rad >= 0.0 &&
p.relative_pose.minimum_inliers >= p.minimum_seed_tracks &&
p.pnp.minimum_inliers >= p.minimum_pnp_correspondences &&
p.relative_pose.minimum_inlier_ratio > 0.0 &&
p.relative_pose.minimum_inlier_ratio <= 1.0 &&
p.pnp.minimum_inlier_ratio > 0.0 && p.pnp.minimum_inlier_ratio <= 1.0;
}
void destroy_result(Lardon3DSparseIncrementalResult *result) {
if (!result) return;
std::free(result->components);
std::free(result->cameras);
std::free(result->landmarks);
std::free(result->observations);
std::free(result->unregistered_images);
*result = {};
}
bool append_component(const Component &component,
const std::vector<Image> &images,
std::vector<Lardon3DSparseIncrementalComponent> *out) {
Lardon3DSparseIncrementalComponent value = {};
value.component_key = component.key;
value.image_count = component.image_indices.size();
for (size_t index : component.image_indices)
if (images[index].id == component.key) value.component_key = images[index].id;
out->push_back(value);
return true;
}
enum class LandmarkCandidateStatus {
accepted,
insufficient,
triangulation_failed,
behind_camera,
reprojection_failed,
};
LandmarkCandidateStatus build_landmark_candidate(
const Track &track, uint64_t component_key,
const std::vector<Image> &images,
const std::map<uint64_t, size_t> &image_index,
const std::vector<Pose> &cameras, const std::set<uint64_t> &registered,
const Lardon3DSparseIncrementalParameters &parameters,
Landmark *candidate, Lardon3DSparseGeometryResult *geometry_status,
Lardon3DSparseIncrementalResult *result) {
std::vector<Lardon3DSparseGeometryPoint2> normalized;
std::vector<Lardon3DSparseGeometryPose> poses;
std::vector<Observation> used;
for (const Observation &observation : track.observations) {
if (!registered.count(observation.image_id)) continue;
const auto pose_it = std::find_if(
cameras.begin(), cameras.end(), [&](const Pose &pose) {
return pose.image_id == observation.image_id;
});
if (pose_it == cameras.end()) continue;
const Image &image = images[image_index.at(observation.image_id)];
Lardon3DSparseGeometryPoint2 point;
if (lardon3d_sparse_geometry_normalize(
&image.calibration, &observation.pixel, 1, &point) !=
LARDON3D_SPARSE_GEOMETRY_OK)
return LandmarkCandidateStatus::triangulation_failed;
normalized.push_back(point);
poses.push_back(pose_it->value);
used.push_back(observation);
}
if (used.size() < 2) return LandmarkCandidateStatus::insufficient;
Lardon3DSparseGeometryPoint3 point;
*geometry_status = lardon3d_sparse_geometry_triangulate_multi_view(
normalized.data(), poses.data(), normalized.size(), &point);
if (*geometry_status != LARDON3D_SPARSE_GEOMETRY_OK) {
return *geometry_status == LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED
? LandmarkCandidateStatus::behind_camera
: LandmarkCandidateStatus::triangulation_failed;
}
Lardon3DSparseGeometryPoint3 refined;
++result->point_refinement_attempts;
const Lardon3DSparseGeometryResult refinement_status =
lardon3d_sparse_geometry_refine_point(
normalized.data(), poses.data(), normalized.size(), &point,
&parameters.refinement, &refined);
if (refinement_status == LARDON3D_SPARSE_GEOMETRY_OK) {
point = refined;
++result->point_refinement_successes;
}
Landmark replacement = {
track.id, component_key, point, std::move(used), 0.0, 0.0};
double squared = 0.0;
double maximum = 0.0;
for (const Observation &observation : replacement.observations) {
const Image &image = images[image_index.at(observation.image_id)];
const Pose &pose = *std::find_if(
cameras.begin(), cameras.end(), [&](const Pose &item) {
return item.image_id == observation.image_id;
});
const double error = reprojection_error(
image, pose, replacement, observation);
if (!std::isfinite(error)) return LandmarkCandidateStatus::behind_camera;
if (error > parameters.reprojection_threshold_px)
return LandmarkCandidateStatus::reprojection_failed;
squared += error * error;
maximum = std::max(maximum, error);
}
replacement.rmse = std::sqrt(
squared / static_cast<double>(replacement.observations.size()));
replacement.median = maximum;
*candidate = std::move(replacement);
return LandmarkCandidateStatus::accepted;
}
void record_landmark_rejection(LandmarkCandidateStatus status,
Lardon3DSparseIncrementalResult *result) {
if (status == LandmarkCandidateStatus::behind_camera) {
++result->rejected_landmarks;
++result->rejected_behind_camera;
} else if (status == LandmarkCandidateStatus::reprojection_failed) {
++result->rejected_landmarks;
++result->rejected_reprojection;
} else if (status == LandmarkCandidateStatus::triangulation_failed) {
++result->triangulation_failures;
}
}
} // namespace
extern "C" bool lardon3d_sparse_incremental_parameters_default(
Lardon3DSparseIncrementalParameters *parameters) {
if (!parameters) return false;
*parameters = {};
parameters->minimum_seed_tracks = 6;
parameters->minimum_seed_landmarks = 6;
parameters->minimum_pnp_correspondences = 6;
parameters->maximum_seed_candidates = 32;
parameters->maximum_registration_rounds = 32;
parameters->maximum_landmarks_per_round = 4096;
parameters->maximum_images = 4096;
parameters->maximum_observations = 1000000;
parameters->maximum_tracks = 250000;
parameters->reprojection_threshold_px = 2.0;
parameters->minimum_track_parallax_rad = 1e-4;
parameters->relative_pose = {1.5, 0.999, 1500, 6, 0.5, 1e-4, 0.5, 0};
parameters->pnp = {1.5, 0.999, 1000, 6, 0.5, 0};
parameters->refinement = {30, 1e-12};
return true;
}
extern "C" Lardon3DSparseIncrementalStatus lardon3d_sparse_incremental_run(
const Lardon3DSparseIncrementalInput *input,
const Lardon3DSparseIncrementalParameters *parameters,
Lardon3DSparseIncrementalResult *result) {
if (!result) return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
destroy_result(result);
try {
if (!input || !parameters || !validate_parameters(*parameters) ||
!input->images || input->image_count == 0 || !input->observations ||
input->observation_count == 0 || input->image_count > parameters->maximum_images ||
input->observation_count > parameters->maximum_observations ||
input->track_set_id == 0 ||
input->calibration_scope_id == 0)
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
std::vector<Image> images;
images.reserve(input->image_count);
for (size_t index = 0; index < input->image_count; ++index) {
const auto &source = input->images[index];
if (source.image_id == 0 || !finite_calibration(source.calibration))
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
images.push_back({source.image_id, source.calibration, 0});
}
std::sort(images.begin(), images.end(),
[](const Image &a, const Image &b) { return a.id < b.id; });
for (size_t index = 1; index < images.size(); ++index)
if (images[index - 1].id == images[index].id)
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
std::map<uint64_t, size_t> image_index;
for (size_t index = 0; index < images.size(); ++index)
image_index[images[index].id] = index;
std::map<uint64_t, Track> tracks_by_id;
for (size_t index = 0; index < input->observation_count; ++index) {
const auto &source = input->observations[index];
if (source.track_id == 0 || source.image_id == 0 ||
source.feature_set_id == 0 || source.feature_index >= source.feature_count ||
image_index.find(source.image_id) == image_index.end() ||
!std::isfinite(source.x) || !std::isfinite(source.y))
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
Track &track = tracks_by_id[source.track_id];
track.id = source.track_id;
for (const Observation &old : track.observations)
if (old.image_id == source.image_id)
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
track.observations.push_back(
{source.track_id, source.image_id, source.feature_set_id,
source.feature_index, {source.x, source.y}});
}
if (tracks_by_id.empty() || tracks_by_id.size() > parameters->maximum_tracks)
return LARDON3D_SPARSE_INCREMENTAL_FAILED;
std::set<std::pair<uint64_t, uint32_t>> feature_observations;
for (const auto &entry : tracks_by_id) {
if (entry.second.observations.size() < 2)
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
for (const Observation &observation : entry.second.observations)
if (!feature_observations.insert(
{observation.feature_set_id, observation.feature_index})
.second)
return LARDON3D_SPARSE_INCREMENTAL_INVALID_ARGUMENT;
}
for (auto &entry : tracks_by_id) {
std::sort(entry.second.observations.begin(),
entry.second.observations.end(),
[](const Observation &left, const Observation &right) {
return left.image_id < right.image_id;
});
}
UnionFind union_find(images.size());
for (const auto &entry : tracks_by_id) {
const auto &observations = entry.second.observations;
for (size_t index = 1; index < observations.size(); ++index)
union_find.unite(image_index[observations[0].image_id],
image_index[observations[index].image_id]);
}
std::map<size_t, Component> components_by_root;
for (size_t index = 0; index < images.size(); ++index)
components_by_root[union_find.find(index)].image_indices.push_back(index);
std::vector<Component> components;
for (auto &entry : components_by_root) {
auto &items = entry.second.image_indices;
std::sort(items.begin(), items.end(), [&](size_t a, size_t b) {
return images[a].id < images[b].id;
});
entry.second.key = images[items.front()].id;
components.push_back(entry.second);
for (size_t item : items) images[item].component = components.size() - 1;
}
std::sort(components.begin(), components.end(),
[](const Component &a, const Component &b) { return a.key < b.key; });
std::map<uint64_t, size_t> component_by_image;
for (size_t component = 0; component < components.size(); ++component)
for (size_t image : components[component].image_indices) {
images[image].component = component;
component_by_image[images[image].id] = component;
}
std::vector<Lardon3DSparseIncrementalComponent> output_components;
for (const Component &component : components)
append_component(component, images, &output_components);
std::vector<Pose> cameras;
std::vector<Landmark> landmarks;
std::set<uint64_t> registered;
std::set<uint64_t> accepted_tracks;
std::map<uint64_t, uint64_t> component_keys;
for (const Component &component : components) component_keys[component.key] = component.key;
for (const Component &component : components) {
if (component.image_indices.size() < 2) continue;
struct Candidate { uint64_t a; uint64_t b; uint32_t shared; };
std::vector<Candidate> candidates;
std::map<std::pair<uint64_t, uint64_t>, uint32_t> shared_by_pair;
for (const auto &entry : tracks_by_id) {
std::vector<uint64_t> track_images;
for (const Observation &observation : entry.second.observations)
if (std::find(track_images.begin(), track_images.end(), observation.image_id) ==
track_images.end())
track_images.push_back(observation.image_id);
std::sort(track_images.begin(), track_images.end());
for (size_t left = 0; left < track_images.size(); ++left)
for (size_t right = left + 1; right < track_images.size(); ++right)
if (component_by_image[track_images[left]] ==
component_by_image[track_images[right]] &&
component_by_image[track_images[left]] ==
component_by_image[component.key])
++shared_by_pair[{track_images[left], track_images[right]}];
}
for (const auto &entry : shared_by_pair)
if (entry.second >= parameters->minimum_seed_tracks)
candidates.push_back({entry.first.first, entry.first.second, entry.second});
std::sort(candidates.begin(), candidates.end(),
[](const Candidate &a, const Candidate &b) {
if (a.shared != b.shared) return a.shared > b.shared;
if (a.a != b.a) return a.a < b.a;
return a.b < b.b;
});
const size_t candidate_limit = std::min<size_t>(
candidates.size(), parameters->maximum_seed_candidates);
result->seed_candidates_available += candidates.size();
bool seeded = false;
for (size_t candidate_index = 0; candidate_index < candidate_limit;
++candidate_index) {
++result->seed_candidates_considered;
const Candidate &candidate = candidates[candidate_index];
const Image &image_a = images[image_index[candidate.a]];
const Image &image_b = images[image_index[candidate.b]];
std::vector<Observation> pair_a, pair_b;
std::vector<uint64_t> track_ids;
for (const auto &entry : tracks_by_id) {
const Observation *a = nullptr, *b = nullptr;
for (const Observation &observation : entry.second.observations) {
if (observation.image_id == candidate.a) a = &observation;
if (observation.image_id == candidate.b) b = &observation;
}
if (a && b) {
track_ids.push_back(entry.first);
pair_a.push_back(*a);
pair_b.push_back(*b);
}
}
std::vector<Lardon3DSparseGeometryPoint2> pixels_a(pair_a.size());
std::vector<Lardon3DSparseGeometryPoint2> pixels_b(pair_b.size());
for (size_t i = 0; i < pair_a.size(); ++i) {
pixels_a[i] = pair_a[i].pixel;
pixels_b[i] = pair_b[i].pixel;
}
std::vector<uint8_t> mask(pair_a.size());
Lardon3DSparseGeometryRelativePoseResult pose_result = {};
pose_result.inlier_mask = mask.data();
pose_result.inlier_mask_capacity = mask.size();
auto relative = parameters->relative_pose;
relative.minimum_inliers = std::max<uint32_t>(
relative.minimum_inliers, parameters->minimum_seed_tracks);
const Lardon3DSparseGeometryResult relative_status =
lardon3d_sparse_geometry_relative_pose(
&image_a.calibration, &image_b.calibration, pixels_a.data(),
pixels_b.data(), pixels_a.size(), &relative, &pose_result);
result->last_seed_geometry_status = relative_status;
result->last_seed_parallax_rad = pose_result.median_parallax_rad;
if (relative_status != LARDON3D_SPARSE_GEOMETRY_OK ||
pose_result.inlier_count < parameters->minimum_seed_landmarks ||
pose_result.median_parallax_rad < parameters->minimum_track_parallax_rad ||
!finite_pose(pose_result.pose_ba))
continue;
Pose pose_a = {candidate.a, identity_pose()};
Pose pose_b = {candidate.b, pose_result.pose_ba};
std::vector<Landmark> seed_landmarks;
for (size_t i = 0; i < pair_a.size(); ++i) {
if (!mask[i]) continue;
++result->triangulation_attempts;
Lardon3DSparseGeometryPoint2 normalized[2];
if (lardon3d_sparse_geometry_normalize(&image_a.calibration,
&pair_a[i].pixel, 1,
&normalized[0]) !=
LARDON3D_SPARSE_GEOMETRY_OK ||
lardon3d_sparse_geometry_normalize(&image_b.calibration,
&pair_b[i].pixel, 1,
&normalized[1]) !=
LARDON3D_SPARSE_GEOMETRY_OK)
continue;
Lardon3DSparseGeometryPoint3 point;
if (lardon3d_sparse_geometry_triangulate_two_view(
&normalized[0], &normalized[1], &pose_a.value, &pose_b.value,
&point) != LARDON3D_SPARSE_GEOMETRY_OK)
{
++result->triangulation_failures;
continue;
}
Lardon3DSparseGeometryPoint3 refined;
Lardon3DSparseGeometryPose seed_poses[2] = {pose_a.value, pose_b.value};
if (lardon3d_sparse_geometry_refine_point(
normalized, seed_poses, 2, &point,
&parameters->refinement, &refined) ==
LARDON3D_SPARSE_GEOMETRY_OK)
point = refined;
Landmark landmark = {track_ids[i], component.key, point, {pair_a[i], pair_b[i]},
0.0, 0.0};
const double error_a = reprojection_error(image_a, pose_a, landmark, pair_a[i]);
const double error_b = reprojection_error(image_b, pose_b, landmark, pair_b[i]);
if (!std::isfinite(error_a) || !std::isfinite(error_b) ||
std::max(error_a, error_b) > parameters->reprojection_threshold_px)
{
if (!std::isfinite(error_a) || !std::isfinite(error_b))
++result->rejected_behind_camera;
else
++result->rejected_reprojection;
continue;
}
landmark.rmse = std::sqrt((error_a * error_a + error_b * error_b) / 2.0);
landmark.median = std::max(error_a, error_b);
seed_landmarks.push_back(landmark);
}
if (seed_landmarks.size() < parameters->minimum_seed_landmarks) continue;
cameras.push_back(pose_a);
cameras.push_back(pose_b);
registered.insert(candidate.a);
registered.insert(candidate.b);
for (Landmark &landmark : seed_landmarks) {
accepted_tracks.insert(landmark.track_id);
landmarks.push_back(landmark);
}
seeded = true;
result->seed_image_a = candidate.a;
result->seed_image_b = candidate.b;
break;
}
if (!seeded) continue;
bool stopped_without_growth = false;
for (uint32_t round = 0;
round < parameters->maximum_registration_rounds; ++round) {
bool all_component_images_registered = true;
for (size_t image_position : component.image_indices)
if (!registered.count(images[image_position].id)) {
all_component_images_registered = false;
break;
}
if (all_component_images_registered) break;
++result->registration_rounds;
struct CandidateImage { uint64_t id; uint32_t support; };
std::vector<CandidateImage> image_candidates;
for (size_t image_position : component.image_indices) {
const uint64_t image_id = images[image_position].id;
if (registered.count(image_id)) continue;
uint32_t support = 0;
for (const Landmark &landmark : landmarks) {
for (const Observation &observation : tracks_by_id[landmark.track_id].observations)
if (observation.image_id == image_id) { ++support; break; }
}
if (support >= parameters->minimum_pnp_correspondences)
image_candidates.push_back({image_id, support});
}
std::sort(image_candidates.begin(), image_candidates.end(),
[](const CandidateImage &a, const CandidateImage &b) {
if (a.support != b.support) return a.support > b.support;
return a.id < b.id;
});
if (image_candidates.empty()) {
stopped_without_growth = true;
break;
}
bool registered_one = false;
for (const CandidateImage &candidate : image_candidates) {
const Image &image = images[image_index[candidate.id]];
std::vector<Lardon3DSparseGeometryPoint3> points;
std::vector<Lardon3DSparseGeometryPoint2> pixels;
std::vector<uint64_t> track_ids;
for (const Landmark &landmark : landmarks) {
const Track &track = tracks_by_id[landmark.track_id];
for (const Observation &observation : track.observations)
if (observation.image_id == candidate.id) {
points.push_back(landmark.point);
pixels.push_back(observation.pixel);
track_ids.push_back(landmark.track_id);
break;
}
}
std::vector<uint8_t> mask(points.size());
Lardon3DSparseGeometryPnPResult pnp_result = {};
pnp_result.inlier_mask = mask.data();
pnp_result.inlier_mask_capacity = mask.size();
++result->registration_attempts;
if (lardon3d_sparse_geometry_pnp(
&image.calibration, points.data(), pixels.data(), points.size(),
&parameters->pnp, &pnp_result) != LARDON3D_SPARSE_GEOMETRY_OK ||
!finite_pose(pnp_result.pose_cw))
{
++result->registration_failures;
continue;
}
++result->registration_successes;
result->last_pnp_inlier_count = pnp_result.inlier_count;
Pose pose = {candidate.id, pnp_result.pose_cw};
cameras.push_back(pose);
registered.insert(candidate.id);
registered_one = true;
break;
}
if (!registered_one) {
stopped_without_growth = true;
break;
}
for (Landmark &landmark : landmarks) {
if (landmark.component_key != component.key) continue;
const Track &track = tracks_by_id[landmark.track_id];
size_t eligible_count = 0;
for (const Observation &observation : track.observations)
if (registered.count(observation.image_id)) ++eligible_count;
if (eligible_count <= landmark.observations.size()) continue;
++result->landmark_update_attempts;
++result->triangulation_attempts;
Landmark replacement;
Lardon3DSparseGeometryResult geometry_status =
LARDON3D_SPARSE_GEOMETRY_OK;
const LandmarkCandidateStatus update_status =
build_landmark_candidate(
track, component.key, images, image_index, cameras,
registered, *parameters, &replacement, &geometry_status,
result);
result->last_triangulation_status = geometry_status;
if (update_status == LandmarkCandidateStatus::accepted) {
landmark = std::move(replacement);
++result->landmark_update_successes;
} else {
++result->landmark_update_failures;
record_landmark_rejection(update_status, result);
}
}
size_t added = 0;
for (const auto &entry : tracks_by_id) {
if (accepted_tracks.count(entry.first) || added >= parameters->maximum_landmarks_per_round)
continue;
size_t eligible_count = 0;
for (const Observation &observation : entry.second.observations)
if (registered.count(observation.image_id)) ++eligible_count;
if (eligible_count < 2) continue;
++result->triangulation_attempts;
Landmark landmark;
Lardon3DSparseGeometryResult geometry_status =
LARDON3D_SPARSE_GEOMETRY_OK;
const LandmarkCandidateStatus candidate_status =
build_landmark_candidate(
entry.second, component.key, images, image_index, cameras,
registered, *parameters, &landmark, &geometry_status, result);
result->last_triangulation_status = geometry_status;
if (candidate_status != LandmarkCandidateStatus::accepted) {
record_landmark_rejection(candidate_status, result);
continue;
}
landmarks.push_back(landmark);
accepted_tracks.insert(entry.first);
++added;
}
}
size_t component_registered = 0;
for (size_t image_position : component.image_indices)
if (registered.count(images[image_position].id)) ++component_registered;
if (component_registered < component.image_indices.size()) {
if (stopped_without_growth)
++result->no_growth_terminations;
else
++result->round_limit_terminations;
}
}
std::sort(cameras.begin(), cameras.end(),
[](const Pose &a, const Pose &b) { return a.image_id < b.image_id; });
std::sort(landmarks.begin(), landmarks.end(),
[](const Landmark &a, const Landmark &b) {
if (a.component_key != b.component_key)
return a.component_key < b.component_key;
return a.track_id < b.track_id;
});
for (const Component &component : components) {
for (auto &output : output_components)
if (output.component_key == component.key) {
for (const Pose &camera : cameras)
if (component_by_image[camera.image_id] ==
component_by_image[component.key]) ++output.registered_image_count;
for (const Landmark &landmark : landmarks)
if (landmark.component_key == component.key) ++output.landmark_count;
}
for (size_t image_index_value : component.image_indices) {
const uint64_t image_id = images[image_index_value].id;
if (!registered.count(image_id))
result->unregistered_image_count++;
}
}
result->component_count = output_components.size();
result->camera_count = cameras.size();
result->landmark_count = landmarks.size();
if (result->component_count) {
result->components = static_cast<Lardon3DSparseIncrementalComponent *>(
std::malloc(result->component_count * sizeof(*result->components)));
if (!result->components) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; }
std::copy(output_components.begin(), output_components.end(), result->components);
}
if (result->camera_count) {
result->cameras = static_cast<Lardon3DSparseIncrementalCamera *>(
std::malloc(result->camera_count * sizeof(*result->cameras)));
if (!result->cameras) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; }
for (size_t i = 0; i < cameras.size(); ++i)
result->cameras[i] = {
cameras[i].image_id,
components[component_by_image[cameras[i].image_id]].key,
cameras[i].value};
}
if (result->landmark_count) {
result->landmarks = static_cast<Lardon3DSparseIncrementalLandmark *>(
std::malloc(result->landmark_count * sizeof(*result->landmarks)));
if (!result->landmarks) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; }
size_t observation_count = 0;
for (const Landmark &landmark : landmarks) observation_count += landmark.observations.size();
result->observation_count = observation_count;
result->observations = static_cast<Lardon3DSparseIncrementalLandmarkObservation *>(
std::malloc(observation_count * sizeof(*result->observations)));
if (!result->observations) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; }
size_t observation_offset = 0;
for (size_t i = 0; i < landmarks.size(); ++i) {
const Landmark &landmark = landmarks[i];
result->landmarks[i] = {landmark.track_id, landmark.track_id, landmark.component_key,
landmark.point, landmark.rmse, landmark.median,
landmark.observations.size()};
for (size_t position = 0; position < landmark.observations.size(); ++position) {
const Observation &observation = landmark.observations[position];
result->observations[observation_offset++] = {
landmark.track_id, landmark.track_id, observation.image_id,
observation.feature_set_id, observation.feature_index,
static_cast<uint32_t>(position)};
}
}
}
if (result->unregistered_image_count) {
result->unregistered_images = static_cast<Lardon3DSparseIncrementalUnregisteredImage *>(
std::malloc(result->unregistered_image_count * sizeof(*result->unregistered_images)));
if (!result->unregistered_images) { destroy_result(result); return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY; }
size_t offset = 0;
for (const Image &image : images)
if (!registered.count(image.id))
result->unregistered_images[offset++] = {
image.id, components[component_by_image[image.id]].key};
}
result->track_set_id = input->track_set_id;
result->calibration_scope_id = input->calibration_scope_id;
result->status = result->camera_count == 0
? LARDON3D_SPARSE_INCREMENTAL_FAILED
: result->unregistered_image_count == 0
? LARDON3D_SPARSE_INCREMENTAL_COMPLETE
: LARDON3D_SPARSE_INCREMENTAL_PARTIAL;
return result->status;
} catch (const std::bad_alloc &) {
destroy_result(result);
return LARDON3D_SPARSE_INCREMENTAL_OUT_OF_MEMORY;
} catch (...) {
destroy_result(result);
return LARDON3D_SPARSE_INCREMENTAL_FAILED;
}
}
extern "C" void lardon3d_sparse_incremental_result_destroy(
Lardon3DSparseIncrementalResult *result) {
destroy_result(result);
}

91
tests/benchmark_sparse_sfm_incremental.cpp Normal file
View file

@ -0,0 +1,91 @@
#include <lardon3d/sparse_sfm_incremental.h>
#include <cstdio>
#include <cstdlib>
#include <chrono>
#include <dirent.h>
#include <sys/resource.h>
#include <vector>
static size_t open_file_descriptor_count() {
DIR *directory = opendir("/proc/self/fd");
if (!directory) return 0;
size_t count = 0;
while (readdir(directory)) ++count;
closedir(directory);
return count >= 2 ? count - 2 : 0;
}
static Lardon3DSparseGeometryPoint2 project(
const Lardon3DSparseGeometryCalibration &calibration,
const Lardon3DSparseGeometryPose &pose,
const Lardon3DSparseGeometryPoint3 &point) {
const double x = point.x + pose.translation_cw[0];
const double y = point.y + pose.translation_cw[1];
const double z = point.z + pose.translation_cw[2];
return {calibration.fx * x / z + calibration.cx,
calibration.fy * y / z + calibration.cy};
}
int main(int argc, char **argv) {
const size_t camera_count = argc > 1 ? std::strtoull(argv[1], nullptr, 10) : 8;
const size_t track_count = argc > 2 ? std::strtoull(argv[2], nullptr, 10) : 500;
if (camera_count < 2 || track_count < 6 || camera_count > 64 ||
track_count > 10000)
return EXIT_FAILURE;
const Lardon3DSparseGeometryCalibration calibration = {
4000, 3000, 2000.0, 2000.0, 2000.0, 1500.0, 0.0, 0.0, 0.0, 0.0};
std::vector<Lardon3DSparseIncrementalImage> images;
std::vector<Lardon3DSparseIncrementalObservation> observations;
images.reserve(camera_count);
observations.reserve(camera_count * track_count);
for (size_t camera = 0; camera < camera_count; ++camera)
images.push_back({100 + camera, calibration});
for (size_t track = 0; track < track_count; ++track) {
const Lardon3DSparseGeometryPoint3 point = {
-1.5 + static_cast<double>(track % 31) * 0.1,
-1.0 + static_cast<double>((track / 31) % 23) * 0.09,
8.0 + static_cast<double>(track % 17) * 0.07};
for (size_t camera = 0; camera < camera_count; ++camera) {
Lardon3DSparseGeometryPose pose = {
{1, 0, 0, 0, 1, 0, 0, 0, 1},
{static_cast<double>(camera) * 0.3, 0, 0}};
const Lardon3DSparseGeometryPoint2 pixel = project(calibration, pose, point);
observations.push_back({track + 1, 100 + camera, 1000 + camera,
static_cast<uint32_t>(track), 8192,
pixel.x, pixel.y});
}
}
Lardon3DSparseIncrementalParameters parameters;
if (!lardon3d_sparse_incremental_parameters_default(&parameters)) return EXIT_FAILURE;
parameters.maximum_registration_rounds = 64;
Lardon3DSparseIncrementalInput input = {
1, 2, images.data(), images.size(), observations.data(), observations.size()};
Lardon3DSparseIncrementalResult result = {};
const auto started = std::chrono::steady_clock::now();
const Lardon3DSparseIncrementalStatus status =
lardon3d_sparse_incremental_run(&input, &parameters, &result);
const auto stopped = std::chrono::steady_clock::now();
struct rusage usage = {};
getrusage(RUSAGE_SELF, &usage);
const double wall_seconds =
std::chrono::duration<double>(stopped - started).count();
const double cpu_seconds =
static_cast<double>(usage.ru_utime.tv_sec + usage.ru_stime.tv_sec) +
static_cast<double>(usage.ru_utime.tv_usec + usage.ru_stime.tv_usec) /
1000000.0;
std::printf("status=%d cameras=%zu tracks=%zu observations=%zu components=%zu "
"registered=%zu landmarks=%zu rounds=%llu seeds=%llu "
"wall_s=%.6f cpu_s=%.6f peak_rss_kib=%ld fds=%zu\n",
static_cast<int>(status), camera_count, track_count,
observations.size(), result.component_count, result.camera_count,
result.landmark_count,
static_cast<unsigned long long>(result.registration_rounds),
static_cast<unsigned long long>(result.seed_candidates_considered),
wall_seconds, cpu_seconds, usage.ru_maxrss,
open_file_descriptor_count());
const bool valid = status == LARDON3D_SPARSE_INCREMENTAL_COMPLETE &&
result.camera_count == camera_count && result.landmark_count > 0;
lardon3d_sparse_incremental_result_destroy(&result);
return valid ? EXIT_SUCCESS : EXIT_FAILURE;
}

1364
tests/test_sparse_sfm_incremental.cpp Normal file
View file

File diff suppressed because it is too large Load diff