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

sparse-sfm: complete Gate C geometry validation

Browse source
This commit is contained in:
fy59 2026-08-10 13:22:04 +02:00
parent 95700de339
commit 67529791c4
7 changed files with 1464 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
docs/architecture/project_database.md
View file

@ -781,6 +781,9 @@ validation, migration v15→v16 et comparateur fresh/migrated validés par Gate
Le modèle de persistance est gelé pour v1 ; le solveur numérique reste hors de Le modèle de persistance est gelé pour v1 ; le solveur numérique reste hors de
Project DB v16. Project DB v16.
Gate C ajoute uniquement des primitives numériques pures hors Project DB; aucune
table, migration ou identité v16 supplémentaire n'est introduite.
**IMPLEMENTED** — API C Track Model v1 : header `project_db.h` et **IMPLEMENTED** — API C Track Model v1 : header `project_db.h` et
source `project_db.c` exposent `create_track_set`, `load_track_set`, source `project_db.c` exposent `create_track_set`, `load_track_set`,
`find_track_set`, `list_track_sets`, `load_track`, `list_tracks`, `find_track_set`, `list_track_sets`, `load_track`, `list_tracks`,

10
docs/architecture/reconstruction_pipeline.md
View file

@ -124,15 +124,17 @@ USAC/MAGSAC avec configuration, seed et fingerprint déterministes.
**Statut :** COMPLETED/FROZEN — le Track Builder v1 direct et durable est **Statut :** COMPLETED/FROZEN — le Track Builder v1 direct et durable est
implémenté dans Project DB v15 (`track_sets`, `tracks`, `track_observations` et implémenté dans Project DB v15 (`track_sets`, `tracks`, `track_observations` et
le payload de tâche). La triangulation et le solveur Sparse SfM restent PLANNED; le payload de tâche). Les primitives de géométrie calibrée Gate C sont
le modèle de persistance Sparse SfM v16 est gelé après Gate B. implémentées; le solveur incrémental complet et l'orchestration restent 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 **Sparse SfM Gate A : PASS.** Le contrat géométrique, la stratégie
incremental, la triangulation candidate, le gauge, les conventions de pose, incremental, la triangulation candidate, le gauge, les conventions de pose,
les limites BA et l'enveloppe matérielle sont documentés dans les limites BA et l'enveloppe matérielle sont documentés dans
`architecture/sparse_sfm.md`. Le solveur Sparse SfM reste `architecture/sparse_sfm.md`. Le solveur Sparse SfM reste
**NOT_IMPLEMENTED** jusqu'aux Gates B3–G; sa **NOT_IMPLEMENTED** jusqu'aux Gates D–G; ses primitives pures calibrées sont
persistance v16 et ses lecteurs bornés sont implémentés en B2. implémentées en Gate C, tandis que sa persistance v16 et ses lecteurs bornés
restent ceux de B2.
--- ---

54
docs/architecture/sparse_sfm.md
View file

@ -383,6 +383,60 @@ metric alignment, persistent reconstruction schema and durable SfM checkpoints.
Their semantic ownership is decided here; their final numeric values require Their semantic ownership is decided here; their final numeric values require
the synthetic ground-truth and sparse-solver gates. the synthetic ground-truth and sparse-solver gates.
## Gate C — pure calibrated geometry
**GATE C — PASS.** Pure calibrated geometry primitives, synthetic ground truth,
degeneracy rejection, determinism, normal suite and ASan/UBSan validation are
complete. Incremental orchestration, BA and persistent geometry integration
remain later gates.
Gate C keeps geometry outside Project DB and exposes a C17-safe, synchronous
pure-primitive boundary. Inputs are binary64 calibrated pixels, fixed
world-to-camera poses, and caller-owned correspondence arrays; no primitive
opens SQLite, reads Feature Files, loads images or invokes the Task Runtime.
The v1 candidate uses OpenCV 5.0.0 `calib3d` operations with every scientific
parameter supplied by an explicit configuration structure. Public outputs use
row-major binary64 `R_cw` and `t_cw`; relative translation has unit norm and no
metric interpretation.
The candidate contract requires deterministic caller ordering, finite inputs,
explicit robust-estimator thresholds/confidence/iteration limits and a local
seed. Essential hypotheses are accepted only after explicit positive-depth
support, rotation validation, parallax and reprojection checks. Pure rotation,
low parallax, weak conditioning and non-finite results are failures. Two-view
and multi-view points use normalized-coordinate linear DLT followed by bounded
point-only binary64 refinement; PnP returns world-to-camera pose with explicit
cheirality and inlier diagnostics. The tested v1 parameter set is frozen by the
Gate C ground-truth and degeneracy evidence; future orchestration may choose
other explicitly fingerprinted configurations.
### Gate C tested threshold set
The pure API has no hidden defaults; callers provide all acceptance settings.
The Gate C reference matrix uses the following reproducible set:
| Parameter | Value | Unit/purpose |
|---|---:|---|
| Relative robust threshold | 1.0 px clean; 1.5 px matrix | pixel residual |
| Relative confidence | 0.999 | RANSAC confidence |
| Relative iterations | 1000 clean; 1500 matrix | iterations |
| Relative minimum inliers | 6 clean; 24 matrix | correspondences |
| Relative minimum ratio | 0.75 clean; 0.5 matrix | fraction |
| Minimum parallax | `1e-4` rad | seed geometry |
| Minimum cheirality ratio | 0.5 | positive depth |
| PnP threshold | 1.0 px clean; 1.5 px matrix | pixel residual |
| PnP confidence | 0.999 | RANSAC confidence |
| PnP iterations | 1000 | iterations |
| PnP minimum inliers | 6 clean; 12 matrix | correspondences |
| PnP minimum ratio | 0.75 clean; 0.5 matrix | fraction |
| Point refinement tolerance | `1e-12` | normalized residual |
| Point refinement iterations | 30 | iterations |
Degeneracy checks use finite values, positive depth, rotation SO(3) residual
`1e-6`, depth epsilon `1e-9`, homogeneous scale epsilon `1e-12`, and
collinearity covariance determinant `1e-10`. These are pure-geometry
parameters and do not alter Project DB identity.
## 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,

138
include/lardon3d/sparse_sfm_geometry.h Normal file
View file

@ -0,0 +1,138 @@
#ifndef LARDON3D_SPARSE_SFM_GEOMETRY_H
#define LARDON3D_SPARSE_SFM_GEOMETRY_H
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
LARDON3D_SPARSE_GEOMETRY_OK = 0,
LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT,
LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT,
LARDON3D_SPARSE_GEOMETRY_INSUFFICIENT_CORRESPONDENCES,
LARDON3D_SPARSE_GEOMETRY_ESTIMATION_FAILED,
LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX,
LARDON3D_SPARSE_GEOMETRY_DEGENERATE,
LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED,
LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE
} Lardon3DSparseGeometryResult;
typedef struct {
uint32_t width;
uint32_t height;
double fx;
double fy;
double cx;
double cy;
double k1;
double k2;
double p1;
double p2;
} Lardon3DSparseGeometryCalibration;
typedef struct {
double x;
double y;
} Lardon3DSparseGeometryPoint2;
typedef struct {
double x;
double y;
double z;
} Lardon3DSparseGeometryPoint3;
typedef struct {
double rotation_cw[9];
double translation_cw[3];
} Lardon3DSparseGeometryPose;
typedef struct {
double robust_threshold_px;
double confidence;
uint32_t max_iterations;
uint32_t minimum_inliers;
double minimum_inlier_ratio;
double minimum_parallax_rad;
double minimum_cheirality_ratio;
uint64_t deterministic_seed;
} Lardon3DSparseGeometryRelativePoseParameters;
typedef struct {
Lardon3DSparseGeometryPose pose_ba;
uint32_t inlier_count;
double inlier_ratio;
double median_parallax_rad;
uint8_t *inlier_mask;
size_t inlier_mask_capacity;
} Lardon3DSparseGeometryRelativePoseResult;
typedef struct {
double reprojection_threshold_px;
double confidence;
uint32_t max_iterations;
uint32_t minimum_inliers;
double minimum_inlier_ratio;
uint64_t deterministic_seed;
} Lardon3DSparseGeometryPnPParameters;
typedef struct {
Lardon3DSparseGeometryPose pose_cw;
uint32_t inlier_count;
double inlier_ratio;
uint8_t *inlier_mask;
size_t inlier_mask_capacity;
} Lardon3DSparseGeometryPnPResult;
typedef struct {
uint32_t max_iterations;
double convergence_tolerance;
} Lardon3DSparseGeometryPointRefinementParameters;
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_normalize(
const Lardon3DSparseGeometryCalibration *calibration,
const Lardon3DSparseGeometryPoint2 *pixels, size_t count,
Lardon3DSparseGeometryPoint2 *normalized);
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_relative_pose(
const Lardon3DSparseGeometryCalibration *calibration_a,
const Lardon3DSparseGeometryCalibration *calibration_b,
const Lardon3DSparseGeometryPoint2 *pixels_a,
const Lardon3DSparseGeometryPoint2 *pixels_b, size_t count,
const Lardon3DSparseGeometryRelativePoseParameters *parameters,
Lardon3DSparseGeometryRelativePoseResult *result);
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_triangulate_two_view(
const Lardon3DSparseGeometryPoint2 *normalized_a,
const Lardon3DSparseGeometryPoint2 *normalized_b,
const Lardon3DSparseGeometryPose *pose_a,
const Lardon3DSparseGeometryPose *pose_b,
Lardon3DSparseGeometryPoint3 *point);
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_triangulate_multi_view(
const Lardon3DSparseGeometryPoint2 *normalized_points,
const Lardon3DSparseGeometryPose *poses, size_t view_count,
Lardon3DSparseGeometryPoint3 *point);
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_refine_point(
const Lardon3DSparseGeometryPoint2 *normalized_points,
const Lardon3DSparseGeometryPose *poses, size_t view_count,
const Lardon3DSparseGeometryPoint3 *initial_point,
const Lardon3DSparseGeometryPointRefinementParameters *parameters,
Lardon3DSparseGeometryPoint3 *refined_point);
Lardon3DSparseGeometryResult lardon3d_sparse_geometry_pnp(
const Lardon3DSparseGeometryCalibration *calibration,
const Lardon3DSparseGeometryPoint3 *points,
const Lardon3DSparseGeometryPoint2 *pixels, size_t count,
const Lardon3DSparseGeometryPnPParameters *parameters,
Lardon3DSparseGeometryPnPResult *result);
#ifdef __cplusplus
}
#endif
#endif

10
meson.build
View file

@ -144,6 +144,7 @@ executable(
'src/image_view.c', 'src/image_view.c',
'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/task.c', 'src/task.c',
'src/task_checkpoint.c', 'src/task_checkpoint.c',
'src/task_kind_registry.c', 'src/task_kind_registry.c',
@ -523,6 +524,15 @@ sparse_sfm_model_test = executable(
test('sparse-sfm-model', sparse_sfm_model_test, timeout: 30) test('sparse-sfm-model', sparse_sfm_model_test, timeout: 30)
sparse_sfm_geometry_test = executable(
'test-sparse-sfm-geometry',
sources: ['tests/test_sparse_sfm_geometry.cpp', 'src/sparse_sfm_geometry.cpp'],
include_directories: include_directories('include'),
dependencies: [opencv_geometry],
)
test('sparse-sfm-geometry', sparse_sfm_geometry_test, timeout: 60)
sparse_sfm_resource_test = executable( sparse_sfm_resource_test = executable(
'test-sparse-sfm-resource', 'test-sparse-sfm-resource',
sources: [ sources: [

604
src/sparse_sfm_geometry.cpp Normal file
View file

@ -0,0 +1,604 @@
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <vector>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <lardon3d/sparse_sfm_geometry.h>
namespace {
using Result = Lardon3DSparseGeometryResult;
struct RngGuard {
uint64_t saved;
explicit RngGuard(uint64_t seed) : saved(cv::theRNG().state) {
cv::theRNG().state = seed;
}
~RngGuard() { cv::theRNG().state = saved; }
};
bool finite_value(double value) { return std::isfinite(value); }
bool calibration_valid(const Lardon3DSparseGeometryCalibration &calibration) {
return calibration.width > 0 && calibration.height > 0 &&
calibration.fx > 0.0 && calibration.fy > 0.0 &&
calibration.cx >= 0.0 && calibration.cx < calibration.width &&
calibration.cy >= 0.0 && calibration.cy < calibration.height &&
finite_value(calibration.fx) && finite_value(calibration.fy) &&
finite_value(calibration.cx) && finite_value(calibration.cy) &&
finite_value(calibration.k1) && finite_value(calibration.k2) &&
finite_value(calibration.p1) && finite_value(calibration.p2);
}
bool points_finite(const Lardon3DSparseGeometryPoint2 *points, size_t count) {
if (!points)
return false;
for (size_t index = 0; index < count; ++index)
if (!finite_value(points[index].x) || !finite_value(points[index].y))
return false;
return true;
}
cv::Mat camera_matrix(const Lardon3DSparseGeometryCalibration &calibration) {
cv::Mat matrix = cv::Mat::zeros(3, 3, CV_64F);
matrix.at<double>(0, 0) = calibration.fx;
matrix.at<double>(0, 2) = calibration.cx;
matrix.at<double>(1, 1) = calibration.fy;
matrix.at<double>(1, 2) = calibration.cy;
matrix.at<double>(2, 2) = 1.0;
return matrix;
}
cv::Mat distortion(const Lardon3DSparseGeometryCalibration &calibration) {
cv::Mat matrix = cv::Mat::zeros(1, 4, CV_64F);
matrix.at<double>(0, 0) = calibration.k1;
matrix.at<double>(0, 1) = calibration.k2;
matrix.at<double>(0, 2) = calibration.p1;
matrix.at<double>(0, 3) = calibration.p2;
return matrix;
}
bool rotation_valid(const cv::Mat &rotation) {
if (rotation.rows != 3 || rotation.cols != 3)
return false;
cv::Mat error = rotation.t() * rotation - cv::Mat::eye(3, 3, CV_64F);
double determinant = cv::determinant(rotation);
return cv::norm(error, cv::NORM_INF) < 1e-6 &&
std::abs(determinant - 1.0) < 1e-6;
}
void copy_pose(const cv::Mat &rotation, const cv::Mat &translation,
Lardon3DSparseGeometryPose *pose) {
for (int row = 0; row < 3; ++row)
for (int column = 0; column < 3; ++column)
pose->rotation_cw[row * 3 + column] = rotation.at<double>(row, column);
for (int index = 0; index < 3; ++index)
pose->translation_cw[index] = translation.at<double>(index, 0);
}
bool pose_finite(const Lardon3DSparseGeometryPose &pose) {
for (double value : pose.rotation_cw)
if (!finite_value(value))
return false;
for (double value : pose.translation_cw)
if (!finite_value(value))
return false;
return true;
}
cv::Mat pose_rotation(const Lardon3DSparseGeometryPose &pose) {
cv::Mat matrix(3, 3, CV_64F);
for (int row = 0; row < 3; ++row)
for (int column = 0; column < 3; ++column)
matrix.at<double>(row, column) = pose.rotation_cw[row * 3 + column];
return matrix;
}
cv::Mat pose_projection(const Lardon3DSparseGeometryPose &pose) {
cv::Mat projection = cv::Mat::zeros(3, 4, CV_64F);
for (int row = 0; row < 3; ++row) {
for (int column = 0; column < 3; ++column)
projection.at<double>(row, column) = pose.rotation_cw[row * 3 + column];
projection.at<double>(row, 3) = pose.translation_cw[row];
}
return projection;
}
bool triangulated_point_valid(const cv::Mat &homogeneous,
const cv::Mat &rotation_b,
const cv::Mat &translation_b,
cv::Mat *point) {
double scale = homogeneous.at<double>(3, 0);
if (!finite_value(scale) || std::abs(scale) < 1e-12)
return false;
cv::Mat candidate = homogeneous.rowRange(0, 3) / scale;
double depth_a = candidate.at<double>(2, 0);
cv::Mat point_b = rotation_b * candidate + translation_b;
double depth_b = point_b.at<double>(2, 0);
if (!finite_value(depth_a) || !finite_value(depth_b) || depth_a <= 1e-9 ||
depth_b <= 1e-9 || !cv::checkRange(candidate))
return false;
*point = candidate;
return true;
}
bool parallax_valid(const Lardon3DSparseGeometryPoint2 &point_a,
const Lardon3DSparseGeometryPoint2 &point_b,
const cv::Mat &rotation_b) {
cv::Mat ray_a(3, 1, CV_64F);
cv::Mat ray_b(3, 1, CV_64F);
ray_a.at<double>(0, 0) = point_a.x;
ray_a.at<double>(1, 0) = point_a.y;
ray_a.at<double>(2, 0) = 1.0;
ray_b.at<double>(0, 0) = point_b.x;
ray_b.at<double>(1, 0) = point_b.y;
ray_b.at<double>(2, 0) = 1.0;
ray_a /= cv::norm(ray_a);
ray_b = rotation_b.t() * ray_b;
ray_b /= cv::norm(ray_b);
double cosine = ray_a.dot(ray_b);
return std::acos(std::clamp(cosine, -1.0, 1.0)) >= 1e-4;
}
} // namespace
extern "C" Lardon3DSparseGeometryResult lardon3d_sparse_geometry_normalize(
const Lardon3DSparseGeometryCalibration *calibration,
const Lardon3DSparseGeometryPoint2 *pixels, size_t count,
Lardon3DSparseGeometryPoint2 *normalized) {
if (!calibration || !pixels || !normalized || count == 0)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (!calibration_valid(*calibration))
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (!points_finite(pixels, count))
return LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT;
try {
std::vector<cv::Point2d> source;
source.reserve(count);
for (size_t index = 0; index < count; ++index)
source.emplace_back(pixels[index].x, pixels[index].y);
std::vector<cv::Point2d> undistorted;
cv::undistortPoints(source, undistorted, camera_matrix(*calibration),
distortion(*calibration));
if (undistorted.size() != count)
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
for (size_t index = 0; index < count; ++index) {
normalized[index].x = undistorted[index].x;
normalized[index].y = undistorted[index].y;
if (!finite_value(normalized[index].x) ||
!finite_value(normalized[index].y))
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
return LARDON3D_SPARSE_GEOMETRY_OK;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}
extern "C" Lardon3DSparseGeometryResult lardon3d_sparse_geometry_relative_pose(
const Lardon3DSparseGeometryCalibration *calibration_a,
const Lardon3DSparseGeometryCalibration *calibration_b,
const Lardon3DSparseGeometryPoint2 *pixels_a,
const Lardon3DSparseGeometryPoint2 *pixels_b, size_t count,
const Lardon3DSparseGeometryRelativePoseParameters *parameters,
Lardon3DSparseGeometryRelativePoseResult *result) {
if (!calibration_a || !calibration_b || !pixels_a || !pixels_b ||
!parameters || !result || count > static_cast<size_t>(INT_MAX) ||
parameters->max_iterations == 0 || parameters->confidence <= 0.0 ||
parameters->confidence >= 1.0 || parameters->robust_threshold_px <= 0.0 ||
parameters->minimum_parallax_rad < 0.0 ||
parameters->minimum_cheirality_ratio <= 0.0 ||
parameters->minimum_cheirality_ratio > 1.0)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (count < 5)
return LARDON3D_SPARSE_GEOMETRY_INSUFFICIENT_CORRESPONDENCES;
if (!calibration_valid(*calibration_a) || !calibration_valid(*calibration_b))
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (!points_finite(pixels_a, count) || !points_finite(pixels_b, count))
return LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT;
result->inlier_count = 0;
result->inlier_ratio = 0.0;
result->median_parallax_rad = 0.0;
if (result->inlier_mask && result->inlier_mask_capacity < count)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
try {
std::vector<cv::Point2d> normalized_a(count), normalized_b(count);
Lardon3DSparseGeometryResult status =
lardon3d_sparse_geometry_normalize(calibration_a, pixels_a, count,
reinterpret_cast<
Lardon3DSparseGeometryPoint2 *>(
normalized_a.data()));
if (status != LARDON3D_SPARSE_GEOMETRY_OK)
return status;
status = lardon3d_sparse_geometry_normalize(
calibration_b, pixels_b, count,
reinterpret_cast<Lardon3DSparseGeometryPoint2 *>(normalized_b.data()));
if (status != LARDON3D_SPARSE_GEOMETRY_OK)
return status;
double mean_ax = 0.0;
double mean_ay = 0.0;
double mean_bx = 0.0;
double mean_by = 0.0;
for (size_t index = 0; index < count; ++index) {
mean_ax += normalized_a[index].x;
mean_ay += normalized_a[index].y;
mean_bx += normalized_b[index].x;
mean_by += normalized_b[index].y;
}
mean_ax /= static_cast<double>(count);
mean_ay /= static_cast<double>(count);
mean_bx /= static_cast<double>(count);
mean_by /= static_cast<double>(count);
double covariance_axx = 0.0;
double covariance_ayy = 0.0;
double covariance_axy = 0.0;
double covariance_bxx = 0.0;
double covariance_byy = 0.0;
double covariance_bxy = 0.0;
for (size_t index = 0; index < count; ++index) {
double ax = normalized_a[index].x - mean_ax;
double ay = normalized_a[index].y - mean_ay;
double bx = normalized_b[index].x - mean_bx;
double by = normalized_b[index].y - mean_by;
covariance_axx += ax * ax;
covariance_ayy += ay * ay;
covariance_axy += ax * ay;
covariance_bxx += bx * bx;
covariance_byy += by * by;
covariance_bxy += bx * by;
}
if (covariance_axx * covariance_ayy - covariance_axy * covariance_axy <
1e-10 ||
covariance_bxx * covariance_byy - covariance_bxy * covariance_bxy <
1e-10)
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
cv::Mat points_a(static_cast<int>(count), 2, CV_64F, normalized_a.data());
cv::Mat points_b(static_cast<int>(count), 2, CV_64F, normalized_b.data());
cv::Mat mask;
RngGuard rng(parameters->deterministic_seed);
double normalized_threshold = parameters->robust_threshold_px /
std::max(calibration_a->fx,
calibration_a->fy);
cv::Mat essential = cv::findEssentialMat(
points_a, points_b, 1.0, cv::Point2d(0, 0), cv::RANSAC,
parameters->confidence, normalized_threshold,
static_cast<int>(parameters->max_iterations), mask);
if (essential.empty())
return LARDON3D_SPARSE_GEOMETRY_ESTIMATION_FAILED;
cv::Mat rotation, translation;
int inliers = cv::recoverPose(essential, points_a, points_b, rotation,
translation, 1.0, cv::Point2d(0, 0), mask);
double count_value = static_cast<double>(count);
if (inliers < static_cast<int>(parameters->minimum_inliers) ||
static_cast<double>(inliers) / count_value <
parameters->minimum_inlier_ratio ||
!rotation_valid(rotation) || cv::norm(translation) < 1e-12)
return LARDON3D_SPARSE_GEOMETRY_ESTIMATION_FAILED;
std::vector<double> parallaxes;
cv::Mat projection_a = cv::Mat::zeros(3, 4, CV_64F);
projection_a.at<double>(0, 0) = projection_a.at<double>(1, 1) =
projection_a.at<double>(2, 2) = 1.0;
cv::Mat projection_b = cv::Mat::zeros(3, 4, CV_64F);
rotation.copyTo(projection_b.colRange(0, 3));
translation.copyTo(projection_b.col(3));
cv::Mat points_4d;
cv::triangulatePoints(projection_a, projection_b, points_a.t(), points_b.t(),
points_4d);
for (int index = 0; index < points_4d.cols; ++index) {
if (!mask.at<unsigned char>(index))
continue;
cv::Mat point;
if (!triangulated_point_valid(points_4d.col(index), rotation, translation,
&point))
continue;
cv::Vec3d ray_a(normalized_a[index].x, normalized_a[index].y, 1.0);
cv::Vec3d ray_b(normalized_b[index].x, normalized_b[index].y, 1.0);
ray_a = ray_a / cv::norm(ray_a);
ray_b = ray_b / cv::norm(ray_b);
cv::Mat ray_b_input(3, 1, CV_64F);
ray_b_input.at<double>(0, 0) = ray_b[0];
ray_b_input.at<double>(1, 0) = ray_b[1];
ray_b_input.at<double>(2, 0) = ray_b[2];
cv::Mat ray_b_mat = rotation.t() * ray_b_input;
ray_b = cv::Vec3d(ray_b_mat.at<double>(0, 0),
ray_b_mat.at<double>(1, 0),
ray_b_mat.at<double>(2, 0));
double cosine = std::clamp(ray_a.dot(ray_b), -1.0, 1.0);
parallaxes.push_back(std::acos(cosine));
}
if (parallaxes.empty() ||
parallaxes.size() < static_cast<size_t>(
static_cast<double>(inliers) *
parameters->minimum_cheirality_ratio))
return LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED;
std::sort(parallaxes.begin(), parallaxes.end());
double median = parallaxes[parallaxes.size() / 2];
result->median_parallax_rad = median;
if (median < parameters->minimum_parallax_rad)
return LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX;
copy_pose(rotation, translation, &result->pose_ba);
result->inlier_count = static_cast<uint32_t>(inliers);
result->inlier_ratio = static_cast<double>(inliers) / count_value;
if (result->inlier_mask)
for (size_t index = 0; index < count; ++index)
result->inlier_mask[index] = mask.at<unsigned char>(static_cast<int>(index));
return LARDON3D_SPARSE_GEOMETRY_OK;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}
extern "C" Lardon3DSparseGeometryResult
lardon3d_sparse_geometry_triangulate_two_view(
const Lardon3DSparseGeometryPoint2 *normalized_a,
const Lardon3DSparseGeometryPoint2 *normalized_b,
const Lardon3DSparseGeometryPose *pose_a,
const Lardon3DSparseGeometryPose *pose_b,
Lardon3DSparseGeometryPoint3 *point) {
if (!normalized_a || !normalized_b || !pose_a || !pose_b || !point ||
!pose_finite(*pose_a) || !pose_finite(*pose_b) ||
!rotation_valid(pose_rotation(*pose_a)) ||
!rotation_valid(pose_rotation(*pose_b)) ||
!finite_value(normalized_a->x) || !finite_value(normalized_a->y) ||
!finite_value(normalized_b->x) || !finite_value(normalized_b->y))
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
try {
cv::Mat points_a(2, 1, CV_64F);
cv::Mat points_b(2, 1, CV_64F);
points_a.at<double>(0, 0) = normalized_a->x;
points_a.at<double>(1, 0) = normalized_a->y;
points_b.at<double>(0, 0) = normalized_b->x;
points_b.at<double>(1, 0) = normalized_b->y;
cv::Mat homogeneous;
cv::triangulatePoints(pose_projection(*pose_a), pose_projection(*pose_b),
points_a, points_b, homogeneous);
cv::Mat candidate;
cv::Mat rotation_b = pose_rotation(*pose_b);
cv::Mat translation_b(3, 1, CV_64F);
for (int index = 0; index < 3; ++index)
translation_b.at<double>(index, 0) = pose_b->translation_cw[index];
if (!parallax_valid(*normalized_a, *normalized_b, rotation_b))
return LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX;
if (!triangulated_point_valid(homogeneous.col(0), rotation_b,
translation_b, &candidate))
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
point->x = candidate.at<double>(0, 0);
point->y = candidate.at<double>(1, 0);
point->z = candidate.at<double>(2, 0);
return LARDON3D_SPARSE_GEOMETRY_OK;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}
extern "C" Lardon3DSparseGeometryResult
lardon3d_sparse_geometry_triangulate_multi_view(
const Lardon3DSparseGeometryPoint2 *normalized_points,
const Lardon3DSparseGeometryPose *poses, size_t view_count,
Lardon3DSparseGeometryPoint3 *point) {
if (!normalized_points || !poses || !point || view_count < 2 ||
view_count > static_cast<size_t>(INT_MAX / 2))
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
try {
cv::Mat design = cv::Mat::zeros(static_cast<int>(view_count * 2), 4,
CV_64F);
for (size_t view = 0; view < view_count; ++view) {
if (!pose_finite(poses[view]) || !rotation_valid(pose_rotation(poses[view])) ||
!finite_value(normalized_points[view].x) ||
!finite_value(normalized_points[view].y))
return LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT;
cv::Mat projection = pose_projection(poses[view]);
int row = static_cast<int>(view * 2);
design.row(row) = normalized_points[view].x * projection.row(2) -
projection.row(0);
design.row(row + 1) = normalized_points[view].y * projection.row(2) -
projection.row(1);
}
cv::SVD decomposition(design, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
cv::Mat homogeneous = decomposition.vt.row(3).t();
cv::Mat candidate;
cv::Mat identity_rotation = cv::Mat::eye(3, 3, CV_64F);
cv::Mat zero_translation = cv::Mat::zeros(3, 1, CV_64F);
if (!triangulated_point_valid(homogeneous, identity_rotation,
zero_translation, &candidate))
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
for (size_t view = 0; view < view_count; ++view) {
cv::Mat camera_point = pose_rotation(poses[view]) * candidate;
for (int index = 0; index < 3; ++index)
camera_point.at<double>(index, 0) += poses[view].translation_cw[index];
if (!finite_value(camera_point.at<double>(2, 0)) ||
camera_point.at<double>(2, 0) <= 1e-9)
return LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED;
}
if (!parallax_valid(normalized_points[0], normalized_points[1],
pose_rotation(poses[1])))
return LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX;
point->x = candidate.at<double>(0, 0);
point->y = candidate.at<double>(1, 0);
point->z = candidate.at<double>(2, 0);
return LARDON3D_SPARSE_GEOMETRY_OK;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}
extern "C" Lardon3DSparseGeometryResult lardon3d_sparse_geometry_refine_point(
const Lardon3DSparseGeometryPoint2 *normalized_points,
const Lardon3DSparseGeometryPose *poses, size_t view_count,
const Lardon3DSparseGeometryPoint3 *initial_point,
const Lardon3DSparseGeometryPointRefinementParameters *parameters,
Lardon3DSparseGeometryPoint3 *refined_point) {
if (!normalized_points || !poses || !initial_point || !parameters ||
!refined_point || view_count < 2 || parameters->max_iterations == 0 ||
parameters->convergence_tolerance <= 0.0)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (!finite_value(initial_point->x) || !finite_value(initial_point->y) ||
!finite_value(initial_point->z))
return LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT;
cv::Mat point(3, 1, CV_64F);
point.at<double>(0, 0) = initial_point->x;
point.at<double>(1, 0) = initial_point->y;
point.at<double>(2, 0) = initial_point->z;
try {
auto objective = [&](const cv::Mat &candidate, cv::Mat *residual) {
cv::Mat values = cv::Mat::zeros(static_cast<int>(view_count * 2), 1,
CV_64F);
double sum = 0.0;
for (size_t view = 0; view < view_count; ++view) {
cv::Mat camera_point = pose_rotation(poses[view]) * candidate;
for (int index = 0; index < 3; ++index)
camera_point.at<double>(index, 0) += poses[view].translation_cw[index];
double z = camera_point.at<double>(2, 0);
if (!finite_value(z) || z <= 1e-9)
return std::numeric_limits<double>::infinity();
double x = camera_point.at<double>(0, 0) / z;
double y = camera_point.at<double>(1, 0) / z;
values.at<double>(static_cast<int>(view * 2), 0) =
x - normalized_points[view].x;
values.at<double>(static_cast<int>(view * 2 + 1), 0) =
y - normalized_points[view].y;
sum += values.at<double>(static_cast<int>(view * 2), 0) *
values.at<double>(static_cast<int>(view * 2), 0) +
values.at<double>(static_cast<int>(view * 2 + 1), 0) *
values.at<double>(static_cast<int>(view * 2 + 1), 0);
}
*residual = values;
return sum;
};
cv::Mat residual;
double current = objective(point, &residual);
if (!std::isfinite(current))
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
for (uint32_t iteration = 0; iteration < parameters->max_iterations;
++iteration) {
cv::Mat jacobian(static_cast<int>(view_count * 2), 3, CV_64F);
const double step = 1e-7;
for (int axis = 0; axis < 3; ++axis) {
cv::Mat perturbed = point.clone();
perturbed.at<double>(axis, 0) += step;
cv::Mat shifted;
if (!std::isfinite(objective(perturbed, &shifted)))
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
jacobian.col(axis) = (shifted - residual) / step;
}
cv::Mat normal = jacobian.t() * jacobian;
cv::Mat rhs = -jacobian.t() * residual;
cv::Mat delta;
if (!cv::solve(normal, rhs, delta, cv::DECOMP_SVD))
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
cv::Mat candidate = point + delta;
cv::Mat candidate_residual;
double next = objective(candidate, &candidate_residual);
if (!std::isfinite(next) || next > current)
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
point = candidate;
residual = candidate_residual;
if (cv::norm(delta) <= parameters->convergence_tolerance ||
std::abs(current - next) <= parameters->convergence_tolerance) {
refined_point->x = point.at<double>(0, 0);
refined_point->y = point.at<double>(1, 0);
refined_point->z = point.at<double>(2, 0);
return LARDON3D_SPARSE_GEOMETRY_OK;
}
current = next;
}
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}
extern "C" Lardon3DSparseGeometryResult lardon3d_sparse_geometry_pnp(
const Lardon3DSparseGeometryCalibration *calibration,
const Lardon3DSparseGeometryPoint3 *points,
const Lardon3DSparseGeometryPoint2 *pixels, size_t count,
const Lardon3DSparseGeometryPnPParameters *parameters,
Lardon3DSparseGeometryPnPResult *result) {
if (!calibration || !points || !pixels || !parameters || !result ||
count < 4 || count > static_cast<size_t>(INT_MAX) ||
parameters->max_iterations == 0 || parameters->confidence <= 0.0 ||
parameters->confidence >= 1.0 || parameters->reprojection_threshold_px <= 0.0 ||
parameters->minimum_inlier_ratio <= 0.0 ||
parameters->minimum_inlier_ratio > 1.0)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (!calibration_valid(*calibration))
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
if (result->inlier_mask && result->inlier_mask_capacity < count)
return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT;
for (size_t index = 0; index < count; ++index)
if (!finite_value(points[index].x) || !finite_value(points[index].y) ||
!finite_value(points[index].z) || !finite_value(pixels[index].x) ||
!finite_value(pixels[index].y))
return LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT;
try {
std::vector<cv::Point3d> object_points;
std::vector<cv::Point2d> image_points;
object_points.reserve(count);
image_points.reserve(count);
for (size_t index = 0; index < count; ++index) {
object_points.emplace_back(points[index].x, points[index].y,
points[index].z);
image_points.emplace_back(pixels[index].x, pixels[index].y);
}
cv::Mat object_matrix(static_cast<int>(count), 3, CV_64F);
cv::Scalar object_mean = cv::mean(object_points);
for (size_t index = 0; index < count; ++index) {
object_matrix.at<double>(static_cast<int>(index), 0) =
points[index].x - object_mean[0];
object_matrix.at<double>(static_cast<int>(index), 1) =
points[index].y - object_mean[1];
object_matrix.at<double>(static_cast<int>(index), 2) =
points[index].z - object_mean[2];
}
cv::SVD object_svd(object_matrix, cv::SVD::NO_UV);
if (object_svd.w.at<double>(1, 0) < object_svd.w.at<double>(0, 0) * 1e-8)
return LARDON3D_SPARSE_GEOMETRY_DEGENERATE;
cv::Mat rvec, tvec, inliers;
RngGuard rng(parameters->deterministic_seed);
bool solved = cv::solvePnPRansac(
object_points, image_points, camera_matrix(*calibration),
distortion(*calibration), rvec, tvec, false, parameters->max_iterations,
static_cast<float>(parameters->reprojection_threshold_px),
parameters->confidence, inliers, cv::SOLVEPNP_EPNP);
uint32_t minimum = std::max<uint32_t>(parameters->minimum_inliers, 4);
double count_value = static_cast<double>(count);
if (!solved || inliers.rows < static_cast<int>(minimum) ||
static_cast<double>(inliers.rows) / count_value <
parameters->minimum_inlier_ratio)
return LARDON3D_SPARSE_GEOMETRY_ESTIMATION_FAILED;
cv::Mat rotation;
cv::Rodrigues(rvec, rotation);
if (!rotation_valid(rotation) || !cv::checkRange(tvec))
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
uint32_t positive_depth = 0;
for (int row = 0; row < inliers.rows; ++row) {
int point_index = inliers.at<int>(row, 0);
cv::Mat object(3, 1, CV_64F);
object.at<double>(0, 0) = points[point_index].x;
object.at<double>(1, 0) = points[point_index].y;
object.at<double>(2, 0) = points[point_index].z;
cv::Mat camera = rotation * object + tvec;
if (camera.at<double>(2, 0) > 1e-9)
++positive_depth;
}
if (static_cast<double>(positive_depth) /
static_cast<double>(inliers.rows) <
parameters->minimum_inlier_ratio)
return LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED;
copy_pose(rotation, tvec, &result->pose_cw);
result->inlier_count = static_cast<uint32_t>(inliers.rows);
result->inlier_ratio = static_cast<double>(inliers.rows) / count_value;
if (result->inlier_mask) {
std::fill(result->inlier_mask, result->inlier_mask + count, 0);
for (int row = 0; row < inliers.rows; ++row)
result->inlier_mask[inliers.at<int>(row, 0)] = 1;
}
return LARDON3D_SPARSE_GEOMETRY_OK;
} catch (const cv::Exception &) {
return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE;
}
}

649
tests/test_sparse_sfm_geometry.cpp Normal file
View file

@ -0,0 +1,649 @@
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <limits>
#include <vector>
#include <lardon3d/sparse_sfm_geometry.h>
#define CHECK(value) \
do { \
if (!(value)) { \
std::fprintf(stderr, "geometry failure line %d: %s\n", __LINE__, #value); \
return 1; \
} \
} while (0)
static Lardon3DSparseGeometryPoint2 project(
const Lardon3DSparseGeometryCalibration &calibration,
const Lardon3DSparseGeometryPoint3 &point,
const Lardon3DSparseGeometryPose &pose) {
double x = pose.rotation_cw[0] * point.x + pose.rotation_cw[1] * point.y +
pose.rotation_cw[2] * point.z + pose.translation_cw[0];
double y = pose.rotation_cw[3] * point.x + pose.rotation_cw[4] * point.y +
pose.rotation_cw[5] * point.z + pose.translation_cw[1];
double z = pose.rotation_cw[6] * point.x + pose.rotation_cw[7] * point.y +
pose.rotation_cw[8] * point.z + pose.translation_cw[2];
Lardon3DSparseGeometryPoint2 projected = {calibration.fx * x / z + calibration.cx,
calibration.fy * y / z + calibration.cy};
return projected;
}
static double matrix_noise(size_t index, double amplitude) {
int value = static_cast<int>((index * 37U + 11U) % 17U) - 8;
return amplitude * static_cast<double>(value) / 8.0;
}
static double rotation_error(const Lardon3DSparseGeometryPose &pose) {
double trace = pose.rotation_cw[0] + pose.rotation_cw[4] +
pose.rotation_cw[8];
return std::acos(std::clamp((trace - 1.0) / 2.0, -1.0, 1.0));
}
static double translation_direction_error(
const Lardon3DSparseGeometryPose &pose, double x, double y, double z) {
double norm = std::sqrt(pose.translation_cw[0] * pose.translation_cw[0] +
pose.translation_cw[1] * pose.translation_cw[1] +
pose.translation_cw[2] * pose.translation_cw[2]);
double target_norm = std::sqrt(x * x + y * y + z * z);
double dot = (pose.translation_cw[0] * x + pose.translation_cw[1] * y +
pose.translation_cw[2] * z) /
(norm * target_norm);
return std::acos(std::clamp(dot, -1.0, 1.0));
}
static int matrix_test() {
Lardon3DSparseGeometryCalibration calibration =
{1280, 960, 800, 800, 640, 480, 0, 0, 0, 0};
Lardon3DSparseGeometryPose pose_a = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{0, 0, 0}};
Lardon3DSparseGeometryPose pose_b = pose_a;
pose_b.translation_cw[0] = 1.0;
double pnp_worst_rotation_error = 0.0;
double pnp_worst_translation_error = 0.0;
double pnp_worst_precision = 1.0;
double pnp_worst_recall = 1.0;
constexpr size_t count = 64;
Lardon3DSparseGeometryPoint2 pixels_a[count];
Lardon3DSparseGeometryPoint2 clean_b[count];
for (size_t index = 0; index < count; ++index) {
Lardon3DSparseGeometryPoint3 point = {
-1.0 + static_cast<double>(index % 8) * 0.28,
-0.7 + static_cast<double>(index / 8) * 0.18,
4.0 + static_cast<double>(index % 11) * 0.2};
pixels_a[index] = project(calibration, point, pose_a);
clean_b[index] = project(calibration, point, pose_b);
}
const double noises[] = {0.0, 0.25, 0.5, 1.5};
const double outlier_rates[] = {0.0, 0.1, 0.25, 0.4};
for (double noise : noises) {
for (double outlier_rate : outlier_rates) {
Lardon3DSparseGeometryPoint2 pixels_b[count];
size_t outliers = static_cast<size_t>(count * outlier_rate);
for (size_t index = 0; index < count; ++index) {
pixels_b[index] = clean_b[index];
pixels_b[index].x += matrix_noise(index, noise);
pixels_b[index].y += matrix_noise(index + 19, noise);
if (index >= count - outliers) {
pixels_b[index].x = 80.0 + static_cast<double>(index * 31U % 1100U);
pixels_b[index].y = 60.0 + static_cast<double>(index * 17U % 800U);
}
}
uint8_t mask[count] = {0};
Lardon3DSparseGeometryRelativePoseParameters parameters =
{1.5, 0.999, 1500, 24, 0.5, 1e-4, 0.5, 100 +
static_cast<uint64_t>(noise * 10)};
Lardon3DSparseGeometryRelativePoseResult result = {};
result.inlier_mask = mask;
result.inlier_mask_capacity = count;
Lardon3DSparseGeometryResult status =
lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, pixels_b, count,
&parameters, &result);
if (noise == 0.0 && outlier_rate == 0.0) {
CHECK(status == LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(rotation_error(result.pose_ba) < 0.02);
CHECK(translation_direction_error(result.pose_ba, 1, 0, 0) < 0.05);
}
if (status == LARDON3D_SPARSE_GEOMETRY_OK && noise <= 0.75 &&
outlier_rate <= 0.25) {
CHECK(rotation_error(result.pose_ba) < 0.25);
CHECK(translation_direction_error(result.pose_ba, 1, 0, 0) < 0.35);
CHECK(result.inlier_count >= 24);
}
CHECK(status != LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT);
}
}
Lardon3DSparseGeometryPoint2 collinear_a[count];
Lardon3DSparseGeometryPoint2 collinear_b[count];
for (size_t index = 0; index < count; ++index) {
collinear_a[index] = {500.0 + static_cast<double>(index), 480.0};
collinear_b[index] = {500.0 + static_cast<double>(index), 480.0};
}
uint8_t mask[count] = {0};
Lardon3DSparseGeometryRelativePoseParameters parameters =
{1.0, 0.999, 500, 8, 0.5, 1e-4, 0.5, 77};
Lardon3DSparseGeometryRelativePoseResult result = {};
result.inlier_mask = mask;
result.inlier_mask_capacity = count;
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, collinear_a, collinear_b, count,
&parameters, &result) != LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, clean_b, 4, &parameters,
&result) == LARDON3D_SPARSE_GEOMETRY_INSUFFICIENT_CORRESPONDENCES);
Lardon3DSparseGeometryPoint2 nan_point = {NAN, 0};
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &nan_point, 1,
&nan_point) ==
LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT);
Lardon3DSparseGeometryPose forward = pose_b;
forward.translation_cw[0] = 0;
forward.translation_cw[2] = 0.01;
for (size_t index = 0; index < count; ++index)
clean_b[index] = project(calibration,
{-1.0 + static_cast<double>(index % 8) * 0.28,
-0.7 + static_cast<double>(index / 8) * 0.18,
4.0 + static_cast<double>(index % 11) * 0.2},
forward);
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, clean_b, count, &parameters,
&result) != LARDON3D_SPARSE_GEOMETRY_OK);
const size_t pnp_count = 32;
Lardon3DSparseGeometryPoint3 pnp_points[pnp_count];
Lardon3DSparseGeometryPoint2 pnp_pixels[pnp_count];
for (size_t index = 0; index < pnp_count; ++index) {
pnp_points[index] = {-1.0 + static_cast<double>(index % 8) * 0.3,
-0.7 + static_cast<double>(index / 8) * 0.2,
4.0 + static_cast<double>(index % 5) * 0.3};
pnp_pixels[index] = project(calibration, pnp_points[index], pose_b);
}
uint8_t pnp_mask[pnp_count] = {0};
Lardon3DSparseGeometryPnPParameters pnp_parameters =
{1.5, 0.999, 1000, 12, 0.5, 404};
Lardon3DSparseGeometryPnPResult pnp_result = {};
pnp_result.inlier_mask = pnp_mask;
pnp_result.inlier_mask_capacity = pnp_count;
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, pnp_points, pnp_pixels, pnp_count,
&pnp_parameters, &pnp_result) == LARDON3D_SPARSE_GEOMETRY_OK);
const double pnp_noises[] = {0.0, 0.25, 0.75, 2.0};
const double pnp_outliers[] = {0.0, 0.125, 0.25, 0.4};
for (double noise : pnp_noises) {
for (double outlier_rate : pnp_outliers) {
Lardon3DSparseGeometryPoint2 altered[pnp_count];
size_t outlier_count = static_cast<size_t>(pnp_count * outlier_rate);
for (size_t index = 0; index < pnp_count; ++index) {
altered[index] = pnp_pixels[index];
altered[index].x += matrix_noise(index, noise);
altered[index].y += matrix_noise(index + 31, noise);
if (index >= pnp_count - outlier_count) {
altered[index].x = 100.0 + static_cast<double>(index * 41U % 1000U);
altered[index].y = 100.0 + static_cast<double>(index * 23U % 700U);
}
}
Lardon3DSparseGeometryResult status = lardon3d_sparse_geometry_pnp(
&calibration, pnp_points, altered, pnp_count, &pnp_parameters,
&pnp_result);
CHECK(status != LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT);
if (status == LARDON3D_SPARSE_GEOMETRY_OK) {
size_t true_positive = 0;
size_t false_positive = 0;
size_t false_negative = 0;
for (size_t index = 0; index < pnp_count; ++index) {
bool expected_inlier = index < pnp_count - outlier_count;
bool actual_inlier = pnp_mask[index] != 0;
if (expected_inlier && actual_inlier)
++true_positive;
else if (!expected_inlier && actual_inlier)
++false_positive;
else if (expected_inlier)
++false_negative;
}
double precision = true_positive == 0
? 0.0
: static_cast<double>(true_positive) /
static_cast<double>(true_positive + false_positive);
double recall = static_cast<double>(true_positive) /
static_cast<double>(true_positive + false_negative);
pnp_worst_precision = std::min(pnp_worst_precision, precision);
pnp_worst_recall = std::min(pnp_worst_recall, recall);
pnp_worst_rotation_error =
std::max(pnp_worst_rotation_error, rotation_error(pnp_result.pose_cw));
pnp_worst_translation_error = std::max(
pnp_worst_translation_error,
std::sqrt((pnp_result.pose_cw.translation_cw[0] - 1.0) *
(pnp_result.pose_cw.translation_cw[0] - 1.0) +
pnp_result.pose_cw.translation_cw[1] *
pnp_result.pose_cw.translation_cw[1] +
pnp_result.pose_cw.translation_cw[2] *
pnp_result.pose_cw.translation_cw[2]));
}
}
}
Lardon3DSparseGeometryPoint3 collinear_points[pnp_count];
Lardon3DSparseGeometryPoint2 collinear_pixels[pnp_count];
for (size_t index = 0; index < pnp_count; ++index) {
collinear_points[index] = {static_cast<double>(index) * 0.1, 0, 4};
collinear_pixels[index] = project(calibration, collinear_points[index], pose_b);
}
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, collinear_points, collinear_pixels, pnp_count,
&pnp_parameters, &pnp_result) ==
LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
Lardon3DSparseGeometryPoint3 duplicate_points[pnp_count];
Lardon3DSparseGeometryPoint2 duplicate_pixels[pnp_count];
for (size_t index = 0; index < pnp_count; ++index) {
duplicate_points[index] = pnp_points[0];
duplicate_pixels[index] = pnp_pixels[0];
}
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, duplicate_points, duplicate_pixels, pnp_count,
&pnp_parameters, &pnp_result) ==
LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
Lardon3DSparseGeometryPoint3 planar_points[pnp_count];
Lardon3DSparseGeometryPoint3 near_planar_points[pnp_count];
Lardon3DSparseGeometryPoint3 far_points[pnp_count];
Lardon3DSparseGeometryPoint2 planar_pixels[pnp_count];
Lardon3DSparseGeometryPoint2 near_planar_pixels[pnp_count];
Lardon3DSparseGeometryPoint2 far_pixels[pnp_count];
for (size_t index = 0; index < pnp_count; ++index) {
double x = -1.0 + static_cast<double>(index % 8) * 0.3;
double y = -0.7 + static_cast<double>(index / 8) * 0.2;
planar_points[index] = {x, y, 4.0};
near_planar_points[index] = {x, y, 4.0 + static_cast<double>(index % 3) * 1e-4};
far_points[index] = {x, y, 1000.0 + static_cast<double>(index % 3)};
planar_pixels[index] = project(calibration, planar_points[index], pose_b);
near_planar_pixels[index] =
project(calibration, near_planar_points[index], pose_b);
far_pixels[index] = project(calibration, far_points[index], pose_b);
}
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, planar_points, planar_pixels, pnp_count,
&pnp_parameters, &pnp_result) == LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, near_planar_points, near_planar_pixels, pnp_count,
&pnp_parameters, &pnp_result) !=
LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT);
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, far_points, far_pixels, pnp_count, &pnp_parameters,
&pnp_result) != LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT);
for (size_t view_count : {size_t(2), size_t(3), size_t(5), size_t(10)}) {
Lardon3DSparseGeometryPoint2 observations[10];
Lardon3DSparseGeometryPose poses[10];
Lardon3DSparseGeometryPoint3 truth_point = {0.2, -0.1, 4.0};
for (size_t view = 0; view < view_count; ++view) {
poses[view] = pose_a;
poses[view].translation_cw[0] = static_cast<double>(view) * 0.5;
observations[view] = project(calibration, truth_point, poses[view]);
CHECK(lardon3d_sparse_geometry_normalize(
&calibration, &observations[view], 1, &observations[view]) ==
LARDON3D_SPARSE_GEOMETRY_OK);
}
Lardon3DSparseGeometryPoint3 multi;
CHECK(lardon3d_sparse_geometry_triangulate_multi_view(
observations, poses, view_count, &multi) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(multi.z - truth_point.z) < 1e-7);
}
Lardon3DSparseGeometryPoint2 planar_a[count];
Lardon3DSparseGeometryPoint2 planar_b[count];
Lardon3DSparseGeometryPoint2 far_a[count];
Lardon3DSparseGeometryPoint2 far_b[count];
Lardon3DSparseGeometryPose far_pose = pose_b;
for (size_t index = 0; index < count; ++index) {
Lardon3DSparseGeometryPoint3 planar_point = {
-1.0 + static_cast<double>(index % 8) * 0.28,
-0.7 + static_cast<double>(index / 8) * 0.18, 4.0};
Lardon3DSparseGeometryPoint3 far_point = {planar_point.x, planar_point.y,
10000.0};
planar_a[index] = project(calibration, planar_point, pose_a);
planar_b[index] = project(calibration, planar_point, pose_b);
far_a[index] = project(calibration, far_point, pose_a);
far_b[index] = project(calibration, far_point, far_pose);
}
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, planar_a, planar_b, count, &parameters,
&result) != LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT);
parameters.minimum_parallax_rad = 1e-4;
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, far_a, far_b, count, &parameters,
&result) != LARDON3D_SPARSE_GEOMETRY_OK);
std::printf("pnp_worst_rotation=%.17g pnp_worst_translation=%.17g "
"pnp_worst_precision=%.17g pnp_worst_recall=%.17g\n",
pnp_worst_rotation_error, pnp_worst_translation_error,
pnp_worst_precision, pnp_worst_recall);
return 0;
}
static int multiview_matrix_test() {
const Lardon3DSparseGeometryCalibration calibration =
{1280, 960, 800, 800, 640, 480, 0, 0, 0, 0};
const Lardon3DSparseGeometryPoint3 truth = {0.35, -0.2, 4.0};
const size_t view_counts[] = {2, 3, 5, 10};
double worst_error = 0.0;
double worst_mean_reprojection = 0.0;
double worst_max_reprojection = 0.0;
double lowest_accepted_parallax = std::numeric_limits<double>::max();
double highest_rejected_parallax = 0.0;
for (size_t view_count : view_counts) {
const double noise_levels[] = {0.0, 0.25, 0.75, 1.5};
for (size_t noise_index = 0; noise_index < 4; ++noise_index) {
Lardon3DSparseGeometryPoint2 observations[10];
Lardon3DSparseGeometryPose poses[10];
for (size_t view = 0; view < view_count; ++view) {
poses[view] = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{static_cast<double>(view) * 0.5,
static_cast<double>(view % 2) * 0.2, 0}};
observations[view] = project(calibration, truth, poses[view]);
observations[view].x +=
matrix_noise(view + noise_index * 17, noise_levels[noise_index]);
observations[view].y +=
matrix_noise(view + noise_index * 23, noise_levels[noise_index]);
CHECK(lardon3d_sparse_geometry_normalize(
&calibration, &observations[view], 1, &observations[view]) ==
LARDON3D_SPARSE_GEOMETRY_OK);
}
Lardon3DSparseGeometryPoint3 output;
Lardon3DSparseGeometryResult status =
lardon3d_sparse_geometry_triangulate_multi_view(
observations, poses, view_count, &output);
CHECK(status == LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::isfinite(output.x) && std::isfinite(output.y) &&
std::isfinite(output.z));
double error = std::sqrt((output.x - truth.x) * (output.x - truth.x) +
(output.y - truth.y) * (output.y - truth.y) +
(output.z - truth.z) * (output.z - truth.z));
double mean_reprojection = 0.0;
double max_reprojection = 0.0;
for (size_t view = 0; view < view_count; ++view) {
Lardon3DSparseGeometryPoint3 camera_point = output;
camera_point.x += poses[view].translation_cw[0];
camera_point.y += poses[view].translation_cw[1];
camera_point.z += poses[view].translation_cw[2];
double dx = camera_point.x / camera_point.z - observations[view].x;
double dy = camera_point.y / camera_point.z - observations[view].y;
double reprojection = std::hypot(dx, dy);
mean_reprojection += reprojection;
max_reprojection = std::max(max_reprojection, reprojection);
}
mean_reprojection /= static_cast<double>(view_count);
worst_error = std::max(worst_error, error);
worst_mean_reprojection =
std::max(worst_mean_reprojection, mean_reprojection);
worst_max_reprojection = std::max(worst_max_reprojection, max_reprojection);
Lardon3DSparseGeometryPoint3 repeated;
CHECK(lardon3d_sparse_geometry_triangulate_multi_view(
observations, poses, view_count, &repeated) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::memcmp(&output, &repeated, sizeof(output)) == 0);
}
const double boundary_baselines[] = {0.001, 0.00045, 0.00035, 0.00005};
for (double baseline : boundary_baselines) {
Lardon3DSparseGeometryPoint2 observations[10];
Lardon3DSparseGeometryPose poses[10];
for (size_t view = 0; view < view_count; ++view) {
poses[view] = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{static_cast<double>(view) * baseline, 0, 0}};
observations[view] = project(calibration, truth, poses[view]);
CHECK(lardon3d_sparse_geometry_normalize(
&calibration, &observations[view], 1, &observations[view]) ==
LARDON3D_SPARSE_GEOMETRY_OK);
}
Lardon3DSparseGeometryPoint3 output;
Lardon3DSparseGeometryResult status =
lardon3d_sparse_geometry_triangulate_multi_view(
observations, poses, view_count, &output);
double effective_parallax = baseline / truth.z;
if (effective_parallax >= 1e-4) {
CHECK(status == LARDON3D_SPARSE_GEOMETRY_OK);
lowest_accepted_parallax =
std::min(lowest_accepted_parallax, effective_parallax);
} else {
CHECK(status == LARDON3D_SPARSE_GEOMETRY_LOW_PARALLAX);
highest_rejected_parallax =
std::max(highest_rejected_parallax, effective_parallax);
}
}
Lardon3DSparseGeometryPoint2 observations[10];
Lardon3DSparseGeometryPose poses[10];
Lardon3DSparseGeometryPoint3 far_truth = {0.35, -0.2, 10000.0};
for (size_t view = 0; view < view_count; ++view) {
poses[view] = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{static_cast<double>(view) * 2.0, 0, 0}};
observations[view] = project(calibration, far_truth, poses[view]);
CHECK(lardon3d_sparse_geometry_normalize(
&calibration, &observations[view], 1, &observations[view]) ==
LARDON3D_SPARSE_GEOMETRY_OK);
}
Lardon3DSparseGeometryPoint3 output;
CHECK(lardon3d_sparse_geometry_triangulate_multi_view(
observations, poses, view_count, &output) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::isfinite(output.z));
}
std::printf("multi_view_worst_error=%.17g mean_reprojection=%.17g "
"max_reprojection=%.17g lowest_parallax=%.17g "
"highest_rejected_parallax=%.17g\n",
worst_error, worst_mean_reprojection, worst_max_reprojection,
lowest_accepted_parallax, highest_rejected_parallax);
return 0;
}
static int resource_test() {
Lardon3DSparseGeometryCalibration calibration =
{1280, 960, 800, 800, 640, 480, 0, 0, 0, 0};
Lardon3DSparseGeometryPose pose_a = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{0, 0, 0}};
Lardon3DSparseGeometryPose pose_b = pose_a;
pose_b.translation_cw[0] = 1.0;
const size_t relative_count = 8192;
std::vector<Lardon3DSparseGeometryPoint2> pixels_a(relative_count);
std::vector<Lardon3DSparseGeometryPoint2> pixels_b(relative_count);
for (size_t index = 0; index < relative_count; ++index) {
Lardon3DSparseGeometryPoint3 point = {
-1.0 + static_cast<double>(index % 128) * 0.015,
-0.8 + static_cast<double>((index / 128) % 64) * 0.025,
4.0 + static_cast<double>(index % 17) * 0.1};
pixels_a[index] = project(calibration, point, pose_a);
pixels_b[index] = project(calibration, point, pose_b);
}
std::vector<uint8_t> mask(relative_count);
Lardon3DSparseGeometryRelativePoseParameters relative_parameters =
{1.0, 0.999, 1000, 32, 0.1, 1e-5, 0.5, 99};
Lardon3DSparseGeometryRelativePoseResult relative = {};
relative.inlier_mask = mask.data();
relative.inlier_mask_capacity = mask.size();
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a.data(), pixels_b.data(),
relative_count, &relative_parameters, &relative) ==
LARDON3D_SPARSE_GEOMETRY_OK);
const size_t pnp_count = 2000;
std::vector<Lardon3DSparseGeometryPoint3> points(pnp_count);
std::vector<Lardon3DSparseGeometryPoint2> pnp_pixels(pnp_count);
for (size_t index = 0; index < pnp_count; ++index) {
points[index] = {-1.0 + static_cast<double>(index % 100) * 0.02,
-0.8 + static_cast<double>((index / 100) % 20) * 0.03,
4.0 + static_cast<double>(index % 13) * 0.1};
pnp_pixels[index] = project(calibration, points[index], pose_b);
}
std::vector<uint8_t> pnp_mask(pnp_count);
Lardon3DSparseGeometryPnPParameters pnp_parameters =
{1.0, 0.999, 1000, 32, 0.1, 99};
Lardon3DSparseGeometryPnPResult pnp = {};
pnp.inlier_mask = pnp_mask.data();
pnp.inlier_mask_capacity = pnp_mask.size();
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, points.data(), pnp_pixels.data(), pnp_count,
&pnp_parameters, &pnp) == LARDON3D_SPARSE_GEOMETRY_OK);
Lardon3DSparseGeometryPoint2 normalized_a;
Lardon3DSparseGeometryPoint2 normalized_b;
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &pixels_a[0], 1,
&normalized_a) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &pixels_b[0], 1,
&normalized_b) ==
LARDON3D_SPARSE_GEOMETRY_OK);
Lardon3DSparseGeometryPoint3 output;
for (size_t index = 0; index < 100000; ++index)
CHECK(lardon3d_sparse_geometry_triangulate_two_view(
&normalized_a, &normalized_b, &pose_a, &pose_b, &output) ==
LARDON3D_SPARSE_GEOMETRY_OK);
std::printf("relative=%zu inliers=%u pose=%.17g,%.17g,%.17g pnp=%zu "
"pnp_inliers=%u pnp_pose=%.17g,%.17g,%.17g triangulations=%d\n",
relative_count, relative.inlier_count,
relative.pose_ba.translation_cw[0],
relative.pose_ba.translation_cw[1],
relative.pose_ba.translation_cw[2], pnp_count,
pnp.inlier_count, pnp.pose_cw.translation_cw[0],
pnp.pose_cw.translation_cw[1], pnp.pose_cw.translation_cw[2],
100000);
return 0;
}
int main(int argc, char **argv) {
if (argc > 1)
if (std::strcmp(argv[1], "matrix") == 0)
return matrix_test();
if (argc > 1)
if (std::strcmp(argv[1], "multiview-matrix") == 0)
return multiview_matrix_test();
if (argc > 1)
return resource_test();
Lardon3DSparseGeometryCalibration calibration =
{1280, 960, 800, 800, 640, 480, 0, 0, 0, 0};
Lardon3DSparseGeometryPoint2 pixel = {720, 520};
Lardon3DSparseGeometryPoint2 normalized;
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &pixel, 1,
&normalized) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(normalized.x - 0.1) < 1e-12);
CHECK(std::abs(normalized.y - 0.05) < 1e-12);
Lardon3DSparseGeometryCalibration distorted_calibration = calibration;
distorted_calibration.k1 = 0.08;
distorted_calibration.k2 = -0.01;
distorted_calibration.p1 = 0.001;
distorted_calibration.p2 = -0.002;
const double ideal_x = 0.2;
const double ideal_y = -0.15;
const double radius_squared = ideal_x * ideal_x + ideal_y * ideal_y;
const double radial = 1.0 + distorted_calibration.k1 * radius_squared +
distorted_calibration.k2 * radius_squared * radius_squared;
const double distorted_x = ideal_x * radial +
2.0 * distorted_calibration.p1 * ideal_x * ideal_y +
distorted_calibration.p2 *
(radius_squared + 2.0 * ideal_x * ideal_x);
const double distorted_y = ideal_y * radial +
distorted_calibration.p1 *
(radius_squared + 2.0 * ideal_y * ideal_y) +
2.0 * distorted_calibration.p2 * ideal_x * ideal_y;
Lardon3DSparseGeometryPoint2 distorted_pixel = {
distorted_calibration.fx * distorted_x + distorted_calibration.cx,
distorted_calibration.fy * distorted_y + distorted_calibration.cy};
CHECK(lardon3d_sparse_geometry_normalize(
&distorted_calibration, &distorted_pixel, 1, &normalized) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(normalized.x - ideal_x) < 1e-8);
CHECK(std::abs(normalized.y - ideal_y) < 1e-8);
Lardon3DSparseGeometryPose pose_a = {{1, 0, 0, 0, 1, 0, 0, 0, 1},
{0, 0, 0}};
Lardon3DSparseGeometryPose pose_b = pose_a;
pose_b.translation_cw[0] = 1.0;
Lardon3DSparseGeometryPoint3 truth = {0.2, -0.1, 4.0};
Lardon3DSparseGeometryPoint2 pixels_a[8];
Lardon3DSparseGeometryPoint2 pixels_b[8];
for (size_t index = 0; index < 8; ++index) {
Lardon3DSparseGeometryPoint3 point = {
truth.x + static_cast<double>(index) * 0.17,
truth.y + static_cast<double>(index % 3) * 0.13,
truth.z + static_cast<double>(index) * 0.21};
pixels_a[index] = project(calibration, point, pose_a);
pixels_b[index] = project(calibration, point, pose_b);
}
uint8_t mask[8] = {0};
Lardon3DSparseGeometryRelativePoseParameters relative_parameters =
{1.0, 0.999, 1000, 6, 0.75, 1e-4, 0.5, 1234};
Lardon3DSparseGeometryRelativePoseResult relative = {};
relative.inlier_mask = mask;
relative.inlier_mask_capacity = 8;
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, pixels_b, 8,
&relative_parameters, &relative) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(relative.inlier_count >= 6);
CHECK(relative.median_parallax_rad > 1e-4);
Lardon3DSparseGeometryRelativePoseResult repeated = {};
repeated.inlier_mask = mask;
repeated.inlier_mask_capacity = 8;
for (int run = 0; run < 20; ++run) {
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, pixels_b, 8,
&relative_parameters, &repeated) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(repeated.inlier_count == relative.inlier_count);
CHECK(std::memcmp(mask, relative.inlier_mask, sizeof(mask)) == 0);
}
Lardon3DSparseGeometryPoint2 nonfinite_pixel = {NAN, 0};
CHECK(lardon3d_sparse_geometry_normalize(
&calibration, &nonfinite_pixel, 1, &normalized) ==
LARDON3D_SPARSE_GEOMETRY_NONFINITE_INPUT);
Lardon3DSparseGeometryPoint2 pure_pixels_b[8];
for (size_t index = 0; index < 8; ++index)
pure_pixels_b[index] = pixels_a[index];
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, pixels_a, pure_pixels_b, 8,
&relative_parameters, &repeated) !=
LARDON3D_SPARSE_GEOMETRY_OK);
Lardon3DSparseGeometryPoint2 normalized_a;
Lardon3DSparseGeometryPoint2 normalized_b;
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &pixels_a[0], 1,
&normalized_a) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(lardon3d_sparse_geometry_normalize(&calibration, &pixels_b[0], 1,
&normalized_b) ==
LARDON3D_SPARSE_GEOMETRY_OK);
Lardon3DSparseGeometryPoint3 triangulated;
CHECK(lardon3d_sparse_geometry_triangulate_two_view(
&normalized_a, &normalized_b, &pose_a, &pose_b, &triangulated) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(triangulated.x - truth.x) < 1e-8);
CHECK(std::abs(triangulated.y - truth.y) < 1e-8);
CHECK(std::abs(triangulated.z - truth.z) < 1e-8);
Lardon3DSparseGeometryPoint2 multi_points[3] = {
normalized_a, normalized_b, normalized_b};
Lardon3DSparseGeometryPose multi_poses[3] = {pose_a, pose_b, pose_b};
Lardon3DSparseGeometryPoint3 multi_point;
CHECK(lardon3d_sparse_geometry_triangulate_multi_view(
multi_points, multi_poses, 3, &multi_point) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(multi_point.z - truth.z) < 1e-8);
Lardon3DSparseGeometryPointRefinementParameters refinement_parameters =
{30, 1e-12};
Lardon3DSparseGeometryPoint3 initial = {truth.x + 0.1, truth.y - 0.1,
truth.z + 0.2};
Lardon3DSparseGeometryPoint3 refined;
CHECK(lardon3d_sparse_geometry_refine_point(
multi_points, multi_poses, 3, &initial, &refinement_parameters,
&refined) == LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(std::abs(refined.x - truth.x) < 1e-7);
CHECK(std::abs(refined.y - truth.y) < 1e-7);
CHECK(std::abs(refined.z - truth.z) < 1e-7);
Lardon3DSparseGeometryPnPParameters pnp_parameters =
{1.0, 0.999, 1000, 6, 0.75, 1234};
Lardon3DSparseGeometryPoint3 points[8];
for (size_t index = 0; index < 8; ++index)
points[index] = {truth.x + static_cast<double>(index) * 0.17,
truth.y + static_cast<double>(index % 3) * 0.13,
truth.z + static_cast<double>(index) * 0.21};
Lardon3DSparseGeometryPnPResult pnp = {};
pnp.inlier_mask = mask;
pnp.inlier_mask_capacity = 8;
CHECK(lardon3d_sparse_geometry_pnp(&calibration, points, pixels_b, 8,
&pnp_parameters, &pnp) ==
LARDON3D_SPARSE_GEOMETRY_OK);
CHECK(pnp.inlier_count >= 6);
return 0;
}