Incorrect conversion of Rodrigues from OpenCV

[prensing]

In OpenCVHelp, the routine rvecToRotation() does not work if the input rvec is all 0.

If rvec = (0,0,0), this is a valid value which means "no rotation". The equivalent rotation matrix is the 3x3 identity. I checked this in Python with OpenCV directly. The current version produces a Rotation3d which is all "NaN".

I am seeing this in the simulation where the MultiTag results exactly produce a EstimatedPose with no rotation.

Build is latest from master branch. Running on Linux with the simulation.

[prensing]

I wonder if it would be better to have OpenCV convert it to a 3x3 rotation matrix, and then use that to create the Rotation3d. Rotation matrices are very tightly defined, and I think WPILib has a constructor from one.

[prensing]

I think the bug is that you are dividing by the norm() to normalize the vector.

Looks to me like you could just use the existing constructor:
Rotation3d(Vector rvec)

[amquake]

Yes, it should be updated to use that Rotation3d constructor. All it does differently is skip the constructor if the rvec norm is 0.

[mcm001]

Working this now

[prensing]

I tested and it seemed to be good. Fixed my particular problem.