Giters

ihmcrobotics / euclid

Vector math, geometry, reference frame, and shapes 2D & 3D

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Static tool method or otherwise to "sanitize"/correct slightly invalid orientations

calvertdw opened this issue · comments

commented

Packing Quaternions directly with non-Euclid data, can result in NotARotationMatrix exceptions, as shown below. Currently there is a workaround to go through LinearTransform3D, that fixes the issue.

It'd be nice if there was just a method to sanitize these orientations, as LinearTransform3D must do somehow, with a static Tools method or something.

// Going through LinearTransform3D first prevents NotARotationMatrix exceptions.
LinearTransform3D linearTransform3D = new LinearTransform3D(EuclidGeometryTools.axisAngleFromZUpToVector3D(gpuPlanarRegion.getNormal()));
linearTransform3D.normalize();
FrameQuaternion orientation = new FrameQuaternion();
orientation.setIncludingFrame(cameraFrame, linearTransform3D.getAsQuaternion());
orientation.changeFrame(ReferenceFrame.getWorldFrame());

us.ihmc.euclid.exceptions.NotARotationMatrixException: The matrix is not a rotation matrix:
/ 0.305,  0.663,  0.683 \
|-0.220,  0.747, -0.627 |
\-0.927,  0.041,  0.374 /
	at us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly.checkIfRotationMatrix(Matrix3DReadOnly.java:393)
	at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.set(RotationMatrixBasics.java:101)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:511)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:409)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiply(RotationMatrixTools.java:308)
	at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.append(RotationMatrixBasics.java:326)
	at us.ihmc.euclid.transform.interfaces.RigidBodyTransformBasics.multiply(RigidBodyTransformBasics.java:673)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.efficientComputeTransform(ReferenceFrame.java:924)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.getTransformToRoot(ReferenceFrame.java:862)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.transformFromThisToDesiredFrame(ReferenceFrame.java:811)
	at us.ihmc.euclid.referenceFrame.interfaces.FrameChangeable.changeFrame(FrameChangeable.java:36)
	at us.ihmc.avatar.gpuPlanarRegions.GPUPlanarRegionExtraction.lambda$computePlanarRegions$6(GPUPlanarRegionExtraction.java:459)
commented

That'd be great if you could provide more info so I can write a test that captures the issue, even better is if you could write a minimal failing test. It seems that for this bug it should be quick to do.

commented

Well there is a matrix in the printout I provided that's considered not a rotation matrix, but if you pack those values into a LinearTransform3D and get it as RotationMatrix it will be valid.

commented

I need the untruncated value of that guy to reproduce the error: gpuPlanarRegion.getNormal()