ground/gcs/src/libs/eigen/bench/quat_slerp.cpp

   1
   2 #include <iostream>
   3 #include <Eigen/Geometry>
   4 #include <bench/BenchTimer.h>
   5 using namespace Eigen;
   6 using namespace std;
   7
   8
   9
  10 template<typename Q>
  11 EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
  12 {
  13   return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
  14 }
  15
  16 template<typename Q>
  17 EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
  18 {
  19   return a.slerp(t,b);
  20 }
  21
  22 template<typename Q>
  23 EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
  24 {
  25   typedef typename Q::Scalar Scalar;
  26   static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
  27   Scalar d = a.dot(b);
  28   Scalar absD = internal::abs(d);
  29   if (absD>=one)
  30     return a;
  31
  32   // theta is the angle between the 2 quaternions
  33   Scalar theta = std::acos(absD);
  34   Scalar sinTheta = internal::sin(theta);
  35
  36   Scalar scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
  37   Scalar scale1 = internal::sin( ( t * theta) ) / sinTheta;
  38   if (d<0)
  39     scale1 = -scale1;
  40
  41   return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
  42 }
  43
  44 template<typename Q>
  45 EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
  46 {
  47   typedef typename Q::Scalar Scalar;
  48   static const Scalar one = Scalar(1) - epsilon<Scalar>();
  49   Scalar d = a.dot(b);
  50   Scalar absD = internal::abs(d);
  51
  52   Scalar scale0;
  53   Scalar scale1;
  54
  55   if (absD>=one)
  56   {
  57     scale0 = Scalar(1) - t;
  58     scale1 = t;
  59   }
  60   else
  61   {
  62     // theta is the angle between the 2 quaternions
  63     Scalar theta = std::acos(absD);
  64     Scalar sinTheta = internal::sin(theta);
  65
  66     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
  67     scale1 = internal::sin( ( t * theta) ) / sinTheta;
  68     if (d<0)
  69       scale1 = -scale1;
  70   }
  71
  72   return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
  73 }
  74
  75 template<typename T>
  76 inline T sin_over_x(T x)
  77 {
  78   if (T(1) + x*x == T(1))
  79     return T(1);
  80   else
  81     return std::sin(x)/x;
  82 }
  83
  84 template<typename Q>
  85 EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
  86 {
  87   typedef typename Q::Scalar Scalar;
  88
  89   Scalar d = a.dot(b);
  90   Scalar theta;
  91   if (d<0.0)
  92     theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
  93   else
  94     theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
  95
  96   // theta is the angle between the 2 quaternions
  97 //   Scalar theta = std::acos(absD);
  98   Scalar sinOverTheta = sin_over_x(theta);
  99
 100   Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
 101   Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
 102   if (d<0)
 103     scale1 = -scale1;
 104
 105   return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
 106 }
 107
 108 template<typename Q>
 109 EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
 110 {
 111   typedef typename Q::Scalar Scalar;
 112
 113   Scalar d = a.dot(b);
 114   Scalar theta;
 115 //   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
 116 //   if (d<0.0)
 117 //     theta = M_PI-theta;
 118
 119   if (d<0.0)
 120     theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
 121   else
 122     theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
 123
 124
 125   Scalar scale0;
 126   Scalar scale1;
 127   if(theta*theta-Scalar(6)==-Scalar(6))
 128   {
 129     scale0 = Scalar(1) - t;
 130     scale1 = t;
 131   }
 132   else
 133   {
 134     Scalar sinTheta = std::sin(theta);
 135     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
 136     scale1 = internal::sin( ( t * theta) ) / sinTheta;
 137     if (d<0)
 138       scale1 = -scale1;
 139   }
 140
 141   return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
 142 }
 143
 144 int main()
 145 {
 146   typedef double RefScalar;
 147   typedef float TestScalar;
 148
 149   typedef Quaternion<RefScalar>  Qd;
 150   typedef Quaternion<TestScalar> Qf;
 151
 152   unsigned int g_seed = (unsigned int) time(NULL);
 153   std::cout << g_seed << "\n";
 154 //   g_seed = 1259932496;
 155   srand(g_seed);
 156
 157   Matrix<RefScalar,Dynamic,1> maxerr(7);
 158   maxerr.setZero();
 159
 160   Matrix<RefScalar,Dynamic,1> avgerr(7);
 161   avgerr.setZero();
 162
 163   cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";
 164
 165   int rep = 100;
 166   int iters = 40;
 167   for (int w=0; w<rep; ++w)
 168   {
 169     Qf a, b;
 170     a.coeffs().setRandom();
 171     a.normalize();
 172     b.coeffs().setRandom();
 173     b.normalize();
 174
 175     Qf c[6];
 176
 177     Qd ar(a.cast<RefScalar>());
 178     Qd br(b.cast<RefScalar>());
 179     Qd cr;
 180
 181
 182
 183     cout.precision(8);
 184     cout << std::scientific;
 185     for (int i=0; i<iters; ++i)
 186     {
 187       RefScalar t = 0.65;
 188       cr = slerp_rw(ar,br,t);
 189
 190       Qf refc = cr.cast<TestScalar>();
 191       c[0] = nlerp(a,b,t);
 192       c[1] = slerp_eigen(a,b,t);
 193       c[2] = slerp_legacy(a,b,t);
 194       c[3] = slerp_legacy_nlerp(a,b,t);
 195       c[4] = slerp_rw(a,b,t);
 196       c[5] = slerp_gael(a,b,t);
 197
 198       VectorXd err(7);
 199       err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
 200 //       std::cout << err[0] << "    ";
 201       for (int k=0; k<6; ++k)
 202       {
 203         err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
 204 //         std::cout << err[k+1] << "    ";
 205       }
 206       maxerr = maxerr.cwise().max(err);
 207       avgerr += err;
 208 //       std::cout << "\n";
 209       b = cr.cast<TestScalar>();
 210       br = cr;
 211     }
 212 //     std::cout << "\n";
 213   }
 214   avgerr /= RefScalar(rep*iters);
 215   cout << "\n\nAccuracy:\n"
 216        << "  max: " << maxerr.transpose() << "\n";
 217   cout << "  avg: " << avgerr.transpose() << "\n";
 218
 219   // perf bench
 220   Quaternionf a,b;
 221   a.coeffs().setRandom();
 222   a.normalize();
 223   b.coeffs().setRandom();
 224   b.normalize();
 225   //b = a;
 226   float s = 0.65;
 227
 228   #define BENCH(FUNC) {\
 229     BenchTimer t; \
 230     for(int k=0; k<2; ++k) {\
 231       t.start(); \
 232       for(int i=0; i<1000000; ++i) \
 233         FUNC(a,b,s); \
 234       t.stop(); \
 235     } \
 236     cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
 237   }
 238
 239   cout << "\nSpeed:\n" << std::fixed;
 240   BENCH(nlerp);
 241   BENCH(slerp_eigen);
 242   BENCH(slerp_legacy);
 243   BENCH(slerp_legacy_nlerp);
 244   BENCH(slerp_rw);
 245   BENCH(slerp_gael);
 246 }
 247