From 67529791c445c3ff9832f08d2e88ba7212040abe Mon Sep 17 00:00:00 2001 From: fy59 Date: Mon, 10 Aug 2026 13:22:04 +0200 Subject: [PATCH] sparse-sfm: complete Gate C geometry validation --- docs/architecture/project_database.md | 3 + docs/architecture/reconstruction_pipeline.md | 10 +- docs/architecture/sparse_sfm.md | 54 ++ include/lardon3d/sparse_sfm_geometry.h | 138 ++++ meson.build | 10 + src/sparse_sfm_geometry.cpp | 604 +++++++++++++++++ tests/test_sparse_sfm_geometry.cpp | 649 +++++++++++++++++++ 7 files changed, 1464 insertions(+), 4 deletions(-) create mode 100644 include/lardon3d/sparse_sfm_geometry.h create mode 100644 src/sparse_sfm_geometry.cpp create mode 100644 tests/test_sparse_sfm_geometry.cpp diff --git a/docs/architecture/project_database.md b/docs/architecture/project_database.md index fc69c2c..5af5310 100644 --- a/docs/architecture/project_database.md +++ b/docs/architecture/project_database.md @@ -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 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 source `project_db.c` exposent `create_track_set`, `load_track_set`, `find_track_set`, `list_track_sets`, `load_track`, `list_tracks`, diff --git a/docs/architecture/reconstruction_pipeline.md b/docs/architecture/reconstruction_pipeline.md index 50756ca..ef9bd25 100644 --- a/docs/architecture/reconstruction_pipeline.md +++ b/docs/architecture/reconstruction_pipeline.md @@ -124,15 +124,17 @@ USAC/MAGSAC avec configuration, seed et fingerprint déterministes. **Statut :** COMPLETED/FROZEN — le Track Builder v1 direct et durable est 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 modèle de persistance Sparse SfM v16 est gelé après Gate B. +le payload de tâche). Les primitives de géométrie calibrée Gate C sont +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 incremental, la triangulation candidate, le gauge, les conventions de pose, les limites BA et l'enveloppe matérielle sont documentés dans `architecture/sparse_sfm.md`. Le solveur Sparse SfM reste -**NOT_IMPLEMENTED** jusqu'aux Gates B3–G; sa -persistance v16 et ses lecteurs bornés sont implémentés en B2. +**NOT_IMPLEMENTED** jusqu'aux Gates D–G; ses primitives pures calibrées sont +implémentées en Gate C, tandis que sa persistance v16 et ses lecteurs bornés +restent ceux de B2. --- diff --git a/docs/architecture/sparse_sfm.md b/docs/architecture/sparse_sfm.md index 117a44f..0712045 100644 --- a/docs/architecture/sparse_sfm.md +++ b/docs/architecture/sparse_sfm.md @@ -383,6 +383,60 @@ metric alignment, persistent reconstruction schema and durable SfM checkpoints. Their semantic ownership is decided here; their final numeric values require 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 No production Sparse SfM, triangulator, camera solver, BA, Project DB v16, diff --git a/include/lardon3d/sparse_sfm_geometry.h b/include/lardon3d/sparse_sfm_geometry.h new file mode 100644 index 0000000..842d60e --- /dev/null +++ b/include/lardon3d/sparse_sfm_geometry.h @@ -0,0 +1,138 @@ +#ifndef LARDON3D_SPARSE_SFM_GEOMETRY_H +#define LARDON3D_SPARSE_SFM_GEOMETRY_H + +#include +#include +#include + +#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 diff --git a/meson.build b/meson.build index 169bc6c..10e670d 100644 --- a/meson.build +++ b/meson.build @@ -144,6 +144,7 @@ executable( 'src/image_view.c', 'src/project.c', 'src/project_db.c', 'src/project_db_sparse_sfm.c', + 'src/sparse_sfm_geometry.cpp', 'src/task.c', 'src/task_checkpoint.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) +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( 'test-sparse-sfm-resource', sources: [ diff --git a/src/sparse_sfm_geometry.cpp b/src/sparse_sfm_geometry.cpp new file mode 100644 index 0000000..30d2f76 --- /dev/null +++ b/src/sparse_sfm_geometry.cpp @@ -0,0 +1,604 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include + +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(0, 0) = calibration.fx; + matrix.at(0, 2) = calibration.cx; + matrix.at(1, 1) = calibration.fy; + matrix.at(1, 2) = calibration.cy; + matrix.at(2, 2) = 1.0; + return matrix; +} + +cv::Mat distortion(const Lardon3DSparseGeometryCalibration &calibration) { + cv::Mat matrix = cv::Mat::zeros(1, 4, CV_64F); + matrix.at(0, 0) = calibration.k1; + matrix.at(0, 1) = calibration.k2; + matrix.at(0, 2) = calibration.p1; + matrix.at(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(row, column); + for (int index = 0; index < 3; ++index) + pose->translation_cw[index] = translation.at(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(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(row, column) = pose.rotation_cw[row * 3 + column]; + projection.at(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(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(2, 0); + cv::Mat point_b = rotation_b * candidate + translation_b; + double depth_b = point_b.at(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(0, 0) = point_a.x; + ray_a.at(1, 0) = point_a.y; + ray_a.at(2, 0) = 1.0; + ray_b.at(0, 0) = point_b.x; + ray_b.at(1, 0) = point_b.y; + ray_b.at(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 source; + source.reserve(count); + for (size_t index = 0; index < count; ++index) + source.emplace_back(pixels[index].x, pixels[index].y); + std::vector 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(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 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(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(count); + mean_ay /= static_cast(count); + mean_bx /= static_cast(count); + mean_by /= static_cast(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(count), 2, CV_64F, normalized_a.data()); + cv::Mat points_b(static_cast(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(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(count); + if (inliers < static_cast(parameters->minimum_inliers) || + static_cast(inliers) / count_value < + parameters->minimum_inlier_ratio || + !rotation_valid(rotation) || cv::norm(translation) < 1e-12) + return LARDON3D_SPARSE_GEOMETRY_ESTIMATION_FAILED; + std::vector parallaxes; + cv::Mat projection_a = cv::Mat::zeros(3, 4, CV_64F); + projection_a.at(0, 0) = projection_a.at(1, 1) = + projection_a.at(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(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(0, 0) = ray_b[0]; + ray_b_input.at(1, 0) = ray_b[1]; + ray_b_input.at(2, 0) = ray_b[2]; + cv::Mat ray_b_mat = rotation.t() * ray_b_input; + ray_b = cv::Vec3d(ray_b_mat.at(0, 0), + ray_b_mat.at(1, 0), + ray_b_mat.at(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( + static_cast(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(inliers); + result->inlier_ratio = static_cast(inliers) / count_value; + if (result->inlier_mask) + for (size_t index = 0; index < count; ++index) + result->inlier_mask[index] = mask.at(static_cast(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(0, 0) = normalized_a->x; + points_a.at(1, 0) = normalized_a->y; + points_b.at(0, 0) = normalized_b->x; + points_b.at(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(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(0, 0); + point->y = candidate.at(1, 0); + point->z = candidate.at(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(INT_MAX / 2)) + return LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT; + try { + cv::Mat design = cv::Mat::zeros(static_cast(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(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(index, 0) += poses[view].translation_cw[index]; + if (!finite_value(camera_point.at(2, 0)) || + camera_point.at(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(0, 0); + point->y = candidate.at(1, 0); + point->z = candidate.at(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(0, 0) = initial_point->x; + point.at(1, 0) = initial_point->y; + point.at(2, 0) = initial_point->z; + try { + auto objective = [&](const cv::Mat &candidate, cv::Mat *residual) { + cv::Mat values = cv::Mat::zeros(static_cast(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(index, 0) += poses[view].translation_cw[index]; + double z = camera_point.at(2, 0); + if (!finite_value(z) || z <= 1e-9) + return std::numeric_limits::infinity(); + double x = camera_point.at(0, 0) / z; + double y = camera_point.at(1, 0) / z; + values.at(static_cast(view * 2), 0) = + x - normalized_points[view].x; + values.at(static_cast(view * 2 + 1), 0) = + y - normalized_points[view].y; + sum += values.at(static_cast(view * 2), 0) * + values.at(static_cast(view * 2), 0) + + values.at(static_cast(view * 2 + 1), 0) * + values.at(static_cast(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(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(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(0, 0); + refined_point->y = point.at(1, 0); + refined_point->z = point.at(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(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 object_points; + std::vector 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(count), 3, CV_64F); + cv::Scalar object_mean = cv::mean(object_points); + for (size_t index = 0; index < count; ++index) { + object_matrix.at(static_cast(index), 0) = + points[index].x - object_mean[0]; + object_matrix.at(static_cast(index), 1) = + points[index].y - object_mean[1]; + object_matrix.at(static_cast(index), 2) = + points[index].z - object_mean[2]; + } + cv::SVD object_svd(object_matrix, cv::SVD::NO_UV); + if (object_svd.w.at(1, 0) < object_svd.w.at(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(parameters->reprojection_threshold_px), + parameters->confidence, inliers, cv::SOLVEPNP_EPNP); + uint32_t minimum = std::max(parameters->minimum_inliers, 4); + double count_value = static_cast(count); + if (!solved || inliers.rows < static_cast(minimum) || + static_cast(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(row, 0); + cv::Mat object(3, 1, CV_64F); + object.at(0, 0) = points[point_index].x; + object.at(1, 0) = points[point_index].y; + object.at(2, 0) = points[point_index].z; + cv::Mat camera = rotation * object + tvec; + if (camera.at(2, 0) > 1e-9) + ++positive_depth; + } + if (static_cast(positive_depth) / + static_cast(inliers.rows) < + parameters->minimum_inlier_ratio) + return LARDON3D_SPARSE_GEOMETRY_CHEIRALITY_FAILED; + copy_pose(rotation, tvec, &result->pose_cw); + result->inlier_count = static_cast(inliers.rows); + result->inlier_ratio = static_cast(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(row, 0)] = 1; + } + return LARDON3D_SPARSE_GEOMETRY_OK; + } catch (const cv::Exception &) { + return LARDON3D_SPARSE_GEOMETRY_NUMERIC_FAILURE; + } +} diff --git a/tests/test_sparse_sfm_geometry.cpp b/tests/test_sparse_sfm_geometry.cpp new file mode 100644 index 0000000..712fdcf --- /dev/null +++ b/tests/test_sparse_sfm_geometry.cpp @@ -0,0 +1,649 @@ +#include +#include +#include +#include +#include +#include + +#include + +#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((index * 37U + 11U) % 17U) - 8; + return amplitude * static_cast(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(index % 8) * 0.28, + -0.7 + static_cast(index / 8) * 0.18, + 4.0 + static_cast(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(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(index * 31U % 1100U); + pixels_b[index].y = 60.0 + static_cast(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(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, + ¶meters, &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(index), 480.0}; + collinear_b[index] = {500.0 + static_cast(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, + ¶meters, &result) != LARDON3D_SPARSE_GEOMETRY_OK); + CHECK(lardon3d_sparse_geometry_relative_pose( + &calibration, &calibration, pixels_a, clean_b, 4, ¶meters, + &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(index % 8) * 0.28, + -0.7 + static_cast(index / 8) * 0.18, + 4.0 + static_cast(index % 11) * 0.2}, + forward); + CHECK(lardon3d_sparse_geometry_relative_pose( + &calibration, &calibration, pixels_a, clean_b, count, ¶meters, + &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(index % 8) * 0.3, + -0.7 + static_cast(index / 8) * 0.2, + 4.0 + static_cast(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(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(index * 41U % 1000U); + altered[index].y = 100.0 + static_cast(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(true_positive) / + static_cast(true_positive + false_positive); + double recall = static_cast(true_positive) / + static_cast(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(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(index % 8) * 0.3; + double y = -0.7 + static_cast(index / 8) * 0.2; + planar_points[index] = {x, y, 4.0}; + near_planar_points[index] = {x, y, 4.0 + static_cast(index % 3) * 1e-4}; + far_points[index] = {x, y, 1000.0 + static_cast(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(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(index % 8) * 0.28, + -0.7 + static_cast(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, ¶meters, + &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, ¶meters, + &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::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(view) * 0.5, + static_cast(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(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(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(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 pixels_a(relative_count); + std::vector pixels_b(relative_count); + for (size_t index = 0; index < relative_count; ++index) { + Lardon3DSparseGeometryPoint3 point = { + -1.0 + static_cast(index % 128) * 0.015, + -0.8 + static_cast((index / 128) % 64) * 0.025, + 4.0 + static_cast(index % 17) * 0.1}; + pixels_a[index] = project(calibration, point, pose_a); + pixels_b[index] = project(calibration, point, pose_b); + } + std::vector 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 points(pnp_count); + std::vector pnp_pixels(pnp_count); + for (size_t index = 0; index < pnp_count; ++index) { + points[index] = {-1.0 + static_cast(index % 100) * 0.02, + -0.8 + static_cast((index / 100) % 20) * 0.03, + 4.0 + static_cast(index % 13) * 0.1}; + pnp_pixels[index] = project(calibration, points[index], pose_b); + } + std::vector 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(index) * 0.17, + truth.y + static_cast(index % 3) * 0.13, + truth.z + static_cast(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(index) * 0.17, + truth.y + static_cast(index % 3) * 0.13, + truth.z + static_cast(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; +}