SolvePnP seems......sensitive

I suggest you don’t try using the “rVec” directly. You should unpack it using the routine Rodrigues() to a 3x3 rotation matrix. That worked correctly for us.

You can see our Python code to go from rVec to rot_matrix to individual angles here:
https://github.com/ligerbots/VisionServer/blob/master/server/rrtargetfinder2019.py
Look in routine compute_output_values().

We also wrote a white paper:
A Step by Step Run-through of FRC Vision Processing
One of the sections explains how we find the angles from the solvePnP output.

1 Like