quat_slerp.cpp
Go to the documentation of this file.
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);
244  BENCH(slerp_rw);
245  BENCH(slerp_gael);
246 }
247 
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
Eigen::g_seed
static unsigned int g_seed
Definition: main.h:170
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
asin
const EIGEN_DEVICE_FUNC AsinReturnType asin() const
Definition: ArrayCwiseUnaryOps.h:311
Q
Quaternion Q
Definition: testQuaternion.cpp:27
ceres::acos
Jet< T, N > acos(const Jet< T, N > &f)
Definition: jet.h:432
main
int main()
Definition: quat_slerp.cpp:144
slerp_legacy
EIGEN_DONT_INLINE Q slerp_legacy(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:23
Eigen::Triplet< double >
time
#define time
Definition: timeAdaptAutoDiff.cpp:31
slerp_eigen
EIGEN_DONT_INLINE Q slerp_eigen(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:17
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
slerp_gael
EIGEN_DONT_INLINE Q slerp_gael(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:109
slerp_legacy_nlerp
EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:45
std
Definition: BFloat16.h:88
BenchTimer.h
nlerp
EIGEN_DONT_INLINE Q nlerp(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:11
Eigen::Quaternion::Scalar
_Scalar Scalar
Definition: 3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h:279
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
abs
#define abs(x)
Definition: datatypes.h:17
Eigen::PlainObjectBase< Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > >::setZero
EIGEN_DEVICE_FUNC Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > & setZero(Index size)
Definition: CwiseNullaryOp.h:562
NULL
#define NULL
Definition: ccolamd.c:609
BENCH
#define BENCH(FUNC)
sin_over_x
T sin_over_x(T x)
Definition: quat_slerp.cpp:76
align_3::t
Point2 t(10, 10)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
slerp_rw
EIGEN_DONT_INLINE Q slerp_rw(const Q &a, const Q &b, typename Q::Scalar t)
Definition: quat_slerp.cpp:85
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
EIGEN_DONT_INLINE
#define EIGEN_DONT_INLINE
Definition: Macros.h:940


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:50