From c9e660593b235d768e08ac6c9e6afc6a46431dfc Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 23:03:39 +0000 Subject: [PATCH] Add new prediction model Also add a script for testing models on simulated data. --- driver_files/src/Driver/TrackerDevice.cpp | 49 ++++- driver_files/src/Driver/TrackerDevice.hpp | 4 +- driver_files/src/Driver/VRDriver.cpp | 16 +- driver_files/src/Driver/VRDriver.hpp | 1 + predictor_comparison.py | 254 ++++++++++++++++++++++ 5 files changed, 311 insertions(+), 13 deletions(-) create mode 100644 predictor_comparison.py diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index b390f8d9..bc856fba 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -30,7 +30,7 @@ std::string ExampleDriver::TrackerDevice::GetSerial() return this->serial_; } -void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmooth) +void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmooth, bool velocity) { std::lock_guard guard(pose_mutex); @@ -46,6 +46,7 @@ void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmoo prev_positions.resize(msaved); max_time = mtime; smoothing = msmooth; + use_velocity = velocity; Log("Settings changed! " + std::to_string(msaved) + ' ' + std::to_string(mtime) + ' ' + std::to_string(msmooth) + ' ' + std::to_string(velocity)); } @@ -65,10 +66,6 @@ void ExampleDriver::TrackerDevice::Update() // Update pose timestamp _pose_timestamp = time_now; - // Copy the previous position data - double previous_position[3] = { 0 }; - std::copy(std::begin(pose.vecPosition), std::end(pose.vecPosition), std::begin(previous_position)); - PoseInfo next_pose, pose_rate; if (get_next_pose(Seconds(0), next_pose, &pose_rate) != 0) return; @@ -82,7 +79,7 @@ void ExampleDriver::TrackerDevice::Update() } { - // Guard around uses of |smoothing| + // Guard around uses of |smoothing| and |use_velocity| std::lock_guard guard(pose_mutex); pose.vecPosition[0] = next_pose[0] * (1 - smoothing) + pose.vecPosition[0] * smoothing; @@ -93,6 +90,11 @@ void ExampleDriver::TrackerDevice::Update() pose.qRotation.x = next_pose[4] * (1 - smoothing) + pose.qRotation.x * smoothing; pose.qRotation.y = next_pose[5] * (1 - smoothing) + pose.qRotation.y * smoothing; pose.qRotation.z = next_pose[6] * (1 - smoothing) + pose.qRotation.z * smoothing; + + if (!use_velocity) + { + pose_rate.fill(0); + } } //normalize @@ -156,7 +158,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n statuscode = 1; } - int curr_saved = 0; + size_t curr_saved = 0; double avg_time = 0; double avg_time2 = 0; @@ -183,11 +185,11 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n avg_time2 /= curr_saved; const double st = std::sqrt(avg_time2 - avg_time * avg_time); - for (int i = 0; i < next_pose.size(); i++) + for (size_t i = 0; i < next_pose.size(); i++) { double avg_val = 0; double avg_tval = 0; - for (int ii = 0; ii < curr_saved; ii++) + for (size_t ii = 0; ii < curr_saved; ii++) { const PrevPose& prev_pose = prev_positions[ii]; avg_val += prev_pose.pose[i]; @@ -203,6 +205,35 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n pose_rate[i] = -m; // -ve since |new_time| and |PrevPose::time| increase into the past } + if (use_velocity) + { + // Velocity prediction only tested on position + for (size_t elem = 0; elem < 3; elem++) + { + double vel = 0; + double weights = 0; + for (size_t i = 0; i < curr_saved - 1; i++) + { + const PrevPose& now = prev_positions[i]; + const PrevPose& prev = prev_positions[i + 1]; + + const double dx = now.pose[elem] - prev.pose[elem]; + const double dt = now.time - prev.time; + const double weight = 1.0 / (1.0 + i); + + vel += weight * dx / dt; + weights += weight; + } + vel /= weights; + + const PrevPose& latest = prev_positions.front(); + const double pos = latest.pose[elem] + vel * (new_time - latest.time); + + next_pose[elem] = pos; + pose_rate[elem] = -vel; // -ve since |new_time| and |PrevPose::time| increase into the past + } + } + return statuscode; } diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 5ab3c647..1e5c4ba2 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -40,7 +40,7 @@ namespace ExampleDriver { virtual void DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize) override; virtual vr::DriverPose_t GetPose() override; - void reinit(int msaved, double mtime, double msmooth); + void reinit(int msaved, double mtime, double msmooth, bool use_velocity); void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, Seconds time_offset); int get_next_pose(Seconds time_offset, PoseInfo& next_pose, PoseInfo* pose_rate = nullptr) const; @@ -63,6 +63,6 @@ namespace ExampleDriver { std::chrono::system_clock::time_point last_update; double max_time = 1; double smoothing = 0; - + bool use_velocity = false; }; }; diff --git a/driver_files/src/Driver/VRDriver.cpp b/driver_files/src/Driver/VRDriver.cpp index 724e69ef..2315f000 100644 --- a/driver_files/src/Driver/VRDriver.cpp +++ b/driver_files/src/Driver/VRDriver.cpp @@ -110,7 +110,7 @@ void ExampleDriver::VRDriver::PipeThread() auto addtracker = std::make_shared(name, role); this->AddDevice(addtracker); this->trackers_.push_back(addtracker); - addtracker->reinit(tracker_max_saved, tracker_max_time, tracker_smoothing); + addtracker->reinit(tracker_max_saved, tracker_max_time, tracker_smoothing, tracker_use_velocity); s = s + " added"; } else if (word == "addstation") @@ -266,7 +266,7 @@ void ExampleDriver::VRDriver::PipeThread() iss >> msmooth; for (auto& device : this->trackers_) - device->reinit(msaved,mtime,msmooth); + device->reinit(msaved,mtime,msmooth, tracker_use_velocity); tracker_max_saved = msaved; tracker_max_time = mtime; @@ -274,6 +274,18 @@ void ExampleDriver::VRDriver::PipeThread() s = s + " changed"; } + else if (word == "use_velocity") + { + int use_velocity; + iss >> use_velocity; + + tracker_use_velocity = use_velocity; + + for (auto& device : this->trackers_) + device->reinit(tracker_max_saved, tracker_max_time, tracker_smoothing, tracker_use_velocity); + + s = s + " changed"; + } else { s = s + " unrecognized"; diff --git a/driver_files/src/Driver/VRDriver.hpp b/driver_files/src/Driver/VRDriver.hpp index dbbd437e..82e9ee80 100644 --- a/driver_files/src/Driver/VRDriver.hpp +++ b/driver_files/src/Driver/VRDriver.hpp @@ -66,5 +66,6 @@ namespace ExampleDriver { int tracker_max_saved = 10; double tracker_max_time = 1; double tracker_smoothing = 0; + bool tracker_use_velocity = false; }; }; diff --git a/predictor_comparison.py b/predictor_comparison.py new file mode 100644 index 00000000..0b172740 --- /dev/null +++ b/predictor_comparison.py @@ -0,0 +1,254 @@ +#!/usr/bin/python +import numpy as np + +############################ +# Simulated movement classes +############################ + +# Linear motion with gradient |m| passing through (t0,h0) +class GenerateLinear: + def __init__(self, t0, h0, m): + self.t0 = t0 + self.h0 = h0 + self.m = m + + def at(self, t): + return self.h0 + self.m * (t - self.t0) + + def __str__(self): + return f"{type(self).__name__}({self.t0}, {self.h0}, {self.m})" + +# SHM with scale |scale| at frequency |hz| +class GenerateSHM: + def __init__(self, scale, hz): + self.scale = scale + self.hz = hz + + def at(self, t): + return self.scale * np.sin(2 * np.pi * self.hz * t) + + def __str__(self): + return f"{type(self).__name__}({self.scale}, {self.hz})" + +# Impulse step from |h0| to |h1| at time |t| +class GenerateStep: + def __init__(self, h0, t, h1): + self.t = t + self.h0 = h0 + self.h1 = h1 + + def at(self, t): + return np.where(t < self.t, self.h0, self.h1) + + def __str__(self): + return f"{type(self).__name__}({self.h0}, {self.t}, {self.h1})" + +# Smoothstep from (t0,h0) to (t1,h1) +class GenerateSmooth: + def __init__(self, t0, h0, t1, h1): + self.t0 = t0 + self.h0 = h0 + self.t1 = t1 + self.h1 = h1 + + def at(self, t): + res = np.where(t <= self.t0, self.h0, 0).astype(float) + res += np.where(t >= self.t1, self.h1, 0) + s = self.h0 + t * t * (3 - 2 * t) * (self.h1 - self.h0) + res += np.where((self.t0 < t) & (t < self.t1), s, 0) + return res + + def __str__(self): + return f"{type(self).__name__}({self.t0}, {self.h0}, {self.t1}, {self.h1})" + + +################### +# Predictor classes +################### + +# Linear regression model +class PredictorLinear: + def predict(self, ts, xs, t): + avg_t = np.mean(ts) + std_t = np.std(ts) + avg_x = np.mean(xs) + txs = ts * xs + avg_tx = np.mean(txs) + m = (avg_tx - avg_t * avg_x) / (std_t * std_t) + return avg_x + m * (t - avg_t) + + def __str__(self): + return f"{type(self).__name__}()" + +# Simple velocity model +class PredictorVelocity: + def predict(self, ts, xs, t): + dx = xs[-1] - xs[-2] + dt = ts[-1] - ts[-2] + v = dx / dt + return xs[-1] + v * (t - ts[-1]) + + def __str__(self): + return f"{type(self).__name__}()" + +# Weighted velocity model +class PredictorWeightedVelocity: + def predict(self, ts, xs, t): + v = 0.0 + ws = 0.0 + for i in range(len(ts) - 1): + idx = -1 - i + dx = xs[idx] - xs[idx - 1] + dt = ts[idx] - ts[idx - 1] + w = 1.0 / (1.0 + i) + v += w * dx / dt + ws += w + v /= ws + return xs[-1] + v * (t - ts[-1]) + + def __str__(self): + return f"{type(self).__name__}()" + + +############## +# Test harness +############## + +class Harness: + def __init__(self, generator, predictor, fps, latency): + self.generator = generator + self.predictor = predictor + self.spf = 1 / fps + self.rng = np.random.default_rng() + # Setup 1s of timestamps + self.ts = np.arange(0, 1, self.spf) - latency + + def perturb(self, data, delta): + data += (2 * self.rng.random(data.shape) - 1) * delta + return data + + def test(self, ts, t1): + ts = self.perturb(ts, self.spf * 0.02) # +-2% of a frame + xs = self.generator.at(ts) + xs = self.perturb(xs, 0.01) # +-1cm measurement error + predicted = self.predictor.predict(ts, xs, t1) + actual = self.generator.at(t1) + return predicted - actual + + def run(self, window): + repetitions = 10 + deltas = [] + for start in range(len(self.ts) - window + 1): + for _ in range(repetitions): + win = self.ts[start : start + window] + t = (start + window) * self.spf + delta = self.test(win, t) + deltas.append(delta) + return np.mean(deltas), np.std(deltas) + + +#################### +# Setup tests to run +#################### + +predictors = [ + PredictorLinear(), + PredictorVelocity(), + PredictorWeightedVelocity(), +] +generators = [ + # Basic movement + GenerateLinear(0, 0.1, 0.2), + GenerateLinear(0.2, 0.9, -2), + # Circular movements + GenerateSHM(0.8, 0.2), + GenerateSHM(0.6, 2.1), + GenerateSHM(0.3, 6), + # Sudden jumps + GenerateStep(0.2, 0.5, 0.21), # 1cm + GenerateStep(0.4, 0.5, 0.3), # 10cm + # Smooth movement + GenerateSmooth(0.2, -0.4, 1.1, 0.5), # slow + GenerateSmooth(0.4, -0.4, 0.6, 0.5), # fast +] +fpss = [ + 10, + 20, + 30, +] +latencies = [ + #0, + 0.001, + 0.005, + 0.02, + 0.05, +] +windows = [ + #1, + 2, + #5, + 10, + #20, +] + + +############### +# Run the tests +############### + +from collections import namedtuple +Result = namedtuple('Result', 'predictor generator fps latency window mean stddev') +results = [] +for predictor in predictors: + for generator in generators: + for fps in fpss: + for latency in latencies: + for window in windows: + # We only test 1s of data, so fps must be at least window size + if fps >= window: + harness = Harness(generator, predictor, fps, latency) + mean,stddev = harness.run(window) + results.append(Result(predictor, generator, fps, latency, window, mean, stddev)) + +# Dump it all as a CSV +with open('out.csv', 'w+') as f: + f.write("predictor,generator,fps,latency,window,mean,stddev\n") + for result in results: + f.write(f'"{result.predictor}","{result.generator}",{result.fps},{result.latency},{result.window},{result.mean},{result.stddev}\n') + + +########################## +# Do some basic processing +########################## + +predictor_counts = {} +for generator in generators: + for fps in fpss: + for latency in latencies: + # Find best for this combo, over different window sizes + filtered = [] + for window in windows: + for result in results: + if result.fps == fps and result.latency == latency and result.window == window and result.generator == generator: + filtered.append(result) + + # Sort them + def scorer(elem): + # TODO: factor in stddev better + return abs(elem.mean) + 2 * elem.stddev + filtered.sort(key=scorer) + + print(f"Top 3 for {generator} / {fps}fps / {latency}s latency:") + for idx in range(3): + result = filtered[idx] + name = f"{result.predictor}" + print(f"\t{name:30} window={result.window}:\t{result.mean:.5}\t{result.stddev:.5}\t({scorer(result):.5})") + + # Score based on ranking + if result.predictor not in predictor_counts: + predictor_counts[result.predictor] = 0 + predictor_counts[result.predictor] += 1 / (1 + idx) # scorer(result) + +print("Final scores:") +for predictor, count in predictor_counts.items(): + print(f"\t{predictor}: {count:.1f}")