Add an extra step to enhance the numerical stability.

git-svn-id: https://emgucv.svn.sourceforge.net/svnroot/emgucv/trunk@1153 d7f09016-e345-0410-b530-edf29a71df78

Emgu.CV.Extern/quaternions.h

@@ -203,11 +203,13 @@ typedef struct Quaternions
       _mm_storeu_pd(buffer+2, _mm_sub_pd(_mm_set1_pd(1.0), _mm_mul_pd(_mm_set1_pd(2.0), _mm_add_pd( _mm_mul_pd(_yx, _yx), _mm_mul_pd(_zy, _zy)))));
       _mm_storeu_pd(buffer, _mm_mul_pd(_mm_set1_pd(2.0), _mm_add_pd(_mm_mul_pd(_mm_load1_pd(&w), _mm_shuffle_pd( _zy,_yx, _MM_SHUFFLE2(0,1)) ), _mm_mul_pd(_yx, _zy))));
       *xDst = atan2(buffer[1], buffer[2]);
-      *yDst = asin(2.0 * (w * y - z * x));
+      double v = 2.0 * (w * y - z * x);
+      *yDst = asin(v > 1.0 ? 1.0 : (v < -1.0? -1.0 : v)); //extra step to enhance the numerical stability.
       *zDst = atan2(buffer[0], buffer[3]);
 #else
       *xDst = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x*x + y*y));
-      *yDst = asin(2.0 * (w * y - z * x));
+      double v = 2.0 * (w * y - z * x);
+      *yDst = asin(v > 1.0 ? 1.0 : (v < -1.0? -1.0 : v)); //extra step to enhance the numerical stability.
       *zDst = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y*y + z*z));
 #endif
    }

Emgu.CV.Test/AutoTestQuaternions.cs

@@ -257,5 +257,55 @@ namespace Emgu.CV.Test
          deltaDegree = Math.Abs(y / Math.PI * 180.0 - 31.5);
          Assert.LessOrEqual(deltaDegree, epsilon);
       }
+
+      [Test]
+      public void TestQuaternionsSlerp4()
+      {
+         Random r = new Random();
+         Quaternions q1 = new Quaternions();
+         q1.AxisAngle = new MCvPoint3D64f(0.0, 175.0 / 180 * Math.PI, 0.0);
+         Quaternions q2 = new Quaternions();
+         q2.AxisAngle = new MCvPoint3D64f(0.0, 5.0 / 180 * Math.PI, 0.0);
+
+         double epsilon = 1.0e-12;
+         double x = 0, y = 0, z = 0;
+
+         Quaternions q = q1.Slerp(q2, 0.5);
+         q.GetEuler(ref x, ref y, ref z);
+         Assert.IsFalse(double.IsNaN(x));
+         Assert.IsFalse(double.IsNaN(y));
+         Assert.IsFalse(double.IsNaN(z));
+         double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 90.0);
+         Assert.LessOrEqual(deltaDegree, epsilon);
+      }
+
+      [Test]
+      public void TestQuaternionsSlerp5()
+      {
+         Random r = new Random();
+         Quaternions q1 = new Quaternions();
+         q1.AxisAngle = new MCvPoint3D64f(0.0, 355.0 / 180 * Math.PI, 0.0);
+         Quaternions q2 = new Quaternions();
+         q2.AxisAngle = new MCvPoint3D64f(0.0, 5.0 / 180 * Math.PI, 0.0);
+
+         double epsilon = 1.0e-12;
+         double x = 0, y = 0, z = 0;
+
+         Quaternions q = q1.Slerp(q2, 0.5);
+         q.GetEuler(ref x, ref y, ref z);
+         Assert.IsFalse(double.IsNaN(x));
+         Assert.IsFalse(double.IsNaN(y));
+         Assert.IsFalse(double.IsNaN(z));
+         double deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
+         Assert.LessOrEqual(deltaDegree, epsilon);
+
+         q = q2.Slerp(q1, 0.5);
+         q.GetEuler(ref x, ref y, ref z);
+         Assert.IsFalse(double.IsNaN(x));
+         Assert.IsFalse(double.IsNaN(y));
+         Assert.IsFalse(double.IsNaN(z));
+         deltaDegree = Math.Abs(y / Math.PI * 180.0 - 0.0);
+         Assert.LessOrEqual(deltaDegree, epsilon);
+      }
    }
 }

tests/cvextern_test/cvextern_test.cpp

@@ -74,6 +74,21 @@ void Test_double_MulS()
    cout <<"Test mulS: " << (success ? "Passed" : "Failed") << std::endl;
 }
 
+void Test_quaternions()
+{ 
+   Quaternions q1, q2, q;
+   CvPoint3D64f a1, a2;
+   a1.x = 0.0; a1.y = 175.0 / 180.0 * CV_PI; a1.z = 0.0;
+   a2.x = 0.0; a2.y = 5.0 / 180.0 * CV_PI; a2.z = 0.0;
+   q1.setAxisAngle(&a1);
+   q2.setAxisAngle(&a2);
+   
+   q1.slerp(&q2, 0.5, &q);
+   double x=0, y=0, z=0;
+   q.getEuler(&x, &y, &z);
+
+}
+
 #ifdef _MSC_VER
 void Test_quaternions_performance()
 {
@@ -120,7 +135,7 @@ int main()
    Test_2D_cross_product();
    Test_3D_cross_product();
    Test_double_MulS();
- 
+   Test_quaternions();
 #ifdef _MSC_VER
    Test_quaternions_performance();
    cin >>tmp; //wait for input only if compiling with visual C++