Sped up pose estimation by reducing allocations and string parse in matd_op

[ckorris-imt]

My friend/colleague @jonlinsner and I found that estimate_tag_pose was much slower than expected - over a millisecond per pose on my work desktop. This was because of a lot of allocation that happens when calling matd_op, due to parsing a string each call, and matd_create, which makes new matrices. That means that every intermediate 3x3 or 3x1 result in the process (which runs iteratively in a loop) results in temporary heap allocations - about 26,000 per pose.

This PR rewrites the hot path (orthogonal_iteration, fix_pose_ambiguities, calculate_F) to do the same math on fixed-size, stack-allocated values. In an attempt to be consistent with existing code, I added utilities in common/svd33 to mirror how matd_svd uses common/svd22.

I had Claude build harnesses to test the before and after using 20,000 randomized synthetic detections (changing poses, focal lengths, tag size, and sub-pixel corner noise) and found that the worst-case difference was ~9e-11 for rotation and ~6e-13 for translation.

On my work PC, the cost to run estimate_tag_pose once went from ~1.86ms to ~0.08ms.

In full disclosure, Claude generated the corrected code, as it's a much better mathematician than I. I supervised it closely to make it consistent with the rest of the code base, and to make sure tests were thorough. It actually found an additional place for performance (which is noted in a comment) which reduced it by about ~30 µs more, but I wanted the code to be closer to your existing, tried-and-true math. If this PR is accepted, I'll do more tests to make sure there's no accuracy regression and open a second PR to address that.

In addition to the test harness, I logged the time it takes to detect one tag from within my Unity application. It's about 10x faster, including some extra overhead (but also including the second, much smaller performance improvement I've deferred for now).

For verifying, the test harnesses can be found here with instructions.

[fallenmi]

I reviewed the pose-estimation rewrite from exact base 0e16a12dd380fd607e4afd54712ee9b1ffb9ec8f to exact head 56524a89bfddd99c167d7cef4de496462f45827b, and also tested its clean merge into current master b7c0ebe9aa20f82ec7a828579004f9e706bfecd9 (merge tree b2a20f4f2bdfe7485a96f66af5026e94ad28158b).

All seven upstream tests passed on exact base, head, and current merge. I also ran a deterministic 10,000-case pose oracle spanning camera intrinsics, tag scales/distances, rotations, and corner noise. Base and head produced the second pose for the same 9,168 cases, all returned values were finite, and the largest base/head differences were 1.82e-7 in a rotation-matrix element and 2.35e-9 in a translation element. Head and current-merge outputs were byte-identical. In this harness, the rewrite reduced average pose time from 638 us to 45.3 us (about 14x).

The new fixed-size svd33 implementation also passed 100,000 deterministic full-rank, rank-deficient, repeated-singular-value, near-singular, zero, and scaled matrices: zero reconstruction, orthogonality, ordering, or finiteness failures, with maximum relative reconstruction error 1.56e-15. The pose and SVD oracles completed on exact head and current merge under ASan/UBSan with no sanitizer finding.

GitHub currently exposes no check suite, workflow run, or commit-status context for this head, so the exact-source local results above are the available test evidence. I did not find a correctness blocker in the submitted change.

Disclosure: I used OpenAI Codex to assist this review; I independently verified the exact refs, current merge, code paths, numerical comparisons, performance measurement, and test results described above.