Dual-Marker Relative Pose Estimation
System Overhaul

This is a project at my current employer. For confidentiality, product, customer, and site details are omitted; the write-up focuses on the problem-solving process and engineering decisions. The improved code runs on robots in production.

PART 1Pose stabilization — the filters that did nothing

The code had an LPF and a median filter, yet the output jittered almost exactly like the raw signal. Tracing the code revealed the LPF was re-initialized every frame, remembering no state — and its cutoff was set to 100 Hz, driving the interpolation coefficient to 1 so values passed through unchanged. Pitch variance also grew with marker distance (z), and error cases produced 48 frames jumping by more than 2°.

Rebuilding the filtering pipeline

• Fixed the repeated LPF initialization; re-tuned cutoff 100 Hz → 5 Hz via quantitative frequency sweeps
• Replaced Euler-angle filtering with a SLERP-based LPF handling quaternion sign ambiguity
• Swapped quaternion median from geodesic medoid to Weiszfeld's algorithm
• Prototyped a LUT-based distance-adaptive filter from pitch variance versus distance, but did not integrate it into the production code. Comparative experiments favored a One Euro Filter (rate-adaptive filtering), which was adopted instead.

Ceres-based pose estimation

Instead of stock OpenCV solvePnP, I re-implemented pose estimation to minimize reprojection error directly with Ceres Solver — Huber loss to bound outlier corners, optimization on the quaternion manifold to avoid rotation singularities, and soft constraints on roll/yaw, which the mechanism physically limits. Six version combinations were replayed on the same rosbag data and compared on pitch std/range, z std, and gap/jump counts to pick the final configuration.

PART 2Detection & compute — better detection costs more

On long-wheelbase vehicles the marker-to-camera distance grows, so far-range detection failed — only 5.8% of frames detected while driving. Turning on precise corner refinement (APRILTAG) recovered detection but multiplied processing time ~5×, while two always-on detector nodes loaded the onboard CPU. The task: satisfy detection performance and real-time budget at once.

Detection — quantitative sweep of preprocessing × refinement

• Sharpening preprocessing on the ROI improved both missed and false detections at range
• Compared 12 combinations of cornerRefinement (NONE · SUBPIX · APRILTAG) × preprocessing on identical data
• Diagnosed an OpenCV APRILTAG infinite-loop issue found during the sweep; resolved via library upgrade

Compute — dynamic ROI and execution gating

Rather than scanning full frames, a Kalman filter tracks the marker center and a dynamic ROI detects only where the marker should be. A 4-state design — INIT · TRACKING · SEARCHING · RECOVERY — gradually widens the search on tracking loss and falls back to full-frame search after sustained failure. Detector nodes are now activated only when the driving-mode flag requires them.

To mimic production conditions, latency was analyzed under stress-ng CPU load — not just the mean but P95/P99 tail latency, since intermittent spikes are what break real-time control.

DEMODemo video

Marker detection and ROI tracking with water reflections in the scene — TRACKING stays stable under low light and reflective glare