quat_slerp.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <Eigen/Geometry>
00004 #include <bench/BenchTimer.h>
00005 using namespace Eigen;
00006 using namespace std;
00007 
00008 
00009 
00010 template<typename Q>
00011 EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
00012 {
00013   return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
00014 }
00015 
00016 template<typename Q>
00017 EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
00018 {
00019   return a.slerp(t,b);
00020 }
00021 
00022 template<typename Q>
00023 EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
00024 {
00025   typedef typename Q::Scalar Scalar;
00026   static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
00027   Scalar d = a.dot(b);
00028   Scalar absD = internal::abs(d);
00029   if (absD>=one)
00030     return a;
00031 
00032   // theta is the angle between the 2 quaternions
00033   Scalar theta = std::acos(absD);
00034   Scalar sinTheta = internal::sin(theta);
00035 
00036   Scalar scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00037   Scalar scale1 = internal::sin( ( t * theta) ) / sinTheta;
00038   if (d<0)
00039     scale1 = -scale1;
00040 
00041   return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
00042 }
00043 
00044 template<typename Q>
00045 EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
00046 {
00047   typedef typename Q::Scalar Scalar;
00048   static const Scalar one = Scalar(1) - epsilon<Scalar>();
00049   Scalar d = a.dot(b);
00050   Scalar absD = internal::abs(d);
00051   
00052   Scalar scale0;
00053   Scalar scale1;
00054   
00055   if (absD>=one)
00056   {
00057     scale0 = Scalar(1) - t;
00058     scale1 = t;
00059   }
00060   else
00061   {
00062     // theta is the angle between the 2 quaternions
00063     Scalar theta = std::acos(absD);
00064     Scalar sinTheta = internal::sin(theta);
00065 
00066     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00067     scale1 = internal::sin( ( t * theta) ) / sinTheta;
00068     if (d<0)
00069       scale1 = -scale1;
00070   }
00071 
00072   return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
00073 }
00074 
00075 template<typename T>
00076 inline T sin_over_x(T x)
00077 {
00078   if (T(1) + x*x == T(1))
00079     return T(1);
00080   else
00081     return std::sin(x)/x;
00082 }
00083 
00084 template<typename Q>
00085 EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
00086 {
00087   typedef typename Q::Scalar Scalar;
00088   
00089   Scalar d = a.dot(b);
00090   Scalar theta;
00091   if (d<0.0)
00092     theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
00093   else
00094     theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
00095   
00096   // theta is the angle between the 2 quaternions
00097 //   Scalar theta = std::acos(absD);
00098   Scalar sinOverTheta = sin_over_x(theta);
00099 
00100   Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
00101   Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
00102   if (d<0)
00103     scale1 = -scale1;
00104 
00105   return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
00106 }
00107 
00108 template<typename Q>
00109 EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
00110 {
00111   typedef typename Q::Scalar Scalar;
00112   
00113   Scalar d = a.dot(b);
00114   Scalar theta;
00115 //   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
00116 //   if (d<0.0)
00117 //     theta = M_PI-theta;
00118   
00119   if (d<0.0)
00120     theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
00121   else
00122     theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
00123   
00124   
00125   Scalar scale0;
00126   Scalar scale1;
00127   if(theta*theta-Scalar(6)==-Scalar(6))
00128   {
00129     scale0 = Scalar(1) - t;
00130     scale1 = t;
00131   }
00132   else
00133   {
00134     Scalar sinTheta = std::sin(theta);
00135     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00136     scale1 = internal::sin( ( t * theta) ) / sinTheta;
00137     if (d<0)
00138       scale1 = -scale1;
00139   }
00140 
00141   return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
00142 }
00143 
00144 int main()
00145 {
00146   typedef double RefScalar;
00147   typedef float TestScalar;
00148   
00149   typedef Quaternion<RefScalar>  Qd;
00150   typedef Quaternion<TestScalar> Qf;
00151   
00152   unsigned int g_seed = (unsigned int) time(NULL);
00153   std::cout << g_seed << "\n";
00154 //   g_seed = 1259932496;
00155   srand(g_seed);
00156   
00157   Matrix<RefScalar,Dynamic,1> maxerr(7);
00158   maxerr.setZero();
00159   
00160   Matrix<RefScalar,Dynamic,1> avgerr(7);
00161   avgerr.setZero();
00162   
00163   cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";
00164   
00165   int rep = 100;
00166   int iters = 40;
00167   for (int w=0; w<rep; ++w)
00168   {
00169     Qf a, b;
00170     a.coeffs().setRandom();
00171     a.normalize();
00172     b.coeffs().setRandom();
00173     b.normalize();
00174     
00175     Qf c[6];
00176     
00177     Qd ar(a.cast<RefScalar>());
00178     Qd br(b.cast<RefScalar>());
00179     Qd cr;
00180     
00181     
00182     
00183     cout.precision(8);
00184     cout << std::scientific;
00185     for (int i=0; i<iters; ++i)
00186     {
00187       RefScalar t = 0.65;
00188       cr = slerp_rw(ar,br,t);
00189       
00190       Qf refc = cr.cast<TestScalar>();
00191       c[0] = nlerp(a,b,t);
00192       c[1] = slerp_eigen(a,b,t);
00193       c[2] = slerp_legacy(a,b,t);
00194       c[3] = slerp_legacy_nlerp(a,b,t);
00195       c[4] = slerp_rw(a,b,t);
00196       c[5] = slerp_gael(a,b,t);
00197       
00198       VectorXd err(7);
00199       err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
00200 //       std::cout << err[0] << "    ";
00201       for (int k=0; k<6; ++k)
00202       {
00203         err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
00204 //         std::cout << err[k+1] << "    ";
00205       }
00206       maxerr = maxerr.cwise().max(err);
00207       avgerr += err;
00208 //       std::cout << "\n";
00209       b = cr.cast<TestScalar>();
00210       br = cr;
00211     }
00212 //     std::cout << "\n";
00213   }
00214   avgerr /= RefScalar(rep*iters);
00215   cout << "\n\nAccuracy:\n"
00216        << "  max: " << maxerr.transpose() << "\n";
00217   cout << "  avg: " << avgerr.transpose() << "\n";
00218   
00219   // perf bench
00220   Quaternionf a,b;
00221   a.coeffs().setRandom();
00222   a.normalize();
00223   b.coeffs().setRandom();
00224   b.normalize();
00225   //b = a;
00226   float s = 0.65;
00227     
00228   #define BENCH(FUNC) {\
00229     BenchTimer t; \
00230     for(int k=0; k<2; ++k) {\
00231       t.start(); \
00232       for(int i=0; i<1000000; ++i) \
00233         FUNC(a,b,s); \
00234       t.stop(); \
00235     } \
00236     cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
00237   }
00238   
00239   cout << "\nSpeed:\n" << std::fixed;
00240   BENCH(nlerp);
00241   BENCH(slerp_eigen);
00242   BENCH(slerp_legacy);
00243   BENCH(slerp_legacy_nlerp);
00244   BENCH(slerp_rw);
00245   BENCH(slerp_gael);
00246 }
00247 


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:33:15