fy59/lardon3d
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
lardon3d/tests/test_sparse_sfm_geometry.cpp
fy59 b84f860d86 refactor(core): freeze global maintenance baseline 
Complete the A-to-Z Lardon3D maintenance and coherence pass.

Generalize host resource policy, remove the global CPU12 ceiling, preserve
host CPU/RAM reserves, scale Task capabilities through the Resource Governor,
and validate deterministic parallel GV execution.

Migrate Project DB to v23 with data-driven camera, lens, optical configuration
and calibration profiles, including manual lenses without EXIF.

Integrate safe optional LARDON SSD swap/scratch control with Governor and F10
drain/safe-to-unplug semantics.

Refactor the ncurses TUI into a runtime observatory with durable progress,
elapsed time, smoothed ETA, throughput, resource telemetry, Governor state,
optics workflow, colors and compact/no-color fallbacks.

Reconcile Queue lifetime, persistence, concurrency, comments, tests,
README, AGENTS and canonical documentation.

GLOBAL_MAINTENANCE_AUDIT=PASS/FROZEN
2026-09-01 08:00:47 +02:00

794 lines
38 KiB
C++
Raw Blame History

#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 bool relative_result_equal(
const Lardon3DSparseGeometryRelativePoseResult &left,
const Lardon3DSparseGeometryRelativePoseResult &right) {
return std::memcmp(left.pose_ba.rotation_cw, right.pose_ba.rotation_cw,
sizeof(left.pose_ba.rotation_cw)) == 0 &&
std::memcmp(left.pose_ba.translation_cw, right.pose_ba.translation_cw,
sizeof(left.pose_ba.translation_cw)) == 0 &&
left.inlier_count == right.inlier_count &&
left.inlier_ratio == right.inlier_ratio &&
left.median_parallax_rad == right.median_parallax_rad &&
left.inlier_mask == right.inlier_mask &&
left.inlier_mask_capacity == right.inlier_mask_capacity;
}
static bool pnp_result_equal(const Lardon3DSparseGeometryPnPResult &left,
const Lardon3DSparseGeometryPnPResult &right) {
return std::memcmp(left.pose_cw.rotation_cw, right.pose_cw.rotation_cw,
sizeof(left.pose_cw.rotation_cw)) == 0 &&
std::memcmp(left.pose_cw.translation_cw, right.pose_cw.translation_cw,
sizeof(left.pose_cw.translation_cw)) == 0 &&
left.inlier_count == right.inlier_count &&
left.inlier_ratio == right.inlier_ratio &&
left.inlier_mask == right.inlier_mask &&
left.inlier_mask_capacity == right.inlier_mask_capacity;
}
static int signed_parameter_boundary_test() {
const Lardon3DSparseGeometryCalibration calibration =
{1280, 960, 800, 800, 640, 480, 0, 0, 0, 0};
constexpr size_t count = 8;
Lardon3DSparseGeometryPoint2 relative_a[count];
Lardon3DSparseGeometryPoint2 relative_b[count];
Lardon3DSparseGeometryPoint3 pnp_points[count];
Lardon3DSparseGeometryPoint2 pnp_pixels[count];
for (size_t index = 0; index < count; ++index) {
relative_a[index] = {500.0 + static_cast<double>(index), 480.0};
relative_b[index] = {510.0 + static_cast<double>(index), 480.0};
pnp_points[index] = {static_cast<double>(index), 0.0, 4.0};
pnp_pixels[index] = {500.0 + static_cast<double>(index), 480.0};
}
auto relative_rejects_without_mutation =
[&](uint32_t max_iterations, uint32_t minimum_inliers) {
uint8_t mask[count];
std::fill(mask, mask + count, UINT8_C(0xa5));
uint8_t mask_before[count];
std::memcpy(mask_before, mask, sizeof(mask));
Lardon3DSparseGeometryRelativePoseResult result = {};
for (size_t index = 0; index < 9; ++index)
result.pose_ba.rotation_cw[index] = 10.0 + static_cast<double>(index);
for (size_t index = 0; index < 3; ++index)
result.pose_ba.translation_cw[index] =
20.0 + static_cast<double>(index);
result.inlier_count = 71;
result.inlier_ratio = 0.375;
result.median_parallax_rad = 0.125;
result.inlier_mask = mask;
result.inlier_mask_capacity = count;
const Lardon3DSparseGeometryRelativePoseResult before = result;
Lardon3DSparseGeometryRelativePoseParameters parameters =
{1.0, 0.999, max_iterations, minimum_inliers, 0.5, 1e-4, 0.5, 7};
const Lardon3DSparseGeometryResult status =
lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, relative_a, relative_b, count,
&parameters, &result);
return status == LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT &&
relative_result_equal(result, before) &&
std::memcmp(mask, mask_before, sizeof(mask)) == 0;
};
auto pnp_rejects_without_mutation =
[&](uint32_t max_iterations, uint32_t minimum_inliers) {
uint8_t mask[count];
std::fill(mask, mask + count, UINT8_C(0x5a));
uint8_t mask_before[count];
std::memcpy(mask_before, mask, sizeof(mask));
Lardon3DSparseGeometryPnPResult result = {};
for (size_t index = 0; index < 9; ++index)
result.pose_cw.rotation_cw[index] = 30.0 + static_cast<double>(index);
for (size_t index = 0; index < 3; ++index)
result.pose_cw.translation_cw[index] =
40.0 + static_cast<double>(index);
result.inlier_count = 83;
result.inlier_ratio = 0.625;
result.inlier_mask = mask;
result.inlier_mask_capacity = count;
const Lardon3DSparseGeometryPnPResult before = result;
Lardon3DSparseGeometryPnPParameters parameters =
{1.0, 0.999, max_iterations, minimum_inliers, 0.5, 9};
const Lardon3DSparseGeometryResult status = lardon3d_sparse_geometry_pnp(
&calibration, pnp_points, pnp_pixels, count, &parameters, &result);
return status == LARDON3D_SPARSE_GEOMETRY_INVALID_ARGUMENT &&
pnp_result_equal(result, before) &&
std::memcmp(mask, mask_before, sizeof(mask)) == 0;
};
const uint32_t signed_maximum =
static_cast<uint32_t>(std::numeric_limits<int>::max());
const uint32_t unrepresentable_values[] = {
signed_maximum + UINT32_C(1), std::numeric_limits<uint32_t>::max()};
for (uint32_t value : unrepresentable_values) {
CHECK(relative_rejects_without_mutation(value, 6));
CHECK(relative_rejects_without_mutation(100, value));
CHECK(pnp_rejects_without_mutation(value, 6));
CHECK(pnp_rejects_without_mutation(100, value));
}
CHECK(relative_rejects_without_mutation(0, 6));
CHECK(pnp_rejects_without_mutation(0, 6));
/*
* INT_MAX itself remains valid. These collinear fixtures reach the existing
* deterministic degeneracy gate without asking either robust solver to run
* an impractically large iteration budget.
*/
Lardon3DSparseGeometryRelativePoseParameters relative_parameters =
{1.0, 0.999, signed_maximum, signed_maximum, 0.5, 1e-4, 0.5, 11};
Lardon3DSparseGeometryRelativePoseResult relative_result = {};
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, relative_a, relative_b, count,
&relative_parameters, &relative_result) ==
LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
relative_parameters.max_iterations = 1;
relative_parameters.minimum_inliers = 0;
CHECK(lardon3d_sparse_geometry_relative_pose(
&calibration, &calibration, relative_a, relative_b, count,
&relative_parameters, &relative_result) ==
LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
Lardon3DSparseGeometryPnPParameters pnp_parameters =
{1.0, 0.999, signed_maximum, signed_maximum, 0.5, 13};
Lardon3DSparseGeometryPnPResult pnp_result = {};
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, pnp_points, pnp_pixels, count, &pnp_parameters,
&pnp_result) == LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
pnp_parameters.max_iterations = 1;
pnp_parameters.minimum_inliers = 0;
CHECK(lardon3d_sparse_geometry_pnp(
&calibration, pnp_points, pnp_pixels, count, &pnp_parameters,
&pnp_result) == LARDON3D_SPARSE_GEOMETRY_DEGENERATE);
return 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) {
CHECK(signed_parameter_boundary_test() == 0);
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;
}
Reference in a new issue View git blame Copy permalink