mirror of
https://github.com/lbgos/rift-s-linux.git
synced 2026-10-18 00:01:59 +02:00
345 lines
14 KiB
C++
345 lines
14 KiB
C++
// Copyright 2026, lbgos
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
#include "catch_amalgamated.hpp"
|
|
#include "internal/constrained_pose.h"
|
|
#include "internal/sample.h"
|
|
#include <array>
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include "constellation_rift_s_data.hpp"
|
|
|
|
namespace {
|
|
struct FactoryCamera
|
|
{
|
|
std::array<float, 4> intrinsics;
|
|
std::array<float, 6> radial;
|
|
std::array<float, 2> tangential;
|
|
xrt_matrix_4x4 device_from_camera;
|
|
};
|
|
// VM124 factory camera calibration recorded in the real r14 session.
|
|
const std::array<FactoryCamera, 5> factory = {{
|
|
{{190.499924f, 190.499924f, 312.359253f, 233.292801f},
|
|
{0.274657339f, 0.072651282f, -0.284994245f, 0.224762872f, -0.0842394754f, 0.0130622024f},
|
|
{-0.000742965436f, -0.000313968281f},
|
|
{{-0.000666916079f, 0.936471105f, -0.350744158f, 0.0f, 0.999993384f, -0.000631041592f, -0.00358627178f, 0.0f,
|
|
-0.00357977417f, -0.350744218f, -0.936464429f, 0.0f, -0.055822663f, -0.0286783446f, -0.0698858649f, 1.0f}}},
|
|
{{190.177917f, 190.177917f, 315.850128f, 237.907181f},
|
|
{0.279467911f, 0.0535824969f, -0.255054951f, 0.202277347f, -0.0756442547f, 0.0117395986f},
|
|
{4.58915711e-05f, -0.000414691196f},
|
|
{{-0.00550019974f, 0.936006129f, -0.351940721f, 0.0f, 0.999975204f, 0.00669397181f, 0.00217518141f, 0.0f,
|
|
0.00439186441f, -0.351920038f, -0.936019778f, 0.0f, 0.0548296794f, -0.0278966054f, -0.0701900721f, 1.0f}}},
|
|
{{190.294937f, 190.294937f, 318.57016f, 233.739182f},
|
|
{0.268673301f, 0.0967906564f, -0.325616896f, 0.257986337f, -0.0972961113f, 0.0150451222f},
|
|
{0.000216994711f, 0.000161503413f},
|
|
{{-0.369307697f, 0.877442598f, -0.306114942f, 0.0f, -0.0468660891f, -0.346567303f, -0.936853588f, 0.0f,
|
|
-0.928124666f, -0.33164084f, 0.169112265f, 0.0f, -0.0773649067f, -0.0278907306f, -0.0490732379f, 1.0f}}},
|
|
{{190.489212f, 190.489212f, 319.239746f, 239.368988f},
|
|
{0.276072472f, 0.0793345198f, -0.300511897f, 0.236862913f, -0.0880616605f, 0.0134672392f},
|
|
{0.000866621442f, -9.05699198e-05f},
|
|
{{0.372085989f, 0.87367934f, -0.313426882f, 0.0f, -0.0441717803f, 0.353956819f, 0.934218109f, 0.0f, 0.927146614f,
|
|
-0.333764851f, 0.170294344f, 0.0f, 0.0775006264f, -0.027320113f, -0.0488821045f, 1.0f}}},
|
|
{{189.886932f, 189.886932f, 315.196442f, 236.638504f},
|
|
{0.275883079f, 0.0488088988f, -0.227129221f, 0.164120957f, -0.0545484312f, 0.00755312294f},
|
|
{0.00021202871f, -0.000509813079f},
|
|
{{-0.99999702f, -0.000720949087f, -0.00232819817f, 0.0f, -0.00232536392f, -0.00390743697f, 0.999989688f, 0.0f,
|
|
-0.000730038912f, 0.999992132f, 0.00390574872f, 0.0f, -0.000778796442f, 0.0459865034f, -0.0662369207f, 1.0f}}},
|
|
}};
|
|
camera_model
|
|
calibrated_camera(unsigned index)
|
|
{
|
|
const auto &f = factory[index];
|
|
camera_model c{};
|
|
c.width = 640;
|
|
c.height = 480;
|
|
c.calib.fx = f.intrinsics[0];
|
|
c.calib.fy = f.intrinsics[1];
|
|
c.calib.cx = f.intrinsics[2];
|
|
c.calib.cy = f.intrinsics[3];
|
|
c.fisheye62_valid = true;
|
|
std::copy(f.radial.begin(), f.radial.end(), c.fisheye62_radial);
|
|
c.fisheye62_p1 = f.tangential[0];
|
|
c.fisheye62_p2 = f.tangential[1];
|
|
return c;
|
|
}
|
|
// Project LED fixtures forward without calling the production inverse.
|
|
xrt_vec2
|
|
factory_project(const FactoryCamera &c, xrt_vec3 p)
|
|
{
|
|
double r = std::hypot(p.x, p.y), theta = std::atan2(r, p.z);
|
|
double a = r > 1e-12 ? theta * p.x / r : 0, b = r > 1e-12 ? theta * p.y / r : 0;
|
|
double polynomial = 1;
|
|
for (unsigned i = 0; i < 6; ++i)
|
|
polynomial += c.radial[i] * std::pow(theta, 2 * i + 2);
|
|
double p1 = c.tangential[0], p2 = c.tangential[1];
|
|
double x = a * polynomial, y = b * polynomial;
|
|
double norm2 = x * x + y * y, dot = p1 * x + p2 * y;
|
|
return {float(c.intrinsics[0] * (x + 2 * x * dot + p1 * norm2) + c.intrinsics[2]),
|
|
float(c.intrinsics[1] * (y + 2 * y * dot + p2 * norm2) + c.intrinsics[3])};
|
|
}
|
|
double
|
|
error(xrt_vec3 a, xrt_vec3 b)
|
|
{
|
|
return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2) + std::pow(a.z - b.z, 2));
|
|
}
|
|
} // namespace
|
|
|
|
TEST_CASE("Rift S Fisheye62 matches calibrated forward pixel references")
|
|
{
|
|
// Fixed forward references cover all five calibrations and both tangential axes.
|
|
const std::array<xrt_vec2, 3> angular = {{{0.8f, 0.6f}, {-1.1f, 0.4f}, {0.2f, -0.7f}}};
|
|
const std::array<std::array<xrt_vec2, 3>, 5> pixels = {{
|
|
{{{497.100364f, 371.917144f}, {49.367862f, 328.687290f}, {355.628758f, 81.468439f}}},
|
|
{{{500.828640f, 376.514696f}, {53.946196f, 332.982240f}, {359.179827f, 86.220055f}}},
|
|
{{{503.808216f, 372.667374f}, {56.865364f, 329.002682f}, {361.892456f, 82.231382f}}},
|
|
{{{505.258047f, 378.673729f}, {57.725275f, 334.557148f}, {362.776597f, 87.375536f}}},
|
|
{{{499.717526f, 374.841928f}, {54.394805f, 331.299469f}, {358.441628f, 85.310572f}}},
|
|
}};
|
|
for (unsigned k = 0; k < factory.size(); ++k) {
|
|
auto c = calibrated_camera(k);
|
|
for (unsigned i = 0; i < angular.size(); ++i) {
|
|
CAPTURE(k, i);
|
|
const double theta = std::hypot(angular[i].x, angular[i].y);
|
|
const double scale = std::sin(theta) / theta;
|
|
const xrt_vec3 ray = {float(angular[i].x * scale), float(angular[i].y * scale),
|
|
float(std::cos(theta))};
|
|
xrt_vec2 pixel;
|
|
REQUIRE(camera_model_project(&c, ray.x, ray.y, ray.z, &pixel.x, &pixel.y));
|
|
CHECK(pixel.x == Catch::Approx(pixels[k][i].x).epsilon(0).margin(0.0001));
|
|
CHECK(pixel.y == Catch::Approx(pixels[k][i].y).epsilon(0).margin(0.0001));
|
|
xrt_vec3 recovered;
|
|
REQUIRE(camera_model_unproject(&c, pixels[k][i].x, pixels[k][i].y, &recovered.x, &recovered.y,
|
|
&recovered.z));
|
|
CHECK(error(recovered, ray) < 1e-6);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_CASE("Rift S factory Fisheye62 pixel bearings round trip across all five sensors")
|
|
{
|
|
for (unsigned k = 0; k < factory.size(); ++k) {
|
|
auto c = calibrated_camera(k);
|
|
for (unsigned y = 0; y < 480; y += 16)
|
|
for (unsigned x = 0; x < 640; x += 16) {
|
|
xrt_vec3 ray;
|
|
REQUIRE(camera_model_unproject(&c, x, y, &ray.x, &ray.y, &ray.z));
|
|
auto pixel = factory_project(factory[k], ray);
|
|
REQUIRE(std::hypot(pixel.x - x, pixel.y - y) < 0.0002);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_CASE("Rift S known LED geometry recovers offset and unit lateral gain through native calibration")
|
|
{
|
|
auto leds = rift_s_ring_leds;
|
|
t_constellation_led_model model{};
|
|
model.leds = leds.data();
|
|
model.num_leds = leds.size();
|
|
constellation_constrained_config config;
|
|
constellation_constrained_default_config(&config);
|
|
config.time_budget_us = 2000000;
|
|
// Isolate metric calibration: geometry and LED identities are known, no acquisition policy.
|
|
config.minimum_tracking_inliers = 3;
|
|
config.minimum_tracking_distinct_leds = 3;
|
|
config.minimum_tilt_distinct_leds = 256;
|
|
config.ambiguity_chi_square = 0.00001;
|
|
double max_error = 0, min_gain = 2, max_gain = 0;
|
|
unsigned recovered = 0;
|
|
for (unsigned k = 0; k < factory.size(); ++k) {
|
|
auto c = calibrated_camera(k);
|
|
xrt_pose world_camera;
|
|
math_pose_from_isometry(&factory[k].device_from_camera, &world_camera);
|
|
// Native camera rays transformed by the actual factory extrinsic into device/world coordinates.
|
|
constellation_constrained_view view{};
|
|
view.calib = &c;
|
|
view.P_world_cam = world_camera;
|
|
math_pose_invert(&world_camera, &view.P_cam_world);
|
|
blobservation observation{};
|
|
view.observation = &observation;
|
|
xrt_vec3 previous_truth{}, previous_recovered{};
|
|
for (int step = -6; step <= 6; ++step) {
|
|
xrt_pose camera_pose = XRT_POSE_IDENTITY;
|
|
camera_pose.position = {step * .11f, -.04f, .45f};
|
|
const xrt_vec3 axis = {1, 0, 0};
|
|
math_quat_from_angle_vector(3.14159265f, &axis, &camera_pose.orientation);
|
|
xrt_pose truth;
|
|
math_pose_transform(&world_camera, &camera_pose, &truth);
|
|
observation = {};
|
|
for (const auto &led : leds) {
|
|
xrt_vec3 local, normal;
|
|
math_pose_transform_point(&camera_pose, &led.pos, &local);
|
|
math_quat_rotate_vec3(&camera_pose.orientation, &led.dir, &normal);
|
|
xrt_vec3 to_camera = {-local.x, -local.y, -local.z};
|
|
math_vec3_normalize(&to_camera);
|
|
if (normal.x * to_camera.x + normal.y * to_camera.y + normal.z * to_camera.z <
|
|
std::cos(LED_ANGLE * M_PI / 180))
|
|
continue;
|
|
auto pixel = factory_project(factory[k], local);
|
|
if (pixel.x < 0 || pixel.x >= 640 || pixel.y < 0 || pixel.y >= 480)
|
|
continue;
|
|
auto &b = observation.blobs[observation.num_blobs++];
|
|
b.x = pixel.x;
|
|
b.y = pixel.y;
|
|
}
|
|
if (observation.num_blobs < 4)
|
|
continue;
|
|
// Translation recovery from an imperfect retained pose exercises the actual multi-LED fitter.
|
|
xrt_pose prior = truth;
|
|
prior.position.x += .001f;
|
|
prior.position.y -= .001f;
|
|
constellation_constrained_result result{};
|
|
REQUIRE(constellation_constrained_verify_pose(
|
|
&model, &view, 1, &prior, CONSTELLATION_CONSTRAINED_PRIOR, &config, &result));
|
|
max_error = std::max(max_error, error(truth.position, result.P_world_model.position));
|
|
if (step > -6) {
|
|
double truth_motion = error(truth.position, previous_truth);
|
|
if (truth_motion > .01) {
|
|
double gain =
|
|
error(result.P_world_model.position, previous_recovered) / truth_motion;
|
|
min_gain = std::min(min_gain, gain);
|
|
max_gain = std::max(max_gain, gain);
|
|
}
|
|
}
|
|
previous_truth = truth.position;
|
|
previous_recovered = result.P_world_model.position;
|
|
++recovered;
|
|
}
|
|
}
|
|
INFO("recovered=" << recovered << " worst position m=" << max_error << " gain=" << min_gain << ".."
|
|
<< max_gain);
|
|
CHECK(recovered >= 45);
|
|
CHECK(max_error < 0.00002);
|
|
CHECK(min_gain > .9999);
|
|
CHECK(max_gain < 1.0001);
|
|
}
|
|
|
|
TEST_CASE("Rift S ring recovers depth with unit gain from 0.3 to 0.8 m through cold and tracked search")
|
|
{
|
|
/* The r18 report: controllers drawn farther than the hands. Project the recorded ring through
|
|
* each factory calibration and extrinsic straight ahead of that camera, then recover it with the
|
|
* search the tracker runs: cold (gravity only, no position) and tracked (trusted prior 2 cm off,
|
|
* known heading, 3 ms fast-path budget). Depth must come back unbiased and with unit gain. */
|
|
auto leds = rift_s_ring_leds;
|
|
t_constellation_led_model model{};
|
|
model.id = 3;
|
|
model.leds = leds.data();
|
|
model.num_leds = leds.size();
|
|
double worst = 0, min_gain = 2, max_gain = 0;
|
|
unsigned cold = 0, tracked = 0;
|
|
for (unsigned k = 0; k < factory.size(); ++k) {
|
|
auto c = calibrated_camera(k);
|
|
xrt_pose world_camera;
|
|
math_pose_from_isometry(&factory[k].device_from_camera, &world_camera);
|
|
constellation_constrained_view view{};
|
|
view.calib = &c;
|
|
view.P_world_cam = world_camera;
|
|
math_pose_invert(&world_camera, &view.P_cam_world);
|
|
blobservation observation{};
|
|
view.observation = &observation;
|
|
for (bool trusted : {false, true}) {
|
|
double previous_depth = 0, previous_recovered = 0;
|
|
for (float depth = 0.3f; depth <= 0.801f; depth += 0.1f) {
|
|
CAPTURE(k, trusted, depth);
|
|
xrt_pose camera_pose = XRT_POSE_IDENTITY;
|
|
camera_pose.position = {0.01f, -0.04f, depth};
|
|
const xrt_vec3 axis = {1, 0, 0};
|
|
math_quat_from_angle_vector(3.14159265f, &axis, &camera_pose.orientation);
|
|
xrt_pose truth;
|
|
math_pose_transform(&world_camera, &camera_pose, &truth);
|
|
observation = {};
|
|
for (const auto &led : leds) {
|
|
xrt_vec3 local, normal;
|
|
math_pose_transform_point(&camera_pose, &led.pos, &local);
|
|
math_quat_rotate_vec3(&camera_pose.orientation, &led.dir, &normal);
|
|
xrt_vec3 to_camera = {-local.x, -local.y, -local.z};
|
|
math_vec3_normalize(&to_camera);
|
|
if (normal.x * to_camera.x + normal.y * to_camera.y + normal.z * to_camera.z <
|
|
std::cos(LED_ANGLE * M_PI / 180))
|
|
continue;
|
|
auto pixel = factory_project(factory[k], local);
|
|
auto &b = observation.blobs[observation.num_blobs++];
|
|
b = {};
|
|
b.x = pixel.x;
|
|
b.y = pixel.y;
|
|
b.width = b.height = 3;
|
|
b.led_id = LED_INVALID_ID;
|
|
}
|
|
REQUIRE(observation.num_blobs >= 5);
|
|
constellation_constrained_config config;
|
|
constellation_constrained_default_config(&config);
|
|
xrt_pose prior = truth;
|
|
if (trusted) {
|
|
config.time_budget_us = 3000;
|
|
#if defined(__SANITIZE_ADDRESS__) || defined(__SANITIZE_THREAD__)
|
|
config.time_budget_us = 200000;
|
|
#endif
|
|
prior.position.x += 0.02f;
|
|
} else {
|
|
prior.position = {42, 42, 42};
|
|
}
|
|
constellation_constrained_result result{};
|
|
REQUIRE(constellation_constrained_search(&model, &view, 1, &prior, trusted, trusted, &config,
|
|
&result));
|
|
(trusted ? tracked : cold)++;
|
|
worst = std::max(worst, error(truth.position, result.P_world_model.position));
|
|
// Depth along this camera's axis, recovered and true.
|
|
xrt_vec3 in_camera;
|
|
math_pose_transform_point(&view.P_cam_world, &result.P_world_model.position, &in_camera);
|
|
if (previous_depth > 0) {
|
|
double gain = (in_camera.z - previous_recovered) / (depth - previous_depth);
|
|
min_gain = std::min(min_gain, gain);
|
|
max_gain = std::max(max_gain, gain);
|
|
}
|
|
previous_depth = depth;
|
|
previous_recovered = in_camera.z;
|
|
}
|
|
}
|
|
}
|
|
INFO("cold=" << cold << " tracked=" << tracked << " worst m=" << worst << " depth gain=" << min_gain << ".."
|
|
<< max_gain);
|
|
CHECK(cold == 30);
|
|
CHECK(tracked == 30);
|
|
CHECK(worst < 0.0005);
|
|
CHECK(min_gain > 0.999);
|
|
CHECK(max_gain < 1.001);
|
|
}
|
|
|
|
TEST_CASE("Rift S wide-angle KB4 bearing converges at the sensor diagonal")
|
|
{
|
|
t_camera_model_params c{};
|
|
c.model = T_DISTORTION_FISHEYE_KB4;
|
|
c.fx = c.fy = rift_s_ring_intrinsics[0];
|
|
c.cx = rift_s_ring_intrinsics[2];
|
|
c.cy = rift_s_ring_intrinsics[3];
|
|
c.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]};
|
|
const double theta = 1.55, bearing = .65;
|
|
xrt_vec3 truth = {float(std::sin(theta) * std::cos(bearing)), float(std::sin(theta) * std::sin(bearing)),
|
|
float(std::cos(theta))};
|
|
float u, v;
|
|
xrt_vec3 recovered;
|
|
REQUIRE(t_camera_models_project(&c, truth.x, truth.y, truth.z, &u, &v));
|
|
REQUIRE(t_camera_models_unproject(&c, u, v, &recovered.x, &recovered.y, &recovered.z));
|
|
CHECK(error(truth, recovered) < .000001);
|
|
}
|
|
|
|
TEST_CASE("Native camera failures leave explicit invalid coordinates")
|
|
{
|
|
auto camera = calibrated_camera(0);
|
|
float x = 7, y = 8, z = 9;
|
|
CHECK_FALSE(camera_model_unproject(&camera, NAN, 240, &x, &y, &z));
|
|
CHECK(std::isnan(x));
|
|
CHECK(std::isnan(y));
|
|
CHECK(std::isnan(z));
|
|
CHECK_FALSE(camera_model_undistort(&camera, NAN, 240, &x, &y));
|
|
CHECK(std::isnan(x));
|
|
CHECK(std::isnan(y));
|
|
CHECK_FALSE(camera_model_project(&camera, 0, 0, -1, &x, &y));
|
|
CHECK(std::isnan(x));
|
|
CHECK(std::isnan(y));
|
|
camera.fisheye62_valid = false;
|
|
camera.calib.model = T_DISTORTION_FISHEYE_KB4;
|
|
CHECK_FALSE(camera_model_unproject(&camera, NAN, 240, &x, &y, &z));
|
|
CHECK(std::isnan(x));
|
|
CHECK(std::isnan(y));
|
|
CHECK(std::isnan(z));
|
|
}
|