From 2ed603b3044d1fbac1dd67e4cc4699cd7cb7838d Mon Sep 17 00:00:00 2001 From: DeeJanuz <45082401+DeeJanuz@users.noreply.github.com> Date: Thu, 8 Oct 2026 21:47:28 -0600 Subject: [PATCH] Hands: the hand ray model (fh_ray.h), with tests and an offline evaluation hands/include/fh_ray.h, shared by the pointer helper and ft-screens (design sections 2.4-2.5): a body model (torso yaw = head yaw low-passed, at most 30 deg behind; shoulders from it), the ray from the shoulder through the knuckles (20 ms prediction), the pointing gate (show tests for 120 ms, hide after 250 ms of a failed keep test; near and "off" hide at once; held through a ray pinch), and One Euro filters on yaw, pitch and origin. - pointer/tests/handray-test.cpp: body, gate timing and tests, filter (just test-c). - ft-handreplay --ray FILE: per set and hand, what the helper reads. - pointer/tools/rayeval: the ray over a replay, with the head poses from the ft-handtest probe's log of the same session (clock matched by the published capture times): how often it shows, why not, jitter, the ray's shift into a pinch. Co-Authored-By: Claude Opus 5.5 --- hands/include/fh_ray.h | 349 +++++++++++++++++++++++++++++++ hands/track/replay.cpp | 29 +++ pointer/tests/handray-test.cpp | 273 ++++++++++++++++++++++++ pointer/tools/rayeval.cpp | 372 +++++++++++++++++++++++++++++++++ 4 files changed, 1023 insertions(+) create mode 100644 hands/include/fh_ray.h create mode 100644 pointer/tests/handray-test.cpp create mode 100644 pointer/tools/rayeval.cpp diff --git a/hands/include/fh_ray.h b/hands/include/fh_ray.h new file mode 100644 index 0000000..994bcfc --- /dev/null +++ b/hands/include/fh_ray.h @@ -0,0 +1,349 @@ +// fh_ray - the hand ray: where a bare hand points, Quest-style, and when it counts as pointing. +// Shared by the pointer helper (the ray drives the ft_pointer SteamVR device: pointer/helper) +// and ft-screens (a second hand's ray for two-hand stretch), so both agree. +// Design: frame-hands/notes/hand-interaction.md, sections 2.4 and 2.5. +// +// Header-only C++17, no dependencies. Everything is in the room (OpenVR's standing universe: +// +x right, +y up, -z forward), metres, radians inside and degrees in Params, seconds on one +// steady clock (CLOCK_MONOTONIC: ft-hands' capture times, as seconds). +// +// Body model (no torso tracking): the torso's yaw is the head's, low-passed (torsoTau, 0.6 s) and +// never more than torsoMaxLag (30 deg) behind. The neck is the head moved by neck (10 cm down, 8 cm +// back), turned by the head's yaw only; a shoulder is the neck moved by (+-shoulderX, shoulderY, +// 0) (17 cm out, 13 cm down), turned by the torso's yaw. Head pitch and roll are ignored, so +// nodding doesn't swing the ray. +// +// Ray: from the shoulder S through the knuckle point K (the index and middle knuckles: the pinch +// point, which holds still as the fingers close, so pinching barely turns the ray). The direction +// is D = normalize(K - S); the origin is K + originAhead (3 cm) along D, in front of the fingers. +// K is predicted predict (20 ms) ahead with its room velocity (differenced across frames). +// +// Pointing (Gate): the ray shows once every "show" test has held for showFor (120 ms), and hides +// once some "keep" test has failed for hideFor (250 ms): +// test show keep (hide past it) +// side the arbiter says hand (and the caller (at once otherwise: "off") +// allows it: in a game, the mouse) +// tracked a two-camera (STEREO) frame within a frame within lostFor (250 ms); one-camera +// stereoWithin (200 ms) depth is 2-6 cm off, fine for a short gap +// raised K at most raisedShow (0.30 m) below raisedHide (0.40 m): gaze pinches sat +// the eyes 0.35-0.45 m below the eyes, elbow resting +// forward K at least forwardShow (0.25 m) ahead forwardHide (0.20 m) +// of the eyes, along the torso's yaw +// palm away palm normal . D >= palmShow (0.3) palmHide (0.0): palm toward you is reserved +// forearm elbow-to-wrist pitch >= forearmShow forearmHide (-50 deg); only with an elbow +// (-35 deg) of elbowConfMin (0.5) confidence +// aim D's pitch in [pitchMin, pitchMax] [pitchMinHide, pitchMaxHide] (-60, 75), +// (-45, 60), yaw within yawShow (70) yawHide (80) +// of the head's +// not near not near a panel (ft-screens' near near: at once (near wins) +// claim, or the fingertip at an overlay) +// not a fist no grip down a grip down +// While a ray pinch is held (Hold), the ray stays up whatever the tests say, until hideFor after +// the release. +// +// Filter (RayFilter): One Euro filters (Casiez, Roussel and Vogel, CHI 2012) on the ray's yaw and +// pitch (minCutoff 0.8 Hz, beta 0.02 per deg/s, dCutoff 1 Hz) and per axis on the origin +// (originMinCutoff 1.5 Hz): the aim matters more than where the beam starts. +#pragma once + +#include +#include +#include +#include + +namespace fhray { + +struct V3 { + double x = 0, y = 0, z = 0; +}; +inline V3 operator+(V3 a, V3 b) { return {a.x + b.x, a.y + b.y, a.z + b.z}; } +inline V3 operator-(V3 a, V3 b) { return {a.x - b.x, a.y - b.y, a.z - b.z}; } +inline V3 operator*(V3 a, double s) { return {a.x * s, a.y * s, a.z * s}; } +inline double Dot(V3 a, V3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; } +inline double Len(V3 a) { return std::sqrt(Dot(a, a)); } +inline V3 Norm(V3 a) { + const double l = Len(a); + return l > 1e-12 ? a * (1 / l) : V3{0, 0, -1}; +} +constexpr double kDeg = M_PI / 180; +// Yaw: 0 along -z, positive turning left (about +y), as OpenVR; pitch: up positive. +inline double YawOf(V3 d) { return std::atan2(-d.x, -d.z); } +inline double PitchOf(V3 d) { return std::asin(std::clamp(Norm(d).y, -1.0, 1.0)); } +inline V3 Dir(double yaw, double pitch) { + return {-std::sin(yaw) * std::cos(pitch), std::sin(pitch), -std::cos(yaw) * std::cos(pitch)}; +} +// v turned by yaw about +y. +inline V3 TurnYaw(V3 v, double yaw) { + const double c = std::cos(yaw), s = std::sin(yaw); + return {v.x * c + v.z * s, v.y, -v.x * s + v.z * c}; +} +inline double WrapPi(double a) { return std::remainder(a, 2 * M_PI); } + +struct Params { + // body + double torsoTau = 0.6, torsoMaxLag = 30; + V3 neck{0, -0.10, 0.08}; + double shoulderX = 0.17, shoulderY = -0.13; + // ray + double originAhead = 0.03, predict = 0.020; + // gate + double showFor = 0.12, hideFor = 0.25; + double stereoWithin = 0.2, lostFor = 0.25; + double raisedShow = 0.30, raisedHide = 0.40; + double forwardShow = 0.25, forwardHide = 0.20; + double palmShow = 0.3, palmHide = 0.0; + double forearmShow = -35, forearmHide = -50, elbowConfMin = 0.5; + double pitchMin = -45, pitchMax = 60, pitchMinHide = -60, pitchMaxHide = 75; + double yawShow = 70, yawHide = 80; + // filter + double minCutoff = 0.8, beta = 0.02, dCutoff = 1.0, originMinCutoff = 1.5, originBeta = 0.5; +}; + +// The torso and shoulders, from the head. +class Body { +public: + explicit Body(const Params &p = {}) : p_(p) {} + void SetParams(const Params &p) { p_ = p; } + // The head's position and forward direction (-z of its pose) at time t. + void Update(double t, V3 head, V3 forward) { + head_ = head; + headYaw_ = YawOf(forward); + if (!init_) { + torso_ = headYaw_, t_ = t, init_ = true; + return; + } + const double dt = std::clamp(t - t_, 0.0, 0.5); + t_ = t; + double lag = WrapPi(headYaw_ - torso_); + lag *= std::exp(-dt / std::max(1e-3, p_.torsoTau)); + const double max = p_.torsoMaxLag * kDeg; + lag = std::clamp(lag, -max, max); + torso_ = WrapPi(headYaw_ - lag); + } + bool Ready() const { return init_; } + V3 Head() const { return head_; } + double HeadYaw() const { return headYaw_; } + double TorsoYaw() const { return torso_; } + V3 TorsoForward() const { return Dir(torso_, 0); } + V3 Neck() const { return head_ + TurnYaw(p_.neck, headYaw_); } + V3 Shoulder(int side) const { + return Neck() + TurnYaw(V3{side ? p_.shoulderX : -p_.shoulderX, p_.shoulderY, 0}, torso_); + } + +private: + Params p_; + V3 head_; + double headYaw_ = 0, torso_ = 0, t_ = 0; + bool init_ = false; +}; + +struct Ray { + V3 origin, dir{0, 0, -1}; +}; + +// One hand at one capture time, in the room. +struct HandFrame { + double t = 0; // capture time, s + bool stereo = false; // triangulated from two or more cameras + V3 knuckle; // K: the index and middle knuckles + V3 wrist; + V3 palmNormal; // unit, out of the palm + V3 elbow; + double elbowConf = 0; // 0: the arm model's guess + bool grip = false; // a grip (closed hand) is down + bool near = false; // near a panel (see "not near" at the top) +}; + +// What each test said for one frame (degrees and metres, for logs and the offline evaluation). +struct Tests { + double below = 0, ahead = 0, palm = 0, forearm = 0, pitch = 0, yawOff = 0; + bool forearmUsed = false; + bool show = false, keep = false; // every show test, every keep test (not counting side, tracking) + std::string fails; // the show tests that failed +}; + +inline Tests Evaluate(const Params &p, const Body &body, const HandFrame &f, const Ray &ray) { + Tests r; + const V3 eye = body.Head(); + r.below = eye.y - f.knuckle.y; + const V3 fwd = body.TorsoForward(); + r.ahead = Dot(f.knuckle - eye, fwd); + r.palm = Dot(f.palmNormal, ray.dir); + r.pitch = PitchOf(ray.dir) / kDeg; + r.yawOff = std::fabs(WrapPi(YawOf(ray.dir) - body.HeadYaw())) / kDeg; + r.forearmUsed = f.elbowConf >= p.elbowConfMin; + if (r.forearmUsed) r.forearm = PitchOf(f.wrist - f.elbow) / kDeg; + bool show = true, keep = true; + auto test = [&](bool s, bool k, const char *name) { + if (!s) r.fails += r.fails.empty() ? name : std::string(",") + name; + show = show && s, keep = keep && k; + }; + test(r.below <= p.raisedShow, r.below <= p.raisedHide, "raised"); + test(r.ahead >= p.forwardShow, r.ahead >= p.forwardHide, "forward"); + test(r.palm >= p.palmShow, r.palm >= p.palmHide, "palm"); + if (r.forearmUsed) test(r.forearm >= p.forearmShow, r.forearm >= p.forearmHide, "forearm"); + test(r.pitch >= p.pitchMin && r.pitch <= p.pitchMax, r.pitch >= p.pitchMinHide && r.pitch <= p.pitchMaxHide, "aim"); + test(r.yawOff <= p.yawShow, r.yawOff <= p.yawHide, "yaw"); + test(!f.grip, !f.grip, "grip"); + r.show = show, r.keep = keep; + return r; +} + +// One Euro filter for one value. +class OneEuro { +public: + OneEuro(double minCutoff = 1, double beta = 0, double dCutoff = 1) : min_(minCutoff), beta_(beta), dc_(dCutoff) {} + void Set(double minCutoff, double beta, double dCutoff) { min_ = minCutoff, beta_ = beta, dc_ = dCutoff; } + void Reset() { init_ = false; } + double operator()(double v, double t) { + if (!init_ || t <= t_) { + if (!init_) x_ = v, dx_ = 0, t_ = t, init_ = true; + return x_; + } + const double dt = t - t_; + t_ = t; + const double d = (v - x_) / dt; + dx_ += Alpha(dc_, dt) * (d - dx_); + x_ += Alpha(min_ + beta_ * std::fabs(dx_), dt) * (v - x_); + return x_; + } + double Value() const { return x_; } + +private: + static double Alpha(double cutoff, double dt) { + const double tau = 1 / (2 * M_PI * cutoff); + return 1 / (1 + tau / dt); + } + double min_, beta_, dc_, x_ = 0, dx_ = 0, t_ = 0; + bool init_ = false; +}; + +// The ray's filter: yaw and pitch in degrees, the origin per axis. +class RayFilter { +public: + explicit RayFilter(const Params &p = {}) { SetParams(p); } + void SetParams(const Params &p) { + yaw_.Set(p.minCutoff, p.beta, p.dCutoff), pitch_.Set(p.minCutoff, p.beta, p.dCutoff); + for (OneEuro &o : o_) o.Set(p.originMinCutoff, p.originBeta, p.dCutoff); + } + void Reset() { + yaw_.Reset(), pitch_.Reset(); + for (OneEuro &o : o_) o.Reset(); + have_ = false; + } + Ray operator()(const Ray &raw, double t) { + double y = YawOf(raw.dir) / kDeg; + if (have_) y = lastYaw_ + std::remainder(y - lastYaw_, 360.0); // unwrapped + lastYaw_ = y, have_ = true; + const double fy = yaw_(y, t), fp = pitch_(PitchOf(raw.dir) / kDeg, t); + Ray r; + r.dir = Dir(fy * kDeg, fp * kDeg); + r.origin = {o_[0](raw.origin.x, t), o_[1](raw.origin.y, t), o_[2](raw.origin.z, t)}; + return r; + } + +private: + OneEuro yaw_, pitch_, o_[3]; + double lastYaw_ = 0; + bool have_ = false; +}; + +// One side's ray: prediction, the gate and the filter. +class HandRay { +public: + explicit HandRay(int side, const Params &p = {}) : side_(side), p_(p), filter_(p) {} + void SetParams(const Params &p) { p_ = p, filter_.SetParams(p); } + int Side() const { return side_; } + + // A new frame of this side's hand. sideIsHand: the ray may show (the arbiter says this side is a + // bare hand, and the caller allows it); without it, a ray shown hides at once ("hide off"). + void Frame(const Body &body, const HandFrame &f, bool sideIsHand) { + // K's room velocity, for the prediction + V3 vk; + if (haveLast_ && f.t > lastT_ && f.t - lastT_ < 0.2) vk = (f.knuckle - lastK_) * (1 / (f.t - lastT_)); + lastK_ = f.knuckle, lastT_ = f.t, haveLast_ = true; + const V3 k = f.knuckle + vk * p_.predict; + const V3 s = body.Shoulder(side_); + raw_.dir = Norm(k - s); + raw_.origin = k + raw_.dir * p_.originAhead; + reach_ = Len(f.knuckle - s); + lastFrame_ = f.t; + if (f.stereo) lastStereo_ = f.t; + tests_ = Evaluate(p_, body, f, raw_); + const bool tracked = f.t - lastStereo_ <= p_.stereoWithin; + const bool showOk = sideIsHand && tracked && tests_.show && !f.near; + const bool keepOk = tests_.keep; + if (!tests_.fails.empty()) why_ = tests_.fails; + else if (!tracked) why_ = "one camera"; + else if (f.near) why_ = "near"; + else why_ = ""; + if (!sideIsHand) why_ = "off"; + // the timers run on capture times + if (!shown_) { + if (showOk) { + if (showSince_ < 0) showSince_ = f.t; + if (f.t - showSince_ >= p_.showFor - 1e-9) Show(f.t); + } else { + showSince_ = -1; + } + } else { + if (!sideIsHand) { + Hide("off"); + } else if (f.near && !held_) { + Hide("near"); + } else if (keepOk || held_ || f.t < holdUntil_) { + failSince_ = -1; + } else { + if (failSince_ < 0) failSince_ = f.t; + if (f.t - failSince_ >= p_.hideFor - 1e-9) Hide(tests_.fails.empty() ? "pose" : tests_.fails); + } + } + if (shown_) out_ = filter_(raw_, f.t); + } + // Each tick, with the time now on the capture clock: the hand lost (no frame for lostFor) hides it. + void Tick(double now, bool sideIsHand) { + if (!shown_) return; + if (!sideIsHand) Hide("off"); + else if (!held_ && now - lastFrame_ > p_.lostFor && now >= holdUntil_) Hide("lost"); + } + // A ray pinch is held: the ray stays up until hideFor after it ends. + void Hold(bool on, double t) { + if (held_ && !on) holdUntil_ = t + p_.hideFor; + held_ = on; + } + void Reset() { Hide("reset"); } + + bool Shown() const { return shown_; } + const Ray &Current() const { return out_; } // filtered + const Ray &Raw() const { return raw_; } // unfiltered, predicted + double Reach() const { return reach_; } // |K - S|: arm out or in (push and pull) + const Tests &LastTests() const { return tests_; } + const std::string &Why() const { return why_; } // what fails now ("" if nothing) + // Changes since the caller last cleared it: "show" or "hide ". + std::string event; + +private: + void Show(double t) { + shown_ = true, failSince_ = -1, showSince_ = -1; + filter_.Reset(); + out_ = filter_(raw_, t); + event = "show"; + } + void Hide(const std::string &why) { + if (!shown_) return; + shown_ = false, failSince_ = -1, showSince_ = -1, held_ = false, holdUntil_ = 0; + event = "hide " + why; + } + int side_; + Params p_; + RayFilter filter_; + Ray raw_, out_; + Tests tests_; + std::string why_; + V3 lastK_; + double lastT_ = 0, lastFrame_ = -1e9, lastStereo_ = -1e9, reach_ = 0; + double showSince_ = -1, failSince_ = -1, holdUntil_ = 0; + bool haveLast_ = false, shown_ = false, held_ = false; +}; + +} // namespace fhray diff --git a/hands/track/replay.cpp b/hands/track/replay.cpp index fff93ce..82cec1b 100644 --- a/hands/track/replay.cpp +++ b/hands/track/replay.cpp @@ -59,6 +59,12 @@ // --skeleton-dump FILE: with --labels, a line per hand matched to a label: time, id, stereo, then // the measured, fitted (unfiltered) and labelled landmarks. // --pinch-fitted: also run a second pinch detector on the fitted tips, and report its count. +// --ray FILE: per processed set, a line per hand with what the pointer helper's hand ray reads +// (hands/include/fh_ray.h; pointer/tools/rayeval): the set's capture time (ns, +// CLOCK_MONOTONIC_RAW as recorded), id, side (L/R), stereo, fitted, its side's pinch +// down, grip down, and pinch begun in this set (0/1 each), then the 21 published points +// (the fit's when it has one; head frame), the wrist's frame as a quaternion (x y z w; +// 0 0 0 1 without a fit) and the elbow. // --depth FILE: per processed set, a line per hand for tools/depth_report.py: its views' // cameras, triangulation residual, hand scale, measured and published palm, and // each view's one-view palm (Tracker::single_view at the hand's scale). The @@ -126,6 +132,7 @@ int main(int argc, char **argv) { PinchParams pinch_params; GripParams grip_params; std::string use = "mono", color_left = "color_video0", color_crop = "subtract", sides = "file"; + std::string ray_out; std::string timeline, depth, poses, models = std::string(argv[0]).substr(0, std::string(argv[0]).rfind('/') + 1) + "../models/ncnn"; for (int i = 2; i < argc; ++i) { const std::string a = argv[i]; @@ -135,6 +142,7 @@ int main(int argc, char **argv) { else if (a == "--timeline" && more) timeline = argv[++i]; else if (a == "--depth" && more) depth = argv[++i]; else if (a == "--poses" && more) poses = argv[++i]; + else if (a == "--ray" && more) ray_out = argv[++i]; else if (a == "--threads" && more) threads = std::atoi(argv[++i]); else if (a == "--models" && more) models = argv[++i]; else if (a == "--cost") cost = true; @@ -287,6 +295,7 @@ int main(int argc, char **argv) { } FILE *dp = depth.empty() ? nullptr : std::fopen(depth.c_str(), "w"); FILE *pp = poses.empty() ? nullptr : std::fopen(poses.c_str(), "w"); + FILE *rp = ray_out.empty() ? nullptr : std::fopen(ray_out.c_str(), "w"); if (dp) for (auto &[name, c] : used) std::fprintf(dp, "# cam %s %.4f %.4f %.4f %.1f\n", name.c_str(), c.origin[0], c.origin[1], c.origin[2], c.fx); @@ -439,6 +448,25 @@ int main(int argc, char **argv) { eval.calibration(tracker.skeleton(), ts); label_hits.push_back({int64_t(t), int(out.size())}); } + if (rp) + for (const Hand *h : out) { + const int sd = h->right() ? 1 : 0; + const bool pdown = (pinch.side(sd).flags & FH_PINCH_DOWN) && pinch.side(sd).hand_id == uint32_t(h->id); + const bool gdown = (grip.side(sd).flags & FH_PINCH_DOWN) && grip.side(sd).hand_id == uint32_t(h->id); + bool began = false; + for (const Pinch::Event &e : pinch.events) began = began || (e.side == sd && std::string(e.what) == "begin"); + const bool fitted = h->fit.valid; + const V3 *pts = fitted ? h->fit.pts : h->smooth; + double q[4] = {0, 0, 0, 1}; + if (fitted) skel::to_quat(h->fit.frames[0], q); + std::fprintf(rp, "%llu %d %s %d %d %d %d %d", (unsigned long long)t, h->id, sd ? "R" : "L", + h->nviews >= 2, fitted, pdown, gdown, began); + for (int k = 0; k < 21; ++k) + for (int j = 0; j < 3; ++j) std::fprintf(rp, " %.5f", pts[k][j]); + std::fprintf(rp, " %.5f %.5f %.5f %.5f", q[0], q[1], q[2], q[3]); + for (int j = 0; j < 3; ++j) std::fprintf(rp, " %.4f", fitted ? h->fit.elbow[j] : 0.0); + std::fputc('\n', rp); + } next_ns = t + uint64_t((std::min(tracker.interval(), pinch.engaged() || grip.engaged() ? 1 / 30.0 : 1.0) - 0.005) * 1e9); for (const Pinch::Event &e : grip.events) { if (std::string(e.what) == "begin") grip_begin_ts[e.side] = ts; @@ -571,6 +599,7 @@ int main(int argc, char **argv) { if (tl) std::fclose(tl); if (dp) std::fclose(dp); if (pp) std::fclose(pp); + if (rp) std::fclose(rp); const double secs = nsets > 1 ? nsets / 30.0 : 0; const Stats &s = tracker.stats; diff --git a/pointer/tests/handray-test.cpp b/pointer/tests/handray-test.cpp new file mode 100644 index 0000000..5b86b66 --- /dev/null +++ b/pointer/tests/handray-test.cpp @@ -0,0 +1,273 @@ +// Unit tests for the hand ray (hands/include/fh_ray.h): the body model, the pointing gate's +// timing and tests, and the filter, with made-up hands at ft-hands' 30 Hz. +// Build and run: just test-c (or g++ -std=c++17 -I. pointer/tests/handray-test.cpp). +#include "hands/include/fh_ray.h" + +#include +#include +#include +#include +#include + +using namespace fhray; + +static int failures = 0; +#define CHECK(cond, ...) \ + do { \ + if (!(cond)) { \ + std::printf("FAIL %s:%d: ", __FILE__, __LINE__); \ + std::printf(__VA_ARGS__); \ + std::printf("\n"); \ + ++failures; \ + } \ + } while (0) + +namespace { + +const V3 kEye{0, 1.6, 0}; +const V3 kForward{0, 0, -1}; +constexpr double kFrame = 1 / 30.0; + +bool Near(double a, double b, double tol) { return std::fabs(a - b) <= tol; } + +// A right hand pointing ahead: knuckles 0.25 m below the eyes, 0.45 m ahead, palm facing away +// and down. +HandFrame Pointing(double t) { + HandFrame f; + f.t = t, f.stereo = true; + f.knuckle = {0.18, 1.35, -0.45}; + f.wrist = {0.17, 1.33, -0.36}; + f.palmNormal = Norm({0, -0.5, -1}); + f.elbow = {0.2, 1.15, -0.1}; + return f; +} + +void TestBody() { + Body b; + b.Update(0, kEye, kForward); + const V3 s = b.Shoulder(1); + CHECK(Near(s.x, 0.17, 1e-9) && Near(s.y, 1.37, 1e-9) && Near(s.z, 0.08, 1e-9), "right shoulder at (%.3f %.3f %.3f)", + s.x, s.y, s.z); + CHECK(Near(b.Shoulder(0).x, -0.17, 1e-9), "left shoulder"); + // the head turns 90 deg left at once: the torso lags at most 30 deg, then follows + b.Update(0.01, kEye, Dir(90 * kDeg, 0)); + CHECK(Near(b.TorsoYaw() / kDeg, 60, 0.5), "torso lags at most 30 deg: %.1f", b.TorsoYaw() / kDeg); + for (int i = 1; i <= 300; ++i) b.Update(0.01 + i * 0.01, kEye, Dir(90 * kDeg, 0)); + CHECK(Near(b.TorsoYaw() / kDeg, 90, 0.5), "and catches up: %.2f", b.TorsoYaw() / kDeg); + // nodding doesn't move the shoulders + Body c; + c.Update(0, kEye, kForward); + const V3 s0 = c.Shoulder(1); + c.Update(0.01, kEye, Dir(0, -40 * kDeg)); + CHECK(Len(c.Shoulder(1) - s0) < 1e-9, "a nod leaves the shoulders"); +} + +// Runs frames from `make` (frame time -> frame) for `secs`; returns the time it first showed (or +// hid, with wantShown false), or -1. +double RunUntil(HandRay &r, Body &b, double &t, double secs, bool wantShown, const std::function &make, + bool sideIsHand = true) { + const double end = t + secs; + while (t < end) { + b.Update(t, kEye, kForward); + r.Frame(b, make(t), sideIsHand); + if (r.Shown() == wantShown) return t; + t += kFrame; + } + return -1; +} + +void TestGateShowHide() { + Body b; + HandRay r(1); + double t = 0; + const double shown = RunUntil(r, b, t, 1, true, Pointing); + CHECK(shown >= 0.12 - 1e-9 && shown < 0.12 + kFrame + 1e-9, "a pointing hand shows after 120 ms: %.3f", shown); + CHECK(r.event == "show", "show event: %s", r.event.c_str()); + // its ray: from the right shoulder through the knuckles + const V3 d = r.Raw().dir; + CHECK(d.z < -0.8 && d.y < 0.05, "aimed ahead: (%.2f %.2f %.2f)", d.x, d.y, d.z); + // the hand drops to 0.35 m below the eyes: inside the keep band, stays + auto lower = [](double y) { + return [y](double tt) { + HandFrame f = Pointing(tt); + f.knuckle.y = 1.6 - y, f.wrist.y = 1.6 - y - 0.02; + return f; + }; + }; + t += kFrame; + CHECK(RunUntil(r, b, t, 1, false, lower(0.35)) < 0, "0.35 m below the eyes: still shown"); + const double from = t; + const double hid = RunUntil(r, b, t, 1, false, lower(0.45)); + CHECK(hid > 0 && hid - from >= 0.25 - 1e-9 && hid - from < 0.25 + 2 * kFrame, "0.45 m below: hidden after %.3f s", + hid - from); + CHECK(r.event.rfind("hide raised", 0) == 0, "hide reason: %s", r.event.c_str()); +} + +void TestLapPinch() { + Body b; + HandRay r(0); + double t = 0; + auto lap = [](double tt) { + HandFrame f; + f.t = tt, f.stereo = true; + f.knuckle = {-0.15, 1.0, -0.35}; // 0.6 m below the eyes + f.wrist = {-0.15, 1.0, -0.27}; + f.palmNormal = {0, 1, 0}; // palm up + return f; + }; + CHECK(RunUntil(r, b, t, 2, true, lap) < 0, "a hand in the lap never shows"); + CHECK(r.Why().find("raised") != std::string::npos && r.Why().find("palm") != std::string::npos, "why: %s", + r.Why().c_str()); +} + +void TestPalmTowardYou() { + Body b; + HandRay r(1); + double t = 0; + RunUntil(r, b, t, 1, true, Pointing); + t += kFrame; + const double from = t; + const double hid = RunUntil(r, b, t, 1, false, [](double tt) { + HandFrame f = Pointing(tt); + f.palmNormal = {0, 0, 1}; + return f; + }); + CHECK(hid > 0 && hid - from < 0.3, "palm toward you hides it (%.3f s)", hid - from); +} + +void TestTrackingAndControllerAndNear() { + Body b; + double t = 0; + { + HandRay r(1); + CHECK(RunUntil(r, b, t, 1, true, [](double tt) { + HandFrame f = Pointing(tt); + f.stereo = false; + return f; + }) < 0, + "one camera only: never shows"); + } + { + HandRay r(1); + t = 0; + RunUntil(r, b, t, 1, true, Pointing); + t += kFrame; + // a short one-camera stretch keeps it + CHECK(RunUntil(r, b, t, 0.3, false, [](double tt) { + HandFrame f = Pointing(tt); + f.stereo = false; + return f; + }) < 0, + "a one-camera stretch keeps it"); + // the side turns controller: hides at once + r.Frame(b, Pointing(t), false); + CHECK(!r.Shown() && r.event == "hide off", "controller: hidden at once (%s)", r.event.c_str()); + } + { + HandRay r(1); + t = 0; + RunUntil(r, b, t, 1, true, Pointing); + HandFrame f = Pointing(t + kFrame); + f.near = true; + r.Frame(b, f, true); + CHECK(!r.Shown() && r.event == "hide near", "near: hidden at once"); + // and doesn't show again while near + t += 2 * kFrame; + CHECK(RunUntil(r, b, t, 0.5, true, [](double tt) { + HandFrame g = Pointing(tt); + g.near = true; + return g; + }) < 0, + "no ray while near"); + } + { + HandRay r(1); + t = 0; + RunUntil(r, b, t, 1, true, Pointing); + const double last = t; + r.Tick(last + 0.2, true); + CHECK(r.Shown(), "200 ms without a frame: kept"); + r.Tick(last + 0.26, true); + CHECK(!r.Shown() && r.event == "hide lost", "260 ms without a frame: lost"); + } +} + +void TestHold() { + Body b; + HandRay r(1); + double t = 0; + RunUntil(r, b, t, 1, true, Pointing); + r.Hold(true, t); + t += kFrame; + // a pinch dragged down into the lap keeps the ray while held + auto low = [](double tt) { + HandFrame f = Pointing(tt); + f.knuckle.y = 1.0; + return f; + }; + CHECK(RunUntil(r, b, t, 1, false, low) < 0, "held: stays up"); + r.Hold(false, t); + const double from = t; + const double hid = RunUntil(r, b, t, 1, false, low); + CHECK(hid > 0 && hid - from >= 0.25 - 1e-9 && hid - from < 0.55, "released: hides after the hold (%.3f s)", + hid - from); +} + +void TestFilter() { + // 2 mm of landmark jitter on K at 0.45 m from the shoulder: about 0.25 deg of raw wobble + std::mt19937 rng(1); + std::normal_distribution n(0, 0.002); + Body b; + HandRay r(1); + double t = 0; + std::vector rawYaw, outYaw; + for (int i = 0; i < 300; ++i, t += kFrame) { + b.Update(t, kEye, kForward); + HandFrame f = Pointing(t); + f.knuckle = f.knuckle + V3{n(rng), n(rng), n(rng)}; + r.Frame(b, f, true); + if (r.Shown() && i > 60) rawYaw.push_back(YawOf(r.Raw().dir) / kDeg), outYaw.push_back(YawOf(r.Current().dir) / kDeg); + } + auto sd = [](const std::vector &v) { + double m = 0, q = 0; + for (double x : v) m += x; + m /= double(v.size()); + for (double x : v) q += (x - m) * (x - m); + return std::sqrt(q / double(v.size())); + }; + const double a = sd(rawYaw), c = sd(outYaw); + std::printf("filter: yaw sd raw %.3f deg (prediction included), filtered %.3f deg\n", a, c); + CHECK(c < 0.1, "resting wobble under 0.1 deg after the filter: %.3f", c); + CHECK(c < a / 3, "the filter takes most of it out: %.3f of %.3f", c, a); + // a deliberate 20 deg sweep in 0.3 s gets through (lag under 3 deg at the end + 0.2 s) + const double y0 = YawOf(r.Current().dir) / kDeg; + for (int i = 0; i < 15; ++i, t += kFrame) { + b.Update(t, kEye, kForward); + HandFrame f = Pointing(t); + const double k = std::min(1.0, i * kFrame / 0.3); + const V3 s = b.Shoulder(1); + const V3 d = Dir(-20 * k * kDeg + YawOf(Pointing(0).knuckle - s), PitchOf(Pointing(0).knuckle - s)); + f.knuckle = s + d * Len(Pointing(0).knuckle - s); + r.Frame(b, f, true); + } + const double moved = y0 - YawOf(r.Current().dir) / kDeg; + CHECK(moved > 17, "a 20 deg sweep: the filtered ray followed %.1f deg", moved); +} + +} // namespace + +int main() { + TestBody(); + TestGateShowHide(); + TestLapPinch(); + TestPalmTowardYou(); + TestTrackingAndControllerAndNear(); + TestHold(); + TestFilter(); + if (failures) { + std::printf("%d failures\n", failures); + return 1; + } + std::printf("handray-test: ok\n"); + return 0; +} diff --git a/pointer/tools/rayeval.cpp b/pointer/tools/rayeval.cpp new file mode 100644 index 0000000..5ef08f9 --- /dev/null +++ b/pointer/tools/rayeval.cpp @@ -0,0 +1,372 @@ +// rayeval: the hand ray (hands/include/fh_ray.h) run offline over a recording, to see how +// often it shows, why it doesn't, how much it wobbles, and whether pinches would be ray pinches +// or gaze pinches. Probe P9 of frame-hands/notes/hand-interaction.md, without the headset. +// +// rayeval RAY.txt PROBE.jsonl [--side L|R|both] [--near-panel] [--csv FILE] [--set NAME=VALUE...] +// +// RAY.txt: ft-handreplay DIR --ray RAY.txt (per set and hand: the points the helper would read). +// PROBE.jsonl: the ft-handtest probe's log from the same session (frame-hands/captures/probe-*), +// which has the head's pose at about 90 Hz on CLOCK_MONOTONIC and the panel it showed. The +// recording's capture times are CLOCK_MONOTONIC_RAW: the offset is found by matching the sets +// the live tracker published (the probe's "cap") with the recorded ones. +// --near-panel: count a hand as near (ft-screens' near claim: the index tip over the probe's +// panel, within 15 cm in front of it) as the helper would hear from ft-screens. +// --csv: per hand frame: t, side, shown, the tests' values, ray yaw and pitch (raw, filtered). +// --set: an fh_ray Params value (predict, minCutoff, beta, dCutoff, palmShow, palmHide, raisedShow, +// raisedHide, forwardShow, forwardHide, showFor, hideFor), to try settings. +// +// Build: g++ -std=c++17 -O2 -I. pointer/tools/rayeval.cpp -o build/rayeval (no dependencies). +#include "hands/include/fh_ray.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace fhray; + +namespace { + +struct M34 { + double m[3][4] = {}; +}; +V3 Pos(const M34 &h) { return {h.m[0][3], h.m[1][3], h.m[2][3]}; } +V3 Rot(const M34 &h, V3 v) { + return {h.m[0][0] * v.x + h.m[0][1] * v.y + h.m[0][2] * v.z, h.m[1][0] * v.x + h.m[1][1] * v.y + h.m[1][2] * v.z, + h.m[2][0] * v.x + h.m[2][1] * v.y + h.m[2][2] * v.z}; +} +V3 RotInv(const M34 &h, V3 v) { + return {h.m[0][0] * v.x + h.m[1][0] * v.y + h.m[2][0] * v.z, h.m[0][1] * v.x + h.m[1][1] * v.y + h.m[2][1] * v.z, + h.m[0][2] * v.x + h.m[1][2] * v.y + h.m[2][2] * v.z}; +} +V3 Cross(V3 a, V3 b) { return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; } +V3 QRot(const double q[4], V3 v) { + const V3 u{q[0], q[1], q[2]}; + const double s = q[3]; + return u * (2 * Dot(u, v)) + v * (s * s - Dot(u, u)) + Cross(u, v) * (2 * s); +} + +// The numbers after `"key":` (a number or a [list]). +bool Numbers(const std::string &line, const char *key, std::vector &out) { + out.clear(); + const std::string k = std::string("\"") + key + "\":"; + size_t at = line.find(k); + if (at == std::string::npos) return false; + at += k.size(); + const bool list = line[at] == '['; + if (list) ++at; + const char *p = line.c_str() + at; + while (true) { + char *end; + const double v = std::strtod(p, &end); + if (end == p) break; + out.push_back(v); + p = end; + if (!list || *p != ',') break; + ++p; + } + return !out.empty(); +} + +struct Frame { + uint64_t t; // raw ns + int id, side; + bool stereo, fitted, pinch, grip, began; + V3 pts[21]; + double q[4]; + V3 elbow; +}; + +double Pct(std::vector v, double p) { + if (v.empty()) return NAN; + std::sort(v.begin(), v.end()); + return v[std::min(v.size() - 1, size_t(p * double(v.size() - 1) + 0.5))]; +} + +} // namespace + +int main(int argc, char **argv) { + if (argc < 3) { + std::fprintf(stderr, "usage: %s RAY.txt PROBE.jsonl [--side L|R|both] [--near-panel] [--csv FILE]\n", argv[0]); + return 2; + } + int only = -1; + bool nearPanel = false; + const char *csvPath = nullptr; + std::vector sets; // --set name=value: fh_ray Params + for (int i = 3; i < argc; ++i) { + if (!std::strcmp(argv[i], "--side") && i + 1 < argc) { + const char *s = argv[++i]; + only = !std::strcmp(s, "L") ? 0 : !std::strcmp(s, "R") ? 1 : -1; + } else if (!std::strcmp(argv[i], "--near-panel")) nearPanel = true; + else if (!std::strcmp(argv[i], "--csv") && i + 1 < argc) csvPath = argv[++i]; + else if (!std::strcmp(argv[i], "--set") && i + 1 < argc) sets.push_back(argv[++i]); + } + Params params; + for (const std::string &kv : sets) { + const size_t eq = kv.find('='); + const std::string k = kv.substr(0, eq); + const double v = eq == std::string::npos ? 0 : std::atof(kv.c_str() + eq + 1); + double *f = k == "predict" ? ¶ms.predict : k == "minCutoff" ? ¶ms.minCutoff : k == "beta" ? ¶ms.beta + : k == "dCutoff" ? ¶ms.dCutoff : k == "palmShow" ? ¶ms.palmShow : k == "palmHide" ? ¶ms.palmHide + : k == "raisedShow" ? ¶ms.raisedShow : k == "raisedHide" ? ¶ms.raisedHide + : k == "forwardShow" ? ¶ms.forwardShow : k == "forwardHide" ? ¶ms.forwardHide + : k == "showFor" ? ¶ms.showFor : k == "hideFor" ? ¶ms.hideFor : nullptr; + if (!f) { + std::fprintf(stderr, "unknown --set %s\n", k.c_str()); + return 2; + } + *f = v; + } + // the probe: head poses by time, the published capture times, the panel + std::vector> heads; + std::vector caps; + M34 panel; + double panelW = 0, panelH = 0; + { + std::ifstream in(argv[2]); + std::string line; + std::vector v; + while (std::getline(in, line)) { + if (line.find("\"panel\"") != std::string::npos) { + const size_t at = line.find("\"panel\""); + const std::string rest = line.substr(at); + if (Numbers(rest, "pose", v) && v.size() == 12) + for (int k = 0; k < 12; ++k) panel.m[k / 4][k % 4] = v[k]; + if (Numbers(rest, "width", v)) panelW = v[0]; + if (Numbers(rest, "height", v)) panelH = v[0]; + continue; + } + std::vector t, cap, h; + if (!Numbers(line, "t", t) || !Numbers(line, "head", h) || h.size() != 12) continue; + M34 m; + for (int k = 0; k < 12; ++k) m.m[k / 4][k % 4] = h[k]; + heads.push_back({uint64_t(t[0]), m}); + if (Numbers(line, "cap", cap)) caps.push_back(uint64_t(cap[0])); + } + } + std::sort(caps.begin(), caps.end()); + caps.erase(std::unique(caps.begin(), caps.end()), caps.end()); + std::sort(heads.begin(), heads.end(), [](auto &a, auto &b) { return a.first < b.first; }); + // the replay's frames + std::vector frames; + { + std::ifstream in(argv[1]); + std::string line; + while (std::getline(in, line)) { + std::istringstream s(line); + Frame f; + std::string side; + int st, fi, pd, gd, bg; + if (!(s >> f.t >> f.id >> side >> st >> fi >> pd >> gd >> bg)) continue; + f.side = side == "R", f.stereo = st, f.fitted = fi, f.pinch = pd, f.grip = gd, f.began = bg; + for (auto &p : f.pts) s >> p.x >> p.y >> p.z; + s >> f.q[0] >> f.q[1] >> f.q[2] >> f.q[3] >> f.elbow.x >> f.elbow.y >> f.elbow.z; + if (s) frames.push_back(f); + } + } + if (heads.empty() || frames.empty() || caps.empty()) { + std::fprintf(stderr, "nothing to evaluate: %zu head poses, %zu captures, %zu hand frames\n", heads.size(), + caps.size(), frames.size()); + return 1; + } + // raw -> mono: the offset most published captures sit at + std::vector setTimes; + for (const Frame &f : frames) setTimes.push_back(f.t); + std::sort(setTimes.begin(), setTimes.end()); + setTimes.erase(std::unique(setTimes.begin(), setTimes.end()), setTimes.end()); + std::map votes; + for (size_t i = 0; i < std::min(caps.size(), 300); ++i) + for (uint64_t r : setTimes) ++votes[(int64_t(r) - int64_t(caps[i])) / 500'000]; + int64_t bin = 0; + int best = 0; + for (auto &[b, n] : votes) + if (n > best) best = n, bin = b; + // refine: the median exact difference within that bin + std::vector diffs; + for (uint64_t c : caps) { + const auto it = std::lower_bound(setTimes.begin(), setTimes.end(), uint64_t(int64_t(c) + bin * 500'000 - 1'000'000)); + for (auto j = it; j != setTimes.end() && int64_t(*j) <= int64_t(c) + bin * 500'000 + 1'500'000; ++j) + diffs.push_back(int64_t(*j) - int64_t(c)); + } + std::sort(diffs.begin(), diffs.end()); + const int64_t off = diffs.empty() ? bin * 500'000 : diffs[diffs.size() / 2]; + std::printf("clock: recording raw = probe mono + %.3f s (%d of %zu published captures matched)\n", off / 1e9, + int(diffs.size()), caps.size()); + + FILE *csv = csvPath ? std::fopen(csvPath, "w") : nullptr; + if (csv) std::fprintf(csv, "t,side,stereo,shown,below,ahead,palm,pitch,yawoff,raw_yaw,raw_pitch,yaw,pitch,near,pinch\n"); + Body body(params); + HandRay rays[2] = {HandRay(0, params), HandRay(1, params)}; + struct SideStats { + int frames = 0, shownFrames = 0, shows = 0, shortShows = 0, pinches = 0, rayPinches = 0, nearFrames = 0; + double showStart = -1, lastT = 0; + std::vector showLens; + std::map hides, fails; + std::vector wobble, wobbleRaw, pinchShift; + std::vector> recent; // filtered dir, last 0.5 s + std::vector> recentRaw; + std::vector> knuckles; + // jitter: the second difference of the ray's yaw and pitch over three frames in a row, + // over sqrt(6) (white noise of sd s gives second differences of sd s * sqrt(6)) + std::vector jitter, jitterRaw; + double py[2][2] = {}, pp[2][2] = {}, pt[2] = {-1, -1}; // [filtered, raw][frames back] + double pinchAt = -1; + V3 pinchDir; + std::vector below, ahead, palm; // when stereo and raised at most 0.5 m below + } st[2]; + const double t0 = frames.front().t / 1e9; + size_t hi = 0; + int skipped = 0; + for (const Frame &f : frames) { + if (only >= 0 && f.side != only) continue; + const int64_t mono = int64_t(f.t) - off; + while (hi + 1 < heads.size() && heads[hi + 1].first <= uint64_t(mono)) ++hi; + const size_t pick = hi + 1 < heads.size() && heads[hi + 1].first - mono < mono - heads[hi].first ? hi + 1 : hi; + if (std::llabs(int64_t(heads[pick].first) - mono) > 30'000'000) { + ++skipped; + continue; + } + const M34 &h = heads[pick].second; + const double t = mono / 1e9; + body.Update(t, Pos(h), Rot(h, V3{0, 0, -1})); + auto room = [&](V3 p) { return Pos(h) + Rot(h, p); }; + HandFrame hf; + hf.t = t, hf.stereo = f.stereo; + hf.knuckle = room((f.pts[5] + f.pts[9]) * 0.5); + hf.wrist = room(f.pts[0]); + if (f.fitted) { + hf.palmNormal = Norm(Rot(h, QRot(f.q, V3{0, -1, 0}))); + hf.elbow = room(f.elbow); + } else { + const V3 n = Cross(f.pts[5] - f.pts[0], f.pts[17] - f.pts[0]); + hf.palmNormal = Norm(Rot(h, f.side ? n : n * -1.0)); + } + hf.grip = f.grip; + if (nearPanel && panelW > 0) { + const V3 tip = room(f.pts[8]); + const V3 q = RotInv(panel, tip - Pos(panel)); + hf.near = std::fabs(q.x) <= panelW / 2 + 0.02 && std::fabs(q.y) <= panelH / 2 + 0.02 && q.z > -0.03 && q.z <= 0.15; + } + HandRay &r = rays[f.side]; + SideStats &s = st[f.side]; + // a pinch begin is decided from the state before this frame (5.2: at begin_ns) + if (f.began) { + ++s.pinches; + if (r.Shown()) ++s.rayPinches, s.pinchAt = t, s.pinchDir = r.Current().dir; + } + r.Hold(f.pinch && r.Shown(), t); + r.Frame(body, hf, true); + ++s.frames; + s.nearFrames += hf.near; + const Tests &ts = r.LastTests(); + if (f.stereo && ts.below < 0.5) s.below.push_back(ts.below), s.ahead.push_back(ts.ahead), s.palm.push_back(ts.palm); + if (!r.event.empty()) { + if (r.event == "show") ++s.shows, s.showStart = t; + else { + ++s.hides[r.event.substr(5)]; + if (s.showStart >= 0) { + s.showLens.push_back(t - s.showStart); + s.shortShows += t - s.showStart < 0.5; + } + s.showStart = -1; + } + r.event.clear(); + } + if (r.Shown()) { + ++s.shownFrames; + } else { + std::string fails = r.Why(); + ++s.fails[fails.empty() ? "(timing)" : fails]; + } + // wobble: while shown and the knuckles hold still (under 2 cm/s over the last 0.3 s), the + // spread of the ray's direction over the last 0.5 s + s.knuckles.push_back({t, hf.knuckle}); + while (!s.knuckles.empty() && t - s.knuckles.front().first > 0.3) s.knuckles.erase(s.knuckles.begin()); + const bool still = s.knuckles.size() >= 4 && + Len(s.knuckles.back().second - s.knuckles.front().second) / std::max(1e-3, t - s.knuckles.front().first) < 0.02; + if (r.Shown()) { + s.recent.push_back({t, r.Current().dir}), s.recentRaw.push_back({t, r.Raw().dir}); + } else { + s.recent.clear(), s.recentRaw.clear(); + } + while (!s.recent.empty() && t - s.recent.front().first > 0.5) s.recent.erase(s.recent.begin()); + while (!s.recentRaw.empty() && t - s.recentRaw.front().first > 0.5) s.recentRaw.erase(s.recentRaw.begin()); + auto spread = [](const std::vector> &w) { + V3 m; + for (auto &e : w) m = m + e.second; + m = Norm(m); + double q = 0; + for (auto &e : w) { + const double a = std::acos(std::clamp(Dot(e.second, m), -1.0, 1.0)) / kDeg; + q += a * a; + } + return std::sqrt(q / double(w.size())); + }; + if (r.Shown() && still && s.recent.size() >= 6) s.wobble.push_back(spread(s.recent)), s.wobbleRaw.push_back(spread(s.recentRaw)); + if (r.Shown() && still) { + const double y[2] = {YawOf(r.Current().dir) / kDeg, YawOf(r.Raw().dir) / kDeg}; + const double p[2] = {PitchOf(r.Current().dir) / kDeg, PitchOf(r.Raw().dir) / kDeg}; + if (s.pt[1] >= 0 && t - s.pt[1] < 0.2) + for (int k = 0; k < 2; ++k) { + const double dy = std::remainder(y[k] - 2 * s.py[k][0] + s.py[k][1], 360.0); + const double dp = p[k] - 2 * s.pp[k][0] + s.pp[k][1]; + (k ? s.jitterRaw : s.jitter).push_back(std::hypot(dy, dp) / std::sqrt(6.0)); + } + for (int k = 0; k < 2; ++k) s.py[k][1] = s.py[k][0], s.py[k][0] = y[k], s.pp[k][1] = s.pp[k][0], s.pp[k][0] = p[k]; + s.pt[1] = s.pt[0], s.pt[0] = t; + } else { + s.pt[0] = s.pt[1] = -1; + } + if (s.pinchAt >= 0 && t - s.pinchAt >= 0.15) { + if (r.Shown()) s.pinchShift.push_back(std::acos(std::clamp(Dot(r.Current().dir, s.pinchDir), -1.0, 1.0)) / kDeg); + s.pinchAt = -1; + } + s.lastT = t; + if (csv) + std::fprintf(csv, "%.4f,%d,%d,%d,%.3f,%.3f,%.3f,%.1f,%.1f,%.2f,%.2f,%.2f,%.2f,%d,%d\n", t - t0, f.side, f.stereo, + r.Shown(), ts.below, ts.ahead, ts.palm, ts.pitch, ts.yawOff, YawOf(r.Raw().dir) / kDeg, + PitchOf(r.Raw().dir) / kDeg, YawOf(r.Current().dir) / kDeg, PitchOf(r.Current().dir) / kDeg, + hf.near, f.pinch); + } + if (csv) std::fclose(csv); + std::printf("hand frames: %zu (%d without a head pose within 30 ms)\n", frames.size(), skipped); + for (int k = 0; k < 2; ++k) { + SideStats &s = st[k]; + if (!s.frames) continue; + if (s.showStart >= 0) s.showLens.push_back(s.lastT - s.showStart); + std::printf("\n%s hand: %d frames, ray shown in %.1f%%; %d shows (%d under 0.5 s), median %.2f s%s\n", + k ? "right" : "left", s.frames, 100.0 * s.shownFrames / s.frames, s.shows, s.shortShows, + Pct(s.showLens, 0.5), nearPanel ? "" : " (no near claims: --near-panel)"); + if (nearPanel) std::printf(" near the probe's panel in %.1f%% of frames\n", 100.0 * s.nearFrames / s.frames); + std::printf(" pinches: %d, %d of them with the ray up (ray pinches), %d gaze pinches\n", s.pinches, s.rayPinches, + s.pinches - s.rayPinches); + std::printf(" hides:"); + for (auto &[w, n] : s.hides) std::printf(" %s %d,", w.c_str(), n); + std::printf("\n not shown because (frames):"); + std::vector> f; + for (auto &[w, n] : s.fails) f.push_back({n, w}); + std::sort(f.rbegin(), f.rend()); + for (size_t i = 0; i < std::min(f.size(), 8); ++i) std::printf(" %s %d,", f[i].second.c_str(), f[i].first); + std::printf("\n hands up (stereo, under 0.5 m below the eyes): below p10/50/90 %.2f/%.2f/%.2f m, ahead " + "%.2f/%.2f/%.2f m, palm.D %.2f/%.2f/%.2f\n", + Pct(s.below, 0.1), Pct(s.below, 0.5), Pct(s.below, 0.9), Pct(s.ahead, 0.1), Pct(s.ahead, 0.5), + Pct(s.ahead, 0.9), Pct(s.palm, 0.1), Pct(s.palm, 0.5), Pct(s.palm, 0.9)); + std::printf(" wobble, shown and the hand still (spread over 0.5 s): filtered p50 %.3f p90 %.3f deg, raw p50 %.3f " + "p90 %.3f deg (%zu windows)\n", + Pct(s.wobble, 0.5), Pct(s.wobble, 0.9), Pct(s.wobbleRaw, 0.5), Pct(s.wobbleRaw, 0.9), s.wobble.size()); + std::printf(" jitter, shown and the hand still (second differences): filtered p50 %.3f p90 %.3f deg, raw p50 " + "%.3f p90 %.3f deg (%zu)\n", + Pct(s.jitter, 0.5), Pct(s.jitter, 0.9), Pct(s.jitterRaw, 0.5), Pct(s.jitterRaw, 0.9), s.jitter.size()); + std::printf(" ray shift 150 ms into a ray pinch: p50 %.2f p90 %.2f deg (%zu)\n", Pct(s.pinchShift, 0.5), + Pct(s.pinchShift, 0.9), s.pinchShift.size()); + } + return 0; +}