Planar PnP Returns Two Poses — I Feed Their Disagreement Into the Kalman Filter Instead of Picking One

Sean TU — Mon, 09 Nov 2026 10:12:57 +0000

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

DEV Community: Sean TU

Planar PnP Returns Two Poses — I Feed Their Disagreement Into the Kalman Filter Instead of Picking One

The method

What the data says so far

What I don't know yet — this is unverified work