[photonlib] Simulation robustness

[amquake]

Makes solvePNP methods return Optionals to handle bad inputs and report failures. This reveals two test cases which have NaN solvePNP results-- uncertain yet to the cause of this...

Also overwrites java changes from #817 since #742 had duplicate fixes

---

• PNPResults can now be empty (isPresent = false)
• solvePNP methods actually handle errors and return empty PNPResults
  • This reveals an odd error where some inputs to solvePNP_SQUARE() resulted in an estimated transform with NaN values, and attempts to handle it
• Overwrites java changes from Don't add bad corners to multi-tag solution #817 since [WIP] Simulation Overhaul #742 had duplicate fixes
• Minor bugfixes

[amquake]

Still kinda stumped on the cause of the NaN results.. here are the offending inputs used in the testMultipleTargets() test:

cammat: [512.1070492931361, 0, 320;
 0, 539.048825737012, 240;
 0, 0, 1]
distcoeffs: [0;
 0;
 0;
 0;
 0]
objpts: [-0.076200001, 0.076200001, 0;
 0.076200001, 0.076200001, 0;
 0.076200001, -0.076200001, 0;
 -0.076200001, -0.076200001, 0]
imgpts: [326.14478, 244.11597;
 318.32449, 244.11569;
 318.32449, 235.88431;
 326.14478, 235.88403]

outputs:

error: [nan;
 nan]
tvecs: [-nan(ind);
 -nan(ind);
 -nan(ind)]
SolvePNP_SQUARE failed!
java.lang.Exception: SolvePNP_SQUARE NaN result

Changing the input by even an unnoticeably small amount causes the error to go away. I've confirmed that duplicating the code and inputs in another project causes the same NaN result. There were only two instances of this in the tests, and one of them was in testVisibilityCupidShuffle() where the targets weren't actually square (but using square solvepnp). So there is just this one instance where the method is being unreasonably picky, and its hard to diagnose through the JNI.

Rather than spending more effort trying to get to the bottom of this, its easier to just... add some negligible noise to the input when this happens and retry 😅.

[amquake]

@mcm001 Looking at #816, should I just change PNPResults here to match there, where there is an isPresent flag (instead of Optionals)?

[mcm001]

isPresent

Perhaps. If protobuf performance doesn't suck, we'll go back to Optional, I think. I don't super care either way

[mcm001]

The reason I split these functions is that the behavior the origin of the Pnpresult transform was based on if it sees 1 target or more than one, previously. Is that no longer the case? It looks like still if you see one tag it puts the tag at the origin, not at where it is on the field?

[amquake]

There was a bug in VisionEstimation previously where that method would give kTagModel.getFieldVertices(knownTags.get(0).pose) to solvePNP_SQUARE instead of TargetModel.kTag16h5.vertices (which is required for that solvepnp method and is assumed in the transforms following it). It should now correctly return the origin-to-camera transform for both single and multi tag.

[mcm001]

Is there a unit test proving this?

[amquake]

Updated summary