sparse-sfm: complete Gate C geometry validation

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`,

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.
 
 ---
 

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,

include/lardon3d/sparse_sfm_geometry.h

@@ -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

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: [

src/sparse_sfm_geometry.cpp

@@ -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;
+  }
+}

tests/test_sparse_sfm_geometry.cpp

@@ -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;
+}