Some misc fixes

[bobsayshilol]

I've split out the new prediction model stuff since while I think it's working well, I need more time to test it proper. This branch is now just the misc changes I'd made and spotted while taking a peek at the code.

Most notably is 4fb40c6 which should stop NaNs from appearing whenever the variance of the position/rotation is 0, ie no movement. This would have caused rxy to be inf due to a divide by 0, and then b would become NaN due to inf * 0 == NaN (since sv would also be 0).

551095f may also be unnecessary since we update the position in the Update() loop, but the documentation does state that it's unreliable and not necessarily every frame so if we do miss a frame it shouldn't stutter.

A bigger issue I'm seeing though is that the server is only getting messages every 0.06s despite inference timestamps reporting 0.02s on the python side. Adding OVERLAPPED support to the named pipe so that a reader is available immediately after the client disconnects made the errors about failing to send go away (in a simulated test case) but it's still only connecting every 0.06s in that test case. Judging by the code a log message about inference time doesn't necessarily imply that there's a new pose so information in the log about that could be useful. Note that lowering the quality of the model or prediction threshold can drop the inference time to 0.01s but the connection still only happens once every 0.06s which confuses me even more. Edit: ended up being a configuration error: lowering the exposure reduced motion blur and resulted in more frequent updates on the server. Another case of "a few hours of trial and error can save you several minutes of looking at the README."

Old description

This is really an amalgamation of 2 PRs, one from when I first poked around with the mediapipe tracking repo a while ago and fixed some issues I noted, and another from the past few days where I was looking into making the tracking more accurate/responsive. In doing so I've also simplified the code in the existing prediction model which has removed avoid what I assume was a division-by-zero check. You can check that it's the same mathematically by expanding the stddev formula and using the observation that sum(x_n) = avg_x * N, or just eyeballing the result with dimensional analysis.

Conceptually, the linear regression model is looking at the "average" velocity over the window of data and trying to predict the next sample from that, which means that if the window isn't close to being odd-symmetric (linear, cubic, etc...) then the prediction can go in the wrong direction. A higher order polynomial approximation might work here, but I reasoned that the latency would be too high and extrapolating that far would send the trackers flying off.

Instead I thought about trying to estimate the instantaneous velocity at the end of the window. To do this I look at all of the snapshot velocities in the window and perform a weighted average on them. Note that the weights I'm using are mostly arbitrary and there might be a better weighting (exponential? logarithmic? option 3?) but this works pretty well for now.

I've included the script that I used to test the new model and convince myself that it might work better. It sets up some time points and introduces random delays between them, then it samples a given curve at those time points plus/minus a random measurement error. It then feeds a window of that data into a predictor and sees how accurate it was to the actual "next" value of the given curve. It should be trivial to extend the script for new prediction methods and movement types too to test their predictive power. For example, it also includes a naive velocity based approach that only looks at the last 2 positions, and as expected it performs worse overall than either the linear regression or the weighted velocity models. Note that the linear regression and the weighted velocity models score very similarly with no clear winner, which was all the convincing I needed to try it out :P

I still need to record an example of it in action, but with a window size of 1 second and 0 additional smoothing there's a big difference in the responsiveness to sudden limb movements and it no longer overpredicts and bounces back when you stop moving (put your hand on your knee and lift your leg quickly). I haven't tested non-0 smoothing yet, but I expect it'll continue to act as a low-pass filter removing the slight jitter in the new model, and I'm using mediapipe only so no testing with the actual AprilTags.

Note that the latest commit is just for testing and I'll drop it at some point. Also I've only just seen #12, so hopefully I'm not treading on any toes!