steam-frame/DeeJanuz--frametop
1
0 Fork 0
mirror of https://github.com/DeeJanuz/frametop.git synced 2026-10-06 00:00:07 +02:00
Code Issues Packages Projects Releases 3 Wiki Activity
Files
0ac2b10cd75f68665eb3e6ee681cd21ce006a063
DeeJanuz--frametop/hands/track/main.cpp
T
DeeJanuzandClaude Opus 5.5 0ac2b10cd7 Remove leftovers before a release 
- gaze/tracker/lab/eyes_track.py: nothing used it
- Input Settings: the pointer role backend, whose page was removed
- ft-screens: no log line for every floating-window resize
- hands: ft-hands --help gives the palm-down default (1: off), the
  uninstall removes ft-handsctl's link, .frame-job is ignored
- Display Settings: "Save as profile…", not "Save current arrangement"
- two stale comments (ft-pointer's grabprobe, ft-gaze)

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-10-01 14:47:53 -06:00

627 lines
34 KiB
C++
Raw Blame History

// ft-hands: hands in 3D from ft-camd's ring, published for Frametop's ft-screens (the hand
// cutouts), and pinches and grips for the pointer. It started as a port of frame-hands'
// Python prototype: the same scheduling, with the models on a few threads.
//
// ft-hands [--seconds N] [--threads N] [--int8] [--status S] [--models DIR] [--nice N]
// [--no-publish] [--record DIR] [--swap-sides] [--cams auto|mono|color|all] ...
// (--help lists them all)
//
// Which cameras (--cams, HANDS_CAMERAS): the four mono IR cameras light the hands with their
// own IR and track well in dim rooms, but in bright light (a sunny room, a window behind the
// hands) they expose for the room and the hands come out dark. The two Arcturus colour
// cameras (the passthrough pair, forward-facing, 145 degrees) are the other way round: dark
// and grainy in a dim room, clear in a lit one. auto (the default) picks by how bright the
// colour cameras' frames are: at HANDS_BRIGHT_ON (mean luma) or over for 2 s, it tracks with
// HANDS_BRIGHT (all: every camera, so hands low at the sides stay in the side cameras; or
// color); under HANDS_BRIGHT_OFF for 2 s, with the mono cameras again. ft-camd runs the colour
// cameras at 2 fps, enough to tell the light, until ft-hands asks for 30
// (/run/user/UID/frametop-hands/color-fps). The colour frames' capture times are on their
// own clock, so they're placed on the mono cameras' by when they were dequeued, less the
// mono cameras' measured delay.
//
// Settings in ~/.config/frametop.conf (FT_<name> in the environment overrides them, and
// options override both): HANDS_SWAP_SIDES (1: as --swap-sides), HANDS_CPUS (as --cpus),
// HANDS_CAMERAS, HANDS_BRIGHT, HANDS_BRIGHT_ON, HANDS_BRIGHT_OFF, HANDS_COLOR_LEFT (which
// colour camera is passthrough_left: color_video0 or color_video3), HANDS_COLOR_CROP
// (subtract or none: tools/check_color.py tells both).
#include "io.h"
#include "pinch.h"
#include "record.h"
#include <sched.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <unistd.h>
#include <cmath>
#include <ctime>
#include <memory>
#include <csignal>
#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <thread>
#include <utility>
namespace {
volatile std::sig_atomic_t g_stop = 0, g_record = 0;
// which calibrated camera each capture pipe carries (XRService's fixed routing)
const char *camera_for_pipe(int node) {
char path[64], name[64] = "";
std::snprintf(path, sizeof path, "/sys/class/video4linux/video%d/name", node);
std::ifstream f(path);
f.getline(name, sizeof name);
if (!std::strcmp(name, "msm_vfe3_video0")) return "slam_left";
if (!std::strcmp(name, "msm_vfe4_video0")) return "slam_right";
if (!std::strcmp(name, "msm_vfe2_video0")) return "upper_left";
if (!std::strcmp(name, "msm_vfe2_video1")) return "upper_right";
return nullptr;
}
// A setting from ~/.config/frametop.conf, or FT_<key> from the environment; "" if unset.
std::string setting(const std::string &key) {
if (const char *v = std::getenv(("FT_" + key).c_str())) return v;
const char *home = std::getenv("HOME");
std::ifstream in(std::string(home ? home : "") + "/.config/frametop.conf");
std::string line, value;
auto trim = [](std::string s) {
s.erase(0, s.find_first_not_of(" \t\"'"));
s.erase(s.find_last_not_of(" \t\"'") + 1);
return s;
};
while (std::getline(in, line)) {
line = line.substr(0, line.find('#'));
const auto eq = line.find('=');
if (eq != std::string::npos && trim(line.substr(0, eq)) == key) value = trim(line.substr(eq + 1));
}
return value;
}
std::vector<int> parse_cpus(const char *p) {
std::vector<int> out;
while (*p) {
char *end;
const long c = std::strtol(p, &end, 10);
if (end == p) break;
out.push_back(int(c));
p = *end == ',' ? end + 1 : end;
}
return out;
}
// Where SIGUSR1 puts recordings: $XDG_DATA_HOME/frametop/hands (~/.local/share/...).
std::string recordings_dir() {
const char *data = std::getenv("XDG_DATA_HOME"), *home = std::getenv("HOME");
std::string dir = data && *data ? data : std::string(home ? home : "") + "/.local/share";
for (const char *part : {"/frametop", "/hands"}) mkdir((dir += part).c_str(), 0700);
return dir;
}
double cpu_seconds() {
rusage r;
getrusage(RUSAGE_SELF, &r);
return r.ru_utime.tv_sec + r.ru_stime.tv_sec + (r.ru_utime.tv_usec + r.ru_stime.tv_usec) / 1e6;
}
enum class Cams { Mono, Color, All };
const char *cams_name(Cams c) { return c == Cams::Mono ? "mono" : c == Cams::Color ? "color" : "all"; }
bool parse_cams(const std::string &s, Cams &out) {
if (s == "mono") out = Cams::Mono;
else if (s == "color") out = Cams::Color;
else if (s == "all") out = Cams::All;
else return false;
return true;
}
// How bright it is, for auto (see the top): the colour frames' mean luma, smoothed over about
// a second, with hysteresis and a 2 s hold each way. No colour frames for 3 s (ft-camd paused
// them, or has none) reads as dim.
struct Lighting {
double on = 40, off = 25;
double level = -1;
bool bright = false;
uint64_t at_ns = 0, since_ns = 0; // the last frame; since when it's wanted the other way
void add(double mean, uint64_t t_ns) {
const double dt = at_ns && t_ns > at_ns ? (t_ns - at_ns) / 1e9 : 1.0;
level = level < 0 ? mean : level + (mean - level) * std::min(1.0, dt / 1.0);
at_ns = t_ns;
}
// True when it switched.
bool update(uint64_t now_ns) {
if (level >= 0 && now_ns - at_ns > 3'000'000'000ull) level = -1;
const bool want = level >= 0 && (bright ? level > off : level >= on);
if (want == bright) return since_ns = 0, false;
if (!since_ns) since_ns = now_ns;
if (now_ns - since_ns < 2'000'000'000ull) return false;
bright = want, since_ns = 0;
return true;
}
};
} // namespace
int main(int argc, char **argv) {
double seconds = 0, status = 5;
int threads = 3, niceness = 5;
bool int8 = false, publish = true, track = true, swap_sides = false;
std::string models = std::string(argv[0]).substr(0, std::string(argv[0]).rfind('/') + 1) + "../models/ncnn";
std::string record, ring_path = "/run/user/" + std::to_string(getuid()) + "/" FH_RING_NAME;
// SteamOS starts user processes on CPUs 0-4 and keeps 5-7 (two A720s and the X4) for
// SteamVR's compositor, whose threads there run at real-time priority, so they always
// win. XRService pins its head tracking to 2-3. frame-hands' probes/core_ab.py
// (2026-09-29, headset on, 3 rounds): on 5-7 a step took 8.4 ms against 13.2 on 2-4,
// latency 9.6 against 14.1 ms, and the compositor's late frames and CPU/GPU time didn't change.
std::vector<int> cpus = {5, 6, 7};
if (const auto c = parse_cpus(setting("HANDS_CPUS").c_str()); !c.empty()) cpus = c;
swap_sides = setting("HANDS_SWAP_SIDES") == "1";
// Which cameras (see the top).
std::string cams_arg = setting("HANDS_CAMERAS"), bright_arg = setting("HANDS_BRIGHT");
std::string color_left = setting("HANDS_COLOR_LEFT"), color_crop = setting("HANDS_COLOR_CROP");
if (cams_arg.empty()) cams_arg = "auto";
if (bright_arg.empty()) bright_arg = "all";
if (color_left.empty()) color_left = "color_video0";
if (color_crop.empty()) color_crop = "subtract";
Lighting light;
if (const std::string v = setting("HANDS_BRIGHT_ON"); !v.empty()) light.on = std::atof(v.c_str());
if (const std::string v = setting("HANDS_BRIGHT_OFF"); !v.empty()) light.off = std::atof(v.c_str());
// How crops are equalized. CLAHE helps the palm search find hands (about 10% more in the
// dim recording), but makes the landmarks jitter, so they get plain crops.
Contrast palm_contrast, hand_contrast{Contrast::None};
double keep_presence = 0.5; // landmark presence a tracked view needs to stay
PinchParams pinch_params;
GripParams grip_params;
bool gesture_log = false; // what the pinch and grip detectors measure, 10 times a second
double record_for = 120;
for (int i = 1; i < argc; ++i) {
const std::string a = argv[i];
const bool more = i + 1 < argc;
if (a == "--seconds" && more) seconds = std::atof(argv[++i]);
else if (a == "--threads" && more) threads = std::max(1, std::atoi(argv[++i]));
else if (a == "--status" && more) status = std::atof(argv[++i]);
else if (a == "--models" && more) models = argv[++i];
else if (a == "--nice" && more) niceness = std::atoi(argv[++i]);
else if (a == "--int8") int8 = true;
else if (a == "--no-publish") publish = false;
else if (a == "--pinch-begin" && more) pinch_params.begin_m = std::atof(argv[++i]);
else if (a == "--pinch-end" && more) pinch_params.end_m = std::atof(argv[++i]);
else if (a == "--pinch-triangulated") pinch_params.triangulated = true;
else if (a == "--pinch-palm-down" && more) pinch_params.palm_down_max = std::atof(argv[++i]);
else if (a == "--grip-begin" && more) grip_params.begin = std::atof(argv[++i]);
else if (a == "--grip-end" && more) grip_params.end = std::atof(argv[++i]);
else if (a == "--gesture-log") gesture_log = true;
else if (a == "--swap-sides") swap_sides = true;
else if (a == "--record-only") track = publish = false;
else if (a == "--ring" && more) ring_path = argv[++i];
else if (a == "--record" && more) record = argv[++i];
else if (a == "--record-for" && more) record_for = std::atof(argv[++i]);
else if (a == "--keep-presence" && more) keep_presence = std::atof(argv[++i]);
else if (a == "--cams" && more) cams_arg = argv[++i];
else if (a == "--bright" && more) bright_arg = argv[++i];
else if (a == "--bright-on" && more) light.on = std::atof(argv[++i]);
else if (a == "--bright-off" && more) light.off = std::atof(argv[++i]);
else if (a == "--color-left" && more) color_left = argv[++i];
else if (a == "--color-crop" && more) color_crop = argv[++i];
else if (a == "--contrast" && more) {
if (!Contrast::parse_pair(argv[++i], palm_contrast, hand_contrast))
return std::fprintf(stderr, "--contrast MODE or PALM/HAND, each clahe[:CLIP]|none|stretch\n"), 1;
} else if (a == "--cpus" && more) {
cpus = parse_cpus(argv[++i]);
if (cpus.empty()) cpus = {5, 6, 7};
}
else {
std::printf("usage: %s [--seconds N] [--threads N] [--int8] [--status S] [--models DIR] [--nice N] [--no-publish]\n"
" [--record DIR] [--record-for S] [--record-only] [--cpus 5,6,7] [--swap-sides]\n"
" [--keep-presence P] (0.5) [--ring PATH] (ft-camd's, or ft-ringplay's)\n"
" [--cams auto|mono|color|all] (auto) [--bright all|color] (all) [--bright-on L] (40) [--bright-off L] (25)\n"
" [--color-left color_video0|color_video3] [--color-crop subtract|none]\n"
" [--pinch-begin M] (0.020) [--pinch-end M] (0.035) [--pinch-triangulated] [--pinch-palm-down MAX] (1: off)\n"
" [--grip-begin R] (1.2) [--grip-end R] (1.45) [--gesture-log]\n"
" [--contrast MODE|PALM/HAND] (clahe[:CLIP], none, stretch; default clahe:2/none)\n"
"Recording saves every frame set for S seconds (120) to DIR/sets.bin, for ft-handreplay; SIGUSR1\n"
"starts one in ~/.local/share/frametop/hands/rec-<time>. --record-only records without tracking, so it\n"
"can run beside a tracking ft-hands. With ft-camd --with-dark, recordings also get each\n"
"camera's newest dark frame, as <name>_dk; with --with-color, the color cameras' as color_video<N>.\n"
"auto picks the cameras by the light (see the top of track/main.cpp).\n"
"Settings in ~/.config/frametop.conf: HANDS_SWAP_SIDES=1, HANDS_CPUS=5,6,7, HANDS_CAMERAS, HANDS_BRIGHT,\n"
"HANDS_BRIGHT_ON, HANDS_BRIGHT_OFF, HANDS_COLOR_LEFT, HANDS_COLOR_CROP (FT_<name> overrides).\n",
argv[0]);
return a == "--help" ? 0 : 1;
}
}
const bool automatic = cams_arg == "auto";
Cams fixed = Cams::Mono, bright_cams = Cams::All;
if ((!automatic && !parse_cams(cams_arg, fixed)) || !parse_cams(bright_arg, bright_cams) || bright_cams == Cams::Mono)
return std::fprintf(stderr, "--cams auto|mono|color|all, --bright all|color\n"), 1;
if (color_crop != "subtract" && color_crop != "none") return std::fprintf(stderr, "--color-crop subtract|none\n"), 1;
std::setvbuf(stdout, nullptr, _IOLBF, 0); // whole lines to the journal as they come
if (nice(niceness) < 0) std::perror("nice"); // the VR stack wins contested CPUs
std::signal(SIGINT, [](int) { g_stop = 1; });
std::signal(SIGTERM, [](int) { g_stop = 1; });
std::signal(SIGUSR1, [](int) { g_record = 1; });
std::string err;
std::map<std::string, Camera> calib;
Ring ring;
Nets nets;
Publisher pub;
GesturePublisher gestures;
Pinch pinch(pinch_params);
Grip grip(grip_params);
std::unique_ptr<Recorder> rec;
uint64_t rec_start = 0;
auto start_recording = [&](const std::string &dir, std::string &e) {
rec = std::make_unique<Recorder>();
if (!rec->open(dir, e)) return rec.reset(), false;
rec_start = mono_ns();
std::printf("recording to %s for %.0f s\n", dir.c_str(), record_for);
std::fflush(stdout);
return true;
};
if (!load_calibration(calib, err) || !ring.open(ring_path.c_str(), err) || !nets.load(models, int8, err) ||
(publish && (!pub.open(err) || !gestures.open(pinch, grip, err))) ||
(!record.empty() && !start_recording(record, err))) {
std::fprintf(stderr, "%s\n", err.c_str());
return 1;
}
if (!ring.alive()) return std::fprintf(stderr, "ft-camd isn't running (no heartbeat)\n"), 1;
std::map<std::string, int> index; // mono calibration name -> ring camera
std::map<std::string, int> color; // colour calibration name (color_video<N>) -> ring camera
// Recorded as they are with each set: "<name>_dk" (ft-camd --with-dark) and "color_video<N>"
// (--with-color). Recorded names hold 15 characters, so "upper_right_dark" wouldn't fit.
std::map<std::string, int> dark;
std::map<std::string, Camera> used;
for (int i = 0; i < ring.cameras(); ++i) {
if (ring.camera(i).flags & FH_CAM_COLOR) {
const std::string name = "color_video" + std::to_string(ring.camera(i).node);
dark[name] = i, color[name] = i;
continue;
}
// ft-camd's cameras by capture pipe; ft-ringplay's (no device) by the name it gives
const char *name = camera_for_pipe(ring.camera(i).node);
if (!name && ring.camera(i).node < 0) name = ring.camera(i).name;
if (!name || !calib.count(name)) continue;
if (ring.camera(i).flags & FH_CAM_DARK) dark[std::string(name) + "_dk"] = i;
else index[name] = i, used[name] = calib[name];
}
// ft-camd tells the side cameras' buffers apart by XRService's allocation order, which
// some XRService restarts reverse; tools/check_sides.py --ring tells when.
if (swap_sides && index.count("slam_left") && index.count("slam_right")) {
std::swap(index["slam_left"], index["slam_right"]);
if (dark.count("slam_left_dk") && dark.count("slam_right_dk")) std::swap(dark["slam_left_dk"], dark["slam_right_dk"]);
std::printf("side cameras swapped (--swap-sides)\n");
}
// The colour pair, calibrated (see the top), unless only the mono cameras are wanted.
if (color.size() == 2 && (automatic || fixed != Cams::Mono)) {
const std::string left = color.count(color_left) ? color_left : color.begin()->first;
const std::string right = color.begin()->first == left ? std::next(color.begin())->first : color.begin()->first;
const int scale = int(std::lround(1972.0 / ring.camera(color[left]).width));
std::map<std::string, Camera> cc;
std::string e;
if (load_color_calibration(cc, left, right, color_crop == "subtract", scale, e)) {
for (auto &[name, cam] : cc) used[name] = cam;
std::printf("colour cameras: %s is passthrough_left, crop %s, 1/%d size\n", left.c_str(), color_crop.c_str(), scale);
} else {
std::fprintf(stderr, "colour cameras left out: %s\n", e.c_str());
color.clear();
}
} else {
color.clear();
}
if (color.empty() && (automatic || fixed != Cams::Mono)) {
if (!automatic) std::printf("no colour cameras (ft-camd --with-color): tracking with the mono ones\n");
fixed = Cams::Mono;
}
const bool switching = automatic && !color.empty();
Cams mode = switching || color.empty() ? Cams::Mono : fixed;
std::printf("cameras:");
for (auto &[name, i] : index) std::printf(" %s=video%d", name.c_str(), ring.camera(i).node);
for (auto &[name, i] : color) std::printf(" %s", name.c_str());
std::printf(" tracking with %s%s models: %s%s, %d threads on CPUs", cams_name(mode),
switching ? " (auto: by the light)" : "", models.c_str(), int8 ? " (int8)" : "", threads);
for (int c : cpus) std::printf(" %d", c);
std::printf("\n");
cpu_set_t set; // the main loop too
CPU_ZERO(&set);
for (int c : cpus) CPU_SET(c, &set);
if (sched_setaffinity(0, sizeof set, &set) < 0) std::perror("sched_setaffinity");
nets.set_contrast(palm_contrast, hand_contrast);
Pool pool(threads, cpus);
Tracker tracker(used, nets, pool);
tracker.set_keep_presence(keep_presence);
std::map<std::string, std::vector<uint8_t>> pixels;
std::map<std::string, uint64_t> last; // per camera: the frame last used
std::map<std::string, uint64_t> lit_seen; // per colour camera: the frame last counted for the light
const uint64_t start = mono_ns();
uint64_t t_status = start, next_ns = 0, t_want = 0, t_glog = 0;
double cpu0 = cpu_seconds();
std::vector<double> lat;
double hands_sum = 0, resid_sum = 0;
int resid_n = 0, left_sets = 0, right_sets = 0, both_sets = 0, color_steps = 0;
// The mono cameras' delay from capture to dequeue (their capture clock is CLOCK_MONOTONIC_RAW),
// to place the colour frames, whose capture clock is their own (see the top).
double mono_delay_ns = -1;
const std::string want_file = run_dir() + "/color-fps";
// The colour pair's newest frames if both are newer than last used and taken together
// (their own clock): their time, on the mono cameras' capture clock, else 0.
auto color_pair = [&](int64_t raw_off, bool copy, std::map<std::string, Image> &images) -> uint64_t {
fh_ring_slot_t meta[2];
std::string names[2];
uint64_t n[2];
int k = 0;
for (auto &[name, i] : color) {
names[k] = name, n[k] = ring.latest(i);
if (n[k] <= last[name] || !ring.meta(i, n[k], &meta[k])) return 0;
++k;
}
if (k != 2 || mono_delay_ns < 0) return 0;
const int64_t apart = int64_t(meta[0].capture_ns) - int64_t(meta[1].capture_ns);
if (std::llabs(apart) > 3'000'000) return 0; // one is a frame ahead: wait for the other
const uint64_t dq = std::min(meta[0].dqbuf_ns, meta[1].dqbuf_ns);
const uint64_t t = uint64_t(int64_t(dq) - int64_t(mono_delay_ns) + raw_off);
if (!copy) return t;
for (int j = 0; j < 2; ++j) {
const int i = color[names[j]];
if (!ring.read(i, n[j], pixels[names[j]], &meta[j])) return 0;
const auto &c = ring.camera(i);
images[names[j]] = {pixels[names[j]].data(), int(c.width), int(c.height), int(c.width)};
}
for (int j = 0; j < 2; ++j) last[names[j]] = n[j];
return t;
};
auto switch_to = [&](Cams to, const char *why) {
if (to == mode) return;
for (auto &[name, cam] : used) {
const bool is_color = color.count(name) > 0;
const bool keep = to == Cams::All || (to == Cams::Color) == is_color;
if (!keep) tracker.drop_camera(name);
}
std::printf("cameras: %s -> %s (%s)\n", cams_name(mode), cams_name(to), why);
mode = to;
};
auto ambient = [&] { // the mono cameras' dark frames: the room's IR light
double sum = 0;
int n = 0;
for (auto &[name, i] : index)
if (ring.camera(i).dark_mean > 0) sum += ring.camera(i).dark_mean, ++n;
return n ? sum / n : -1;
};
while (!g_stop && (seconds <= 0 || (mono_ns() - start) / 1e9 < seconds)) {
if (!ring.alive()) return std::fprintf(stderr, "ft-camd stopped\n"), 2;
const uint64_t now0 = mono_ns();
const int64_t raw_off = raw_minus_mono_ns();
// The light, from the colour frames' brightness (their slot headers only).
for (auto &[name, i] : color) {
fh_ring_slot_t m;
const uint64_t n = ring.latest(i);
if (n && n != lit_seen[name] && ring.meta(i, n, &m)) light.add(m.mean, m.dqbuf_ns), lit_seen[name] = n;
}
if (switching && light.update(now0)) {
char why[96];
std::snprintf(why, sizeof why, "colour frames at %.0f, ambient IR %.1f", light.level, ambient());
switch_to(light.bright ? bright_cams : Cams::Mono, why);
}
// Ask ft-camd for the colour cameras' full rate while tracking or recording with them.
const bool want_color = !color.empty() && (mode != Cams::Mono || rec || g_record);
if (!color.empty() && now0 - t_want > 1'000'000'000) {
t_want = now0;
if (want_color) {
if (FILE *f = std::fopen(want_file.c_str(), "w")) std::fputs("30\n", f), std::fclose(f);
} else {
unlink(want_file.c_str());
}
}
const bool use_mono = mode != Cams::Color, use_color = mode != Cams::Mono;
const bool mono_driven = use_mono || rec || g_record || !track;
std::map<std::string, Image> images;
uint64_t tmin = UINT64_MAX, dq = 0;
if (mono_driven) {
// a new frame set: every mono camera has a newer frame, taken at the same moment
std::map<std::string, uint64_t> latest;
bool ready = true;
for (auto &[name, i] : index) {
latest[name] = ring.latest(i);
ready = ready && latest[name] > last[name];
}
if (!ready) {
std::this_thread::sleep_for(std::chrono::milliseconds(2));
continue;
}
// not needed at the current rate, and not recorded: skip it without copying images
if (track && !rec && !g_record) {
uint64_t t0 = UINT64_MAX, t1 = 0;
bool ok = true;
for (auto &[name, i] : index) {
fh_ring_slot_t meta;
ok = ok && ring.meta(i, latest[name], &meta);
if (ok) t0 = std::min(t0, meta.capture_ns), t1 = std::max(t1, meta.capture_ns);
}
if (ok && t1 - t0 <= 3'000'000 && t0 < next_ns) {
for (auto &[name, i] : index) last[name] = latest[name];
continue;
}
}
std::vector<SetFrame> frames;
uint64_t tmax = 0;
bool ok = true;
for (auto &[name, i] : index) {
fh_ring_slot_t meta;
ok = ok && ring.read(i, latest[name], pixels[name], &meta);
if (!ok) break;
const auto &c = ring.camera(i);
images[name] = {pixels[name].data(), int(c.width), int(c.height), int(c.width)};
frames.push_back({name, pixels[name].data(), c.width, c.height, meta.capture_ns, meta.dqbuf_ns});
tmin = std::min(tmin, meta.capture_ns), tmax = std::max(tmax, meta.capture_ns), dq = std::max(dq, meta.dqbuf_ns);
const double delay = double(int64_t(meta.dqbuf_ns) - (int64_t(meta.capture_ns) - raw_off));
mono_delay_ns = mono_delay_ns < 0 ? delay : mono_delay_ns + 0.02 * (delay - mono_delay_ns);
}
if (!ok || tmax - tmin > 3'000'000) { // torn, or a camera is a frame behind
std::this_thread::sleep_for(std::chrono::milliseconds(1));
continue;
}
for (auto &[name, i] : index) last[name] = latest[name];
if (g_record && !rec) {
g_record = 0;
char name[64];
const std::time_t now = std::time(nullptr);
std::strftime(name, sizeof name, "rec-%Y%m%d-%H%M%S", std::localtime(&now));
const std::string dir = recordings_dir();
std::string e;
if (!start_recording(dir + "/" + name, e)) std::fprintf(stderr, "%s\n", e.c_str());
}
if (rec) { // about 80 MB/s; dark frames double that, color frames add 70 MB/s
if ((mono_ns() - rec_start) / 1e9 < record_for) {
for (auto &[name, i] : dark) { // the newest dark and color frames, as they are
fh_ring_slot_t meta;
const uint64_t n = ring.latest(i);
const auto &c = ring.camera(i);
if (n && ring.read(i, n, pixels[name + "#rec"], &meta))
frames.push_back({name, pixels[name + "#rec"].data(), c.width, c.height, meta.capture_ns,
meta.dqbuf_ns});
}
rec->add(frames);
} else {
const size_t n = rec->written(), d = rec->dropped();
rec.reset(); // writes out what's queued
std::printf("recording done: %zu sets, %zu dropped\n", n, d);
std::fflush(stdout);
if (!track) break;
}
}
if (!track && rec && status > 0 && (mono_ns() - t_status) / 1e9 >= status) {
std::printf("%5.1fs recorded %zu sets, dropped %zu\n", (mono_ns() - start) / 1e9, rec->written(), rec->dropped());
std::fflush(stdout);
t_status = mono_ns();
}
if (!track || tmin < next_ns) continue; // not needed yet at the current rate
if (!use_mono) images.clear(); // colour only, driven by mono while recording
if (use_color && color_pair(raw_off, true, images)) ++color_steps;
if (images.empty()) continue;
} else {
// colour only: a new pair of colour frames
for (auto &[name, i] : index) { // keep the mono cameras' delay current
fh_ring_slot_t meta;
const uint64_t n = ring.latest(i);
if (n && ring.meta(i, n, &meta)) {
const double delay = double(int64_t(meta.dqbuf_ns) - (int64_t(meta.capture_ns) - raw_off));
mono_delay_ns = mono_delay_ns < 0 ? delay : mono_delay_ns + 0.02 * (delay - mono_delay_ns);
}
break;
}
const uint64_t t = color_pair(raw_off, false, images);
if (!t) {
std::this_thread::sleep_for(std::chrono::milliseconds(2));
continue;
}
if (t < next_ns) { // not needed yet: pass it by without copying
for (auto &[name, i] : color) last[name] = ring.latest(i);
continue;
}
if (!color_pair(raw_off, true, images)) continue;
tmin = t, ++color_steps;
for (auto &[name, i] : color) {
fh_ring_slot_t meta;
if (ring.meta(i, last[name], &meta)) dq = std::max(dq, meta.dqbuf_ns);
}
}
const auto hands = tracker.step(images, int64_t(tmin));
const uint64_t capture = uint64_t(int64_t(tmin) - raw_off); // CLOCK_MONOTONIC
const std::vector<Seen> views = tracker.views_now();
grip.update(hands, views, int64_t(capture));
pinch.update(hands, views, int64_t(capture), grip.gripping());
if (gesture_log && capture - t_glog >= 100'000'000) {
t_glog = capture;
for (int k = 0; k < 2; ++k)
if (pinch.world_d[k] >= 0)
std::printf("gesture %s d world %.3f tri %.3f palm-down %.2f curl %.2f%s\n", k ? "right" : "left ",
pinch.world_d[k], pinch.tri_d[k], pinch.palm_down[k], grip.curl[k],
pinch.side(k).flags & FH_PINCH_DOWN ? " PINCH" : grip.side(k).flags & FH_PINCH_DOWN ? " GRIP" : "");
}
// a gesture down or closing gets the full rate, even while the palm holds still
next_ns = tmin + uint64_t((std::min(tracker.interval(), pinch.engaged() || grip.engaged() ? 1 / 30.0 : 1.0) -
0.005) * 1e9);
if (publish) pub.write(hands, capture), gestures.write(pinch, grip, capture);
for (const Pinch::Event &e : grip.events)
std::printf("grip %s %-5s curl %.2f at %+.3f %+.3f %+.3f\n", e.side ? "right" : "left ", e.what, e.distance,
e.point[0], e.point[1], e.point[2]);
for (const Pinch::Event &e : pinch.events)
std::printf("pinch %s %-5s d %.3f m at %+.3f %+.3f %+.3f\n", e.side ? "right" : "left ", e.what, e.distance,
e.point[0], e.point[1], e.point[2]);
lat.push_back((mono_ns() - dq) / 1e6);
hands_sum += double(hands.size());
bool on_left = false, on_right = false; // by where the wrist is, not the model's label
for (const Hand *h : hands) {
if (h->residual >= 0) resid_sum += h->residual * 1000, ++resid_n;
(h->pts[0][0] < 0 ? on_left : on_right) = true;
}
left_sets += on_left, right_sets += on_right, both_sets += on_left && on_right;
const uint64_t now = mono_ns();
if (status > 0 && (now - t_status) / 1e9 >= status) {
const double dt = (now - t_status) / 1e9, cpu1 = cpu_seconds();
const Stats &s = tracker.stats;
std::sort(lat.begin(), lat.end());
std::printf("%5.1fs %4.1f sets/s hands %.2f views %zu palm %3d calls %4.1f ms/batch hand %3d calls %4.1f ms/batch "
"step %4.1f ms latency %4.1f ms resid %.1f mm CPU %3.0f%%\n",
(now - start) / 1e9, s.sets / dt, s.sets ? hands_sum / s.sets : 0, tracker.views(), s.palm_calls,
s.palm_batches ? s.palm_ms / s.palm_batches : 0, s.hand_calls,
s.hand_batches ? s.hand_ms / s.hand_batches : 0, s.sets ? s.step_ms / s.sets : 0,
lat.empty() ? 0 : lat[lat.size() / 2], resid_n ? resid_sum / resid_n : 0, 100 * (cpu1 - cpu0) / dt);
if (!color.empty())
std::printf(" cameras %s%s: colour frames at %.1f (bright at %.0f, dim under %.0f), ambient IR %.1f, "
"%d steps with colour, colour placed %.1f ms after capture\n",
cams_name(mode), switching ? " (auto)" : "", light.level, light.on, light.off, ambient(),
color_steps, mono_delay_ns / 1e6);
if (s.sets)
std::printf(" sets with a hand: left %2.0f%% right %2.0f%% both %2.0f%% views lost %d, handoff misses %d, "
"dups %d, splits %d hands new %d merged %d forgotten %d%s\n",
100.0 * left_sets / s.sets, 100.0 * right_sets / s.sets, 100.0 * both_sets / s.sets, s.lost,
s.handoff_miss, s.dups, s.splits, s.created, s.merged, s.forgotten,
!rec ? "" : (" recorded " + std::to_string(rec->written()) + " dropped " +
std::to_string(rec->dropped())).c_str());
std::printf(" pinches: left %u right %u (held back, palm down: %d %d) grips: left %u right %u",
pinch.side(0).begins, pinch.side(1).begins, pinch.held_back[0], pinch.held_back[1],
grip.side(0).begins, grip.side(1).begins);
for (int k = 0; k < 2; ++k)
if (pinch.side(k).flags & FH_PINCH_TRACKED)
std::printf(" %s %s d %.3f curl %.2f", k ? "right" : "left",
grip.side(k).flags & FH_PINCH_DOWN ? "GRIP"
: pinch.side(k).flags & FH_PINCH_DOWN ? "PINCH"
: "open",
pinch.side(k).distance, grip.curl[k]);
std::printf("\n");
for (const Hand *h : hands)
std::printf(" hand %d %-5s views %d wrist %+.3f %+.3f %+.3f m scale %.2f speed %.2f m/s\n", h->id,
h->right() ? "right" : "left", h->nviews, h->pts[0][0], h->pts[0][1], h->pts[0][2], h->scale,
h->speed);
std::fflush(stdout);
tracker.stats = Stats{};
t_status = now, cpu0 = cpu1;
lat.clear(), hands_sum = 0, resid_sum = 0, resid_n = 0, left_sets = right_sets = both_sets = 0;
color_steps = 0;
}
}
if (!color.empty()) unlink(want_file.c_str());
if (publish) {
pub.write({}, mono_ns());
pinch.release(int64_t(mono_ns())); // a drag in progress ends, as lost
grip.release(int64_t(mono_ns()));
gestures.write(pinch, grip, mono_ns());
}
return 0;
}
Reference in new issue View Git Blame Copy Permalink