public void TestTransformComputation() { EuclideanTransform t; ErrorElements errorElements; ConstructTestCase(out t, out errorElements); var minimizer = new PointToPlaneErrorMinimizer(); var solvedT = KnownCorrespondenceErrorMinimizer.IterativeSolveForTransform(errorElements, minimizer); float dist = (t.translation - solvedT.translation).Length(); Assert.IsTrue(Precision.AlmostEqualInDecimalPlaces(0.0f, dist, 3)); float angle = VectorHelpers.AngularDistance(t.rotation, solvedT.rotation); Assert.IsTrue(Precision.AlmostEqualInDecimalPlaces(0.0, Math.IEEERemainder(angle, Math.PI * 2), 3)); }