I'm a first-year undergrad building a vision system for a campus robot competition run by our university's RoboMaster team. The robot tracks a planar visual marker (80×80 mm) with OpenCV's solvePnPGeneric + SOLVEPNP_IPPE, and the position feeds a 6D constant-velocity Kalman filter that drives the robot toward the marker.
Planar PnP has a well-known problem: it returns two valid pose solutions. The standard practice is to hard-pick the one with the lower reprojection error, or discard the measurement when the two are close. I tried something less binary — and I'm sharing it here because I couldn't find anyone doing exactly this, and I want to know if it holds up.
Marker image: https://imgur.com/a/oyeXVuP
The method
IPPE returns two solutions, each with a reprojection error (e0 for the nominal/min-error solution, e1 for the other) and a translation (t0, t1). Instead of picking one, I take the nominal translation t0 as the measurement and inflate the Kalman measurement noise covariance R by the spread between the two solutions:
S = [e0^2 / (e0^2 + e1^2)] * (t1 - t0)(t1 - t0)^T
R_eff = R_profile + S
The weight approaches 1/2 when the two solutions are equally good (most ambiguous → largest inflation) and approaches 0 when the second solution is much worse (unambiguous → no inflation).
The idea: when the two solutions disagree, tell the filter to trust this measurement less, instead of pretending we know which solution is correct.
Orientation stays out of the filter entirely — yaw is taken raw from the pose per frame. The reason: a wrong-branch error is a discrete jump, not Gaussian noise, and a Kalman filter can't digest that.
What the data says so far
One thing I checked: is the spread even real? One paper (Fourmy et al., Humanoids 2019) claims the two IPPE solutions share the same position and differ only in rotation — if that were true for our solver, S would be ~zero and the whole method vacuous.
It isn't, at least for OpenCV's implementation. On 1088 frames of replay data:
- |t1 − t0| median 18.2 mm (p90 28.2 mm), almost entirely in depth
- Szz median 1.87e-05 m² (~4.3 mm equivalent std), about 75× our initial Rzz
So the signal is real. But that's about all I can claim with confidence.
What I don't know yet — this is unverified work
To be clear about the status: this is a method, not a result. The following are open, and I'm posting partly to get them challenged:
- No prior work found. I searched (35 queries, EN + CN) and found no paper or public implementation doing exactly this. The closest is Fourmy et al. 2019, which inflates rotation covariance in a factor graph when the two errors are close — but it still hard-picks a solution first, and the trigger is an error ratio, not a pose spread. "Not found" is not "does not exist."
- The theoretical basis is an analogy, not a proof. S structurally matches the between-component spread term of a two-component mixture covariance (law of total variance), but with three deviations: the coefficient should be c(1−c), not c (so S overestimates by up to 2× at full ambiguity — conservative, but ad hoc); the mean stays at t0 rather than the mixture mean; and the e²-ratio weight is not a Bayesian posterior. Related methods (PDAF/GPB) put the spread on the posterior P, not on the measurement R.
- No controlled comparison yet. I have not run fixed-R vs. ratio-test hard pick vs. R_profile+S on the same data. That experiment is planned alongside our R/Q calibration on real robot data — which hasn't happened yet.
If you know prior work, see a flaw, or have a better way to handle planar PnP ambiguity in a tracking filter, I'd genuinely like to hear it. I also asked the same question on Robotics Stack Exchange: https://robotics.stackexchange.com/questions/118663
Top comments (1)
Great implementation. Respect for tackling PnP ambiguity head-on instead of blindly trusting the min-reprojection-error heuristic which always tanks right when lighting shifts or the marker skews.
I’m the Founder & CEO of AGI Systems Directorate. In our company, we work to create artificial general intelligence (AGI) through a four-hybrid approach combining autonomous AI agents, robotics, human brain replication mechanisms, and large language models. When you're pushing real-time vision pipelines on edge hardware, handling dual-root ambiguity cleanly is everything.
Your intuition to map the geometric spread directly onto measurement noise covariance R is a clever hack. Structurally, you're grazing the law of total variance from Gaussian mixture models. But dumping that spread into R rather than state covariance P scales down your Kalman gain globally, making the filter treat the sensor as uniformly noisy rather than capturing true multi-modality. Since IPPE gives you two exact geometric manifolds, have you benchmarked this against a lightweight Gaussian Sum Filter or a PDAF formulation where the mixture uncertainty lives in the state belief?
Spot on regarding keeping yaw raw as well. Forcing discrete SO(3) branch jumps into a continuous kinematic filter is an instant recipe for innovation covariance explosion.
Curious how you're handling temporal consistency when IPPE chatters or flips between the two valid roots across consecutive frames on the RoboMaster board?