public void TestQuaternion3() { Random r = new Random(); Quaternions q1 = new Quaternions(); q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble()); Quaternions q2 = new Quaternions(); q2.AxisAngle = q1.AxisAngle; double epsilon = 1.0e-8; EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon); RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z }); Mat m1 = rVec.RotationMatrix; Matrix <double> m2 = new Matrix <double>(3, 3); q1.GetRotationMatrix(m2); Matrix <double> diff = new Matrix <double>(3, 3); CvInvoke.AbsDiff(m1, m2, diff); double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C); EmguAssert.IsTrue(norm < epsilon); Quaternions q4 = q1 * Quaternions.Empty; //EmguAssert.IsTrue(q4.Equals(q1)); }
public void TestQuaternions1() { Quaternions q = new Quaternions(); double epsilon = 1.0e-10; Matrix <double> point = new Matrix <double>(3, 1); point.SetRandNormal(new MCvScalar(), new MCvScalar(20)); using (Matrix <double> pt1 = new Matrix <double>(3, 1)) using (Matrix <double> pt2 = new Matrix <double>(3, 1)) using (Matrix <double> pt3 = new Matrix <double>(3, 1)) { double x1 = 1.0, y1 = 0.2, z1 = 0.1; double x2 = 0.0, y2 = 0.0, z2 = 0.0; q.SetEuler(x1, y1, z1); q.GetEuler(ref x2, ref y2, ref z2); EmguAssert.IsTrue( Math.Abs(x2 - x1) < epsilon && Math.Abs(y2 - y1) < epsilon && Math.Abs(z2 - z1) < epsilon); q.RotatePoints(point, pt1); Matrix <double> rMat = new Matrix <double>(3, 3); q.GetRotationMatrix(rMat); CvInvoke.Gemm(rMat, point, 1.0, null, 0.0, pt2, Emgu.CV.CvEnum.GemmType.Default); CvInvoke.AbsDiff(pt1, pt2, pt3); EmguAssert.IsTrue( pt3[0, 0] < epsilon && pt3[1, 0] < epsilon && pt3[2, 0] < epsilon); } double rotationAngle = 0.2; q.SetEuler(rotationAngle, 0.0, 0.0); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q.SetEuler(0.0, rotationAngle, 0.0); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q.SetEuler(0.0, 0.0, rotationAngle); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q = q * q; EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - rotationAngle) < epsilon); q.SetEuler(0.2, 0.1, 0.05); double t = q.RotationAngle; q = q * q; EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - t) < epsilon); }
public void TestQuaternions1() { Quaternions q = new Quaternions(); double epsilon = 1.0e-10; Matrix<double> point = new Matrix<double>(3, 1); point.SetRandNormal(new MCvScalar(), new MCvScalar(20)); using (Matrix<double> pt1 = new Matrix<double>(3, 1)) using (Matrix<double> pt2 = new Matrix<double>(3, 1)) using (Matrix<double> pt3 = new Matrix<double>(3, 1)) { double x1 = 1.0, y1 = 0.2, z1 = 0.1; double x2 = 0.0, y2 = 0.0, z2 = 0.0; q.SetEuler(x1, y1, z1); q.GetEuler(ref x2, ref y2, ref z2); EmguAssert.IsTrue( Math.Abs(x2 - x1) < epsilon && Math.Abs(y2 - y1) < epsilon && Math.Abs(z2 - z1) < epsilon); q.RotatePoints(point, pt1); Matrix<double> rMat = new Matrix<double>(3, 3); q.GetRotationMatrix(rMat); CvInvoke.Gemm(rMat, point, 1.0, null, 0.0, pt2, Emgu.CV.CvEnum.GemmType.Default); CvInvoke.AbsDiff(pt1, pt2, pt3); EmguAssert.IsTrue( pt3[0, 0] < epsilon && pt3[1, 0] < epsilon && pt3[2, 0] < epsilon); } double rotationAngle = 0.2; q.SetEuler(rotationAngle, 0.0, 0.0); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q.SetEuler(0.0, rotationAngle, 0.0); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q.SetEuler(0.0, 0.0, rotationAngle); EmguAssert.IsTrue(Math.Abs(q.RotationAngle - rotationAngle) < epsilon); q = q * q; EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - rotationAngle) < epsilon); q.SetEuler(0.2, 0.1, 0.05); double t = q.RotationAngle; q = q * q; EmguAssert.IsTrue(Math.Abs(q.RotationAngle / 2.0 - t) < epsilon); }
public void TestQuaternion3() { Random r = new Random(); Quaternions q1 = new Quaternions(); q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble()); Quaternions q2 = new Quaternions(); q2.AxisAngle = q1.AxisAngle; double epsilon = 1.0e-12; Assert.Less(Math.Abs(q1.W - q2.W), epsilon); Assert.Less(Math.Abs(q1.X - q2.X), epsilon); Assert.Less(Math.Abs(q1.Y - q2.Y), epsilon); Assert.Less(Math.Abs(q1.Z - q2.Z), epsilon); RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.x, q1.AxisAngle.y, q1.AxisAngle.z }); Matrix<double> m1 = rVec.RotationMatrix; Matrix<double> m2 = new Matrix<double>(3, 3); q1.GetRotationMatrix(m2); Matrix<double> diff = new Matrix<double>(3, 3); CvInvoke.cvAbsDiff(m1, m2, diff); double norm = CvInvoke.cvNorm(diff, IntPtr.Zero, Emgu.CV.CvEnum.NORM_TYPE.CV_C, IntPtr.Zero); Assert.Less(norm, epsilon); }
public void TestQuaternion3() { Random r = new Random(); Quaternions q1 = new Quaternions(); q1.AxisAngle = new MCvPoint3D64f(r.NextDouble(), r.NextDouble(), r.NextDouble()); Quaternions q2 = new Quaternions(); q2.AxisAngle = q1.AxisAngle; double epsilon = 1.0e-8; EmguAssert.IsTrue(Math.Abs(q1.W - q2.W) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.X - q2.X) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.Y - q2.Y) < epsilon); EmguAssert.IsTrue(Math.Abs(q1.Z - q2.Z) < epsilon); RotationVector3D rVec = new RotationVector3D(new double[] { q1.AxisAngle.X, q1.AxisAngle.Y, q1.AxisAngle.Z }); Mat m1 = rVec.RotationMatrix; Matrix<double> m2 = new Matrix<double>(3, 3); q1.GetRotationMatrix(m2); Matrix<double> diff = new Matrix<double>(3, 3); CvInvoke.AbsDiff(m1, m2, diff); double norm = CvInvoke.Norm(diff, Emgu.CV.CvEnum.NormType.C); EmguAssert.IsTrue(norm < epsilon); Quaternions q4 = q1 * Quaternions.Empty; //EmguAssert.IsTrue(q4.Equals(q1)); }