Add 6328's implementation of PNP distance for Trig Solving to PhotonPoseEstimator

[JuliusZhou124]

https://discord.com/channels/725836368059826228/725846784131203222/1334309604946874460

https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/85

Helps with ambiguous single tag estimates and produces more stability.

[mcm001]

Sweet. I'd like to see some tests as well if we can.

[jwbonner]

I would recommend taking a look at this post and the reply. The original version of this logic that we released (which seems to be implemented here) doesn't work properly when ty is nonzero.

[JuliusZhou124]

I would recommend taking a look at this post and the reply. The original version of this logic that we released (which seems to be implemented here) doesn't work properly when ty is nonzero.

Thanks for the catch! Missed that in the thread. 😅

[mcm001]

Fya, we will still need to see tests for all the edge cases (camera straight, pitched/yawed/rolled, compound angles) before merging. I think our simulation code should be able to get you the fake targets you need, and you can just copy paste some of our existing pose estimator tests.

[JuliusZhou124]

I think our simulation code should be able to get you the fake targets you need, and you can just copy paste some of our existing pose estimator tests.

Is there any fast way to get fake targets from simulation? Brute force copying seems rather tedious.

[mcm001]

Yeah. I think you should be able to use our vision system simulation to generate the target data?

[JuliusZhou124]

Looks like the poses begin to deviate as the targets approach the corners of the camera feed.

Might be an implementation bug? Either way it's 10+ cm off in both x and y when simulating with perfect cameras.

[JuliusZhou124]

Struggles with rolled cameras as well:

[mcm001]

Broadly Lgtm. Delegating to @spacey-sooty :)

[mcm001]

Looks like two new flakes:

• https://github.com/PhotonVision/photonvision/actions/runs/13314440628/job/37184905664?pr=1767#step:5:4580

 Starting process 'command '/usr/bin/clang++''. Working directory: /Users/runner/work/photonvision/photonvision/photon-targeting/build/objs/photontargetingTest/osxuniversal/release/photontargetingCpp Command: /usr/bin/clang++ @/Users/runner/work/photonvision/photonvision/photon-targeting/build/tmp/compilePhotontargetingTestOsxuniversalReleaseGoogleTestExePhotontargetingCpp/options.txt /Users/runner/work/photonvision/photonvision/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp -o /Users/runner/work/photonvision/photonvision/photon-targeting/build/objs/photontargetingTest/osxuniversal/release/photontargetingCpp/at79tvvs4oj7e6ruemd6xlbm5/MultiTargetPNPResult.o
Successfully started process 'command '/usr/bin/clang++''
libc++abi: terminating due to uncaught exception of type std::__1::system_error: mutex lock failed: Invalid argument

• https://github.com/PhotonVision/photonvision/actions/runs/13314440628/job/37184904590?pr=1767#step:5:7240

 [ RUN      ] VisionSystemSimTest.TestMultipleTargets
D:\a\photonvision\photonvision\photon-lib\src\test\native\cpp\VisionSystemSimTest.cpp(401): error: Expected equality of these values:
  static_cast<size_t>(11)
    Which is: 11
  tgtList.size()
    Which is: 10

[  FAILED  ] VisionSystemSimTest.TestMultipleTargets (12 ms)

[mcm001]

@JuliusZhou124 as follow on work, we should update our docs page and stuff. And you should go brag about this in chief delphi!