• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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>
nlerp(const Q & a,const Q & b,typename Q::Scalar t)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>
slerp_eigen(const Q & a,const Q & b,typename Q::Scalar t)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>
slerp_legacy(const Q & a,const Q & b,typename Q::Scalar t)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>
slerp_legacy_nlerp(const Q & a,const Q & b,typename Q::Scalar t)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>
sin_over_x(T x)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>
slerp_rw(const Q & a,const Q & b,typename Q::Scalar t)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>
slerp_gael(const Q & a,const Q & b,typename Q::Scalar t)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 
main()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 
248