DualQuaternion.hpp
Go to the documentation of this file.
1 #ifndef GRL_DUAL_QUATERNION
2 #define GRL_DUAL_QUATERNION
3 
4 
5 /// @todo use boost::geometry::get?
6 template<typename Vector, typename Quaternion>
7 Quaternion TranslationToDual(const Vector& vT){
8  Quaternion vD;
9  vD.w() = -0.5*( vT(0)*q.x() + vT(1)*q.y() + vT(2)*q.z());
10  vD.x() = 0.5*( vT(0)*q.w() + vT(1)*q.z() - vT(2)*q.y());
11  vD.y() = 0.5*(-vT(0)*q.z() + vT(1)*q.w() + vT(2)*q.x());
12  vD.z() = 0.5*( vT(0)*q.y() - vT(1)*q.x() + vT(2)*q.w());
13 
14 return vD
15 }
16 
17 
18 #ifndef DUALQUAT_HPP
19 #define DUALQUAT_HPP
20 
21 #include <iostream>
22 #include <vector>
23 #include <limits>
24 #include <cmath>
25 
26 #include <assert.h>
27 
28 #define EPS(value_t) (std::numeric_limits<value_t>::epsilon())
29 
30 template <class value_t>
31 struct quat {
32 
33  value_t w, x, y, z;
34 
35  quat(bool e=1) : w(e), x(0), y(0), z(0) {}
36 
37  quat(value_t w_,
38  value_t x_,
39  value_t y_,
40  value_t z_) : w(w_), x(x_) , y(y_), z(z_) {}
41 
42  void print () const {
43  std::cout << "(" << w << "," << x
44  << "," << y << "," << z
45  << ")" << std::endl;
46  }
47 
48  quat<value_t> operator+(const quat<value_t>& other) const {
49  return quat<value_t>(w+other.w, x+other.x,
50  y+other.y, z+other.z);
51  }
52 
54  w += other.w; x += other.x; y += other.y; z += other.z;
55  return *this;
56  }
57 
58  quat<value_t> operator-(const quat<value_t>& other) const {
59  return quat<value_t>(w-other.w, x-other.x,
60  y-other.y, z-other.z);
61  }
62 
64  w -= other.w; x -= other.x; y -= other.y; z -= other.z;
65  return *this;
66  }
67 
68  quat<value_t> operator*(const quat<value_t>& other) const {
69  return quat<value_t>(w*other.w-x*other.x-y*other.y-z*other.z,
70  w*other.x+other.w*x+y*other.z-z*other.y,
71  w*other.y+other.w*y+z*other.x-x*other.z,
72  w*other.z+other.w*z+x*other.y-y*other.x);
73  }
74 
76  *this = (*this)*other;
77  return *this;
78  }
79 
80  quat<value_t> operator*(const value_t alpha) const {
81  return quat<value_t>(w*alpha, x*alpha,
82  y*alpha, z*alpha);
83  }
84 
85  quat<value_t> operator/(const value_t alpha) const {
86  return quat<value_t>(w/alpha, x/alpha,
87  y/alpha, z/alpha);
88  }
89 
90  quat<value_t> N() const {
91  const value_t rho = std::sqrt(w*w+x*x+y*y+z*z);
92  return quat<value_t>(w/rho, x/rho, y/rho, z/rho);
93  }
94 
95  quat<value_t> C() const {
96  return quat<value_t>(w, -x, -y, -z);
97  }
98 
99  quat<value_t> I() const {
100  const value_t rho2 = w*w+x*x+y*y+z*z;
101  return quat<value_t>(w/rho2, -x/rho2, -y/rho2, -z/rho2);
102  }
103 
104  quat<value_t> exp () const {
105 
106  assert(w*w < EPS(value_t));
107 
108  value_t dst = std::sqrt(x*x+y*y+z*z);
109  value_t fac = std::sin(dst)/dst;
110 
111  return quat<value_t>(std::cos(dst), x*fac, y*fac, z*fac);
112  }
113 
114  quat<value_t> numexp (value_t eps = EPS(value_t)) const {
115 
116  quat<value_t> pow;
117  quat<value_t> sum;
118  size_t i = 1;
119 
120  while (pow.dot(pow) > eps) {
121  pow *= (*this)/i++;
122  sum += pow;
123  }
124 
125  return sum;
126  }
127 
128  quat<value_t> log () const {
129 
130  assert(isunit());
131 
132  if ((w*w-1)*(w*w-1) < EPS(value_t))
133  return quat<value_t>(0);
134 
135  const value_t inv = 1.0/std::sqrt(x*x+y*y+z*z);
136  const value_t fac = std::acos(w)*inv;
137 
138  return quat<value_t> (0, x*fac, y*fac, z*fac);
139  }
140 
141  value_t dot (const quat<value_t> other) const {
142  return w*other.w+x*other.x+y*other.y+z*other.z;
143  }
144 
145  value_t eucnorm () const {
146  return dot(*this);
147  }
148 
149  value_t lognorm () const {
150  const auto& generator = log();
151  return generator.dot(generator);
152  }
153 
154  value_t eucdist (const quat<value_t>& other) const {
155 
156  if (dot(other) < 0.0) {
157  const auto& difference = (*this)+other;
158  return difference.dot(difference);
159  }
160 
161  const auto& difference = (*this)-other;
162  return difference.dot(difference);
163  }
164 
165  value_t logdist (const quat<value_t>& other) const {
166  const auto& difference = ((*this)*(other.C())).log();
167  return difference.dot(difference);
168  }
169 
170  bool isunit () const {
171  const value_t residue = w*w+x*x+y*y+z*z-1.0;
172  return residue*residue < EPS(value_t);
173  }
174 };
175 
176 template <class value_t>
177 struct dualquat {
178 
179  value_t w, x, y, z, W, X, Y, Z;
180 
181  dualquat(bool e=true) : w(e), x(0), y(0), z(0),
182  W(0), X(0), Y(0), Z(0) {};
183 
184  dualquat(const value_t w_,
185  const value_t x_,
186  const value_t y_,
187  const value_t z_,
188  const value_t W_,
189  const value_t X_,
190  const value_t Y_,
191  const value_t Z_) : w(w_), x(x_), y(y_), z(z_),
192  W(W_), X(X_), Y(Y_), Z(Z_) {}
193 
194  dualquat(const quat<value_t>& real,
195  const quat<value_t>& dual) : w(real.w), x(real.x),
196  y(real.y), z(real.z),
197  W(dual.w), X(dual.x),
198  Y(dual.y), Z(dual.z) {}
199 
200  quat<value_t> real () const {
201  return quat<value_t>(w, x, y, z);
202  }
203 
204  quat<value_t> dual () const {
205  return quat<value_t>(W, X, Y, Z);
206  }
207 
208  void print () const {
209  std::cout << "(" << w << "," << x
210  << "," << y << "," << z
211  << "," << W << "," << X
212  << "," << Y << "," << Z
213  << ")" << std::endl;
214  }
215 
217  return dualquat<value_t>(w+other.w, x+other.x,
218  y+other.y, z+other.z,
219  W+other.W, X+other.X,
220  Y+other.Y, Z+other.Z);
221  }
222 
224  w += other.w; x += other.x; y += other.y; z += other.z;
225  W += other.W; X += other.X; Y += other.Y; Z += other.Z;
226  return *this;
227  }
228 
230  return dualquat<value_t>(w-other.w, x-other.x,
231  y-other.y, z-other.z,
232  W-other.W, X-other.X,
233  Y-other.Y, Z-other.Z);
234  }
235 
237  w -= other.w; x -= other.x; y -= other.y; z -= other.z;
238  W -= other.W; X -= other.X; Y -= other.Y; Z -= other.Z;
239  return *this;
240  }
241 
242  // TODO: expand this
244 
245  const auto& a = real();
246  const auto& A = dual();
247  const auto& b = other.real();
248  const auto& B = other.dual();
249 
250  return dualquat<value_t>(a*b, (a*B)+(A*b));
251  }
252 
254  *this = (*this)*other;
255  return *this;
256  }
257 
258  dualquat<value_t> operator*(const value_t alpha) const {
259  return dualquat<value_t>(w*alpha, x*alpha,
260  y*alpha, z*alpha,
261  W*alpha, X*alpha,
262  Y*alpha, Z*alpha);
263  }
264 
265  dualquat<value_t> operator/(const value_t alpha) const {
266  return dualquat<value_t>(w/alpha, x/alpha,
267  y/alpha, z/alpha,
268  W/alpha, X/alpha,
269  Y/alpha, Z/alpha);
270  }
271 
273 
274  const value_t qq = w*w+x*x+y*y+z*z+EPS(value_t);
275  const value_t qQ = w*W+x*X+y*Y+z*Z;
276  const value_t invqq = 1.0/qq;
277  const value_t invsq = 1.0/std::sqrt(qq);
278  const value_t alpha = qQ*invqq*invsq;
279 
280  return dualquat<value_t>(w*invsq, x*invsq,
281  y*invsq, z*invsq,
282  W*invsq-w*alpha, X*invsq-x*alpha,
283  Y*invsq-y*alpha, Z*invsq-z*alpha);
284  }
285 
287  return dualquat<value_t>(w, -x, -y, -z, W, -X, -Y, -Z);
288  }
289 
291  return dualquat<value_t>(w, x, y, z, -W, -X, -Y, -Z);
292  }
293 
295 
296  const value_t qq = w*w+x*x+y*y+z*z;
297  const value_t qQ = w*W+x*X+y*Y+z*Z;
298  const value_t invqq = 1.0/qq;
299  const value_t alpha = 2.0*qQ*invqq*invqq;
300 
301  return dualquat<value_t>(w*invqq, -x*invqq,
302  -y*invqq, -z*invqq,
303  W*invqq-w*alpha, -X*invqq-x*alpha,
304  -Y*invqq-y*alpha, -Z*invqq-z*alpha);
305  }
306 
308 
309  assert(w*w < EPS(value_t) && W*W < EPS(value_t));
310 
311  if (x*x+y*y+z*z < EPS(value_t))
312  return dualquat<value_t>(1, 0, 0, 0, 0, X, Y, Z);
313 
314  const value_t theta = 2.0*std::sqrt(x*x+y*y+z*z);
315  const value_t invvv = 2.0/theta;
316  const value_t lx = x*invvv;
317  const value_t ly = y*invvv;
318  const value_t lz = z*invvv;
319 
320  const value_t pitch = 2.0*(lx*X+ly*Y+lz*Z);
321  const value_t mx = (2.0*X-pitch*lx)/theta;
322  const value_t my = (2.0*Y-pitch*ly)/theta;
323  const value_t mz = (2.0*Z-pitch*lz)/theta;
324 
325  assert((lx*mx+ly*my+lz*mz)*(lx*mx+ly*my+lz*mz) < EPS(value_t));
326 
327  const value_t cost2 = std::cos(0.5*theta);
328  const value_t sint2 = std::sin(0.5*theta);
329  const value_t alpha = 0.5*pitch*cost2;
330 
331 
332  return dualquat<value_t> (cost2,
333  sint2*lx,
334  sint2*ly,
335  sint2*lz,
336  -0.5*pitch*sint2,
337  sint2*mx+alpha*lx,
338  sint2*my+alpha*ly,
339  sint2*mz+alpha*lz);
340  }
341 
342  dualquat<value_t> numexp (value_t eps = EPS(value_t)) const {
343 
344  dualquat<value_t> pow;
345  dualquat<value_t> sum;
346  size_t i = 1;
347 
348  while (pow.dot(pow) > eps) {
349  pow *= (*this)/i++;
350  sum += pow;
351  }
352 
353  return sum;
354  }
355 
357 
358  assert(isunit());
359 
360  if ((w*w-1)*(w*w-1) < EPS(value_t))
361  return dualquat<value_t>(0, 0, 0, 0, 0, X, Y, Z);
362 
363  const value_t theta = 2.0*std::acos(w);
364  const value_t invvv = 0.5/std::sqrt(x*x+y*y+z*z);
365  const value_t pitch = -4.0*W*invvv;
366  const value_t alpha = pitch*w;
367 
368  const value_t lx = x*invvv;
369  const value_t ly = y*invvv;
370  const value_t lz = z*invvv;
371  const value_t mx = (X-lx*alpha)*invvv;
372  const value_t my = (Y-ly*alpha)*invvv;
373  const value_t mz = (Z-lz*alpha)*invvv;
374 
375  assert((lx*mx+ly*my+lz*mz)*(lx*mx+ly*my+lz*mz) < EPS(value_t));
376 
377  return dualquat<value_t> (0, theta*lx,
378  theta*ly,
379  theta*lz,
380  0, (pitch*lx+theta*mx),
381  (pitch*ly+theta*my),
382  (pitch*lz+theta*mz));
383 
384  }
385 
387 
388  assert(isunit());
389 
390  const auto& lower = real().log();
391  const auto& upper = dual()*real().C();
392 
393  return dualquat<value_t>(lower, upper);
394  }
395 
396  value_t dot(const dualquat<value_t> other) const {
397  return w*other.w+x*other.x+y*other.y+z*other.z+
398  W*other.W+X*other.X+Y*other.Y+Z*other.Z;
399  }
400 
401  value_t eucnorm () const {
402  return dot(*this);
403  }
404 
405  value_t lognorm () const {
406  const auto& generator = log();
407  return generator.dot(generator);
408  }
409 
410  value_t kinnorm () const {
411  const auto& generator = fakelog();
412  return generator.dot(generator);
413  }
414 
415  value_t eucdist (const dualquat<value_t>& other) const {
416 
417  // TODO: could still be errornous
418  if (dot(other) < 0.0) {
419  const auto& difference = (*this)+other;
420  return difference.dot(difference);
421  }
422 
423  const auto& difference = (*this)-other;
424  return difference.dot(difference);
425  }
426 
427  value_t logdist (const dualquat<value_t>& other) const {
428  const auto& difference = ((*this)*(other.C())).log();
429  return difference.dot(difference);
430  }
431 
432  value_t kindist (const dualquat<value_t>& other) const {
433  const auto& difference = ((*this)*(other.C())).fakelog();
434  return difference.dot(difference);
435  }
436 
437  bool isunit() const {
438 
439  const value_t residue0 = w*w+x*x+y*y+z*z-1.0;
440  const value_t residue1 = w*W+x*X+y*Y+z*Z;
441 
442  return residue0*residue0 < EPS(value_t) &&
443  residue1*residue1 < EPS(value_t);
444  }
445 
446 };
447 
448 
449 namespace average {
450 
451 template <class value_t>
452 quat<value_t> QLA (const std::vector<quat<value_t>>& quats) {
453 
454  quat<value_t> result(0);
455 
456  for (size_t i = 0; i < quats.size(); i++)
457  result += quats[i];
458 
459  return result.N();
460 }
461 
462 template <class value_t>
463 quat<value_t> QIA (const std::vector<quat<value_t>>& quats) {
464 
465  quat<value_t> b = QLA(quats);
466 
467  auto logmean = [&]() {
468  quat<value_t> avg(0);
469  for (size_t i = 0; i < quats.size(); i++)
470  avg += (b.C()*quats[i]).log();
471  return avg/quats.size();
472  };
473 
474  auto x = logmean();
475  auto norm = x.dot(x);
476 
477  for(;;){
478 
479  b *= x.exp();
480  auto xnew = logmean();
481 
482  const auto newnorm = xnew.dot(xnew);
483  if(norm < newnorm || newnorm < EPS(value_t))
484  break;
485  else {
486  x = xnew;
487  norm = newnorm;
488  }
489  }
490 
491  return b;
492 }
493 
494 template <class value_t>
495 quat<value_t> QLB (const std::vector<quat<value_t>>& quats,
496  const std::vector<value_t>& weights) {
497 
498  assert(quats.size() == weights.size());
499 
500  quat<value_t> result(0);
501 
502  for (size_t i = 0; i < quats.size(); i++)
503  result += quats[i]*weights[i];
504 
505  return result.N();
506 }
507 
508 template <class value_t>
509 quat<value_t> QIB (const std::vector<quat<value_t>>& quats,
510  const std::vector<value_t>& weights) {
511 
512 
513  assert(quats.size() == weights.size());
514 
515  quat<value_t> b = QLB(quats, weights);
516 
517  auto logmean = [&]() {
518  quat<value_t> avg(0);
519  for (size_t i = 0; i < quats.size(); i++)
520  avg += ((b.C())*quats[i]).log()*weights[i];
521  return avg;
522  };
523 
524  auto x = logmean();
525  auto norm = x.dot(x);
526 
527  for(;;){
528 
529  b *= x.exp();
530  auto xnew = logmean();
531 
532  const auto newnorm = xnew.dot(xnew);
533  if(norm < newnorm || newnorm < EPS(value_t))
534  break;
535  else {
536  x = xnew;
537  norm = newnorm;
538  }
539  }
540 
541  return b;
542 }
543 
544 template <class value_t>
545 dualquat<value_t> DLA (const std::vector<dualquat<value_t>>& quats) {
546 
547  dualquat<value_t> result(0);
548 
549  for (size_t i = 0; i < quats.size(); i++)
550  result += quats[i];
551 
552  return (result).N();
553 }
554 
555 template <class value_t>
556 dualquat<value_t> DIA (const std::vector<dualquat<value_t>>& quats) {
557 
558  dualquat<value_t> b = DLA(quats);
559 
560  auto logmean = [&]() {
561  dualquat<value_t> avg(0);
562  for (size_t i = 0; i < quats.size(); i++)
563  avg += (b.C()*quats[i]).log();
564  return avg/quats.size();
565  };
566 
567  auto x = logmean();
568  auto norm = x.dot(x);
569 
570  for(;;){
571 
572  b *= x.numexp();
573  auto xnew = logmean();
574 
575  const auto newnorm = xnew.dot(xnew);
576  if(norm < newnorm || newnorm < EPS(value_t))
577  break;
578  else {
579  x = xnew;
580  norm = newnorm;
581  }
582  }
583 
584  // std::cout << "precision: " << norm << std::endl;
585 
586  return b;
587 }
588 
589 template <class value_t>
590 dualquat<value_t> DLB (const std::vector<dualquat<value_t>>& quats,
591  const std::vector<value_t>& weights) {
592 
593  assert(quats.size() == weights.size());
594 
595  dualquat<value_t> result(0);
596 
597  for (size_t i = 0; i < quats.size(); i++)
598  result += quats[i]*weights[i];
599 
600  return result.N();
601 }
602 
603 template <class value_t>
604 dualquat<value_t> DIB (const std::vector<dualquat<value_t>>& quats,
605  const std::vector<value_t>& weights) {
606 
607 
608  assert(quats.size() == weights.size());
609 
610  dualquat<value_t> b = DLB(quats, weights);
611 
612  auto logmean = [&]() {
613  dualquat<value_t> avg(0);
614  for (size_t i = 0; i < quats.size(); i++)
615  avg += ((b.C())*quats[i]).log()*weights[i];
616  return avg;
617  };
618 
619  auto x = logmean();
620  auto norm = x.dot(x);
621 
622  for(;;){
623 
624  b *= x.numexp();
625  auto xnew = logmean();
626 
627  const auto newnorm = xnew.dot(xnew);
628  if(norm < newnorm || newnorm < EPS(value_t))
629  break;
630  else {
631  x = xnew;
632  norm = newnorm;
633  }
634 
635  }
636 
637  // std::cout << "precision: " << norm << std::endl;
638 
639  return b;
640 }
641 
642 
643 }
644 #endif
645 
646 
647 
648 #endif
dualquat< value_t > numexp(value_t eps=(std::numeric_limits< value_t >::epsilon())) const
void print() const
dualquat< value_t > D() const
value_t eucnorm() const
dualquat< value_t > & operator+=(const dualquat< value_t > &other)
quat< value_t > & operator-=(const quat< value_t > &other)
value_t eucdist(const quat< value_t > &other) const
quat< value_t > log() const
dualquat< value_t > N() const
dualquat< value_t > I() const
quat< value_t > operator/(const value_t alpha) const
Quaternion TranslationToDual(const Vector &vT)
quat< value_t > & operator*=(const quat< value_t > &other)
value_t lognorm() const
quat< value_t > operator-(const quat< value_t > &other) const
value_t dot(const dualquat< value_t > other) const
dualquat< value_t > exp() const
dualquat< value_t > operator+(const dualquat< value_t > &other) const
value_t dot(const quat< value_t > other) const
quat< value_t > & operator+=(const quat< value_t > &other)
value_t y
quat< value_t > C() const
quat< value_t > numexp(value_t eps=(std::numeric_limits< value_t >::epsilon())) const
quat< value_t > I() const
quat< value_t > QIA(const std::vector< quat< value_t >> &quats)
value_t w
dualquat< value_t > C() const
value_t z
#define EPS(value_t)
value_t logdist(const quat< value_t > &other) const
dualquat< value_t > operator*(const dualquat< value_t > &other) const
dualquat(bool e=true)
dualquat< value_t > fakelog() const
dualquat< value_t > log() const
dualquat< value_t > DIA(const std::vector< dualquat< value_t >> &quats)
value_t kindist(const dualquat< value_t > &other) const
value_t eucdist(const dualquat< value_t > &other) const
dualquat< value_t > & operator-=(const dualquat< value_t > &other)
value_t kinnorm() const
quat< value_t > operator*(const quat< value_t > &other) const
dualquat< value_t > DLA(const std::vector< dualquat< value_t >> &quats)
quat< value_t > dual() const
value_t logdist(const dualquat< value_t > &other) const
dualquat< value_t > operator*(const value_t alpha) const
dualquat< value_t > DIB(const std::vector< dualquat< value_t >> &quats, const std::vector< value_t > &weights)
bool isunit() const
quat< value_t > exp() const
bool isunit() const
dualquat< value_t > operator-(const dualquat< value_t > &other) const
dualquat< value_t > operator/(const value_t alpha) const
value_t eucnorm() const
quat< value_t > N() const
quat< value_t > QLA(const std::vector< quat< value_t >> &quats)
value_t x
quat< value_t > QIB(const std::vector< quat< value_t >> &quats, const std::vector< value_t > &weights)
quat(bool e=1)
void print() const
value_t lognorm() const
quat< value_t > operator+(const quat< value_t > &other) const
dualquat(const quat< value_t > &real, const quat< value_t > &dual)
quat< value_t > real() const
dualquat< value_t > DLB(const std::vector< dualquat< value_t >> &quats, const std::vector< value_t > &weights)
dualquat(const value_t w_, const value_t x_, const value_t y_, const value_t z_, const value_t W_, const value_t X_, const value_t Y_, const value_t Z_)
quat(value_t w_, value_t x_, value_t y_, value_t z_)
quat< value_t > QLB(const std::vector< quat< value_t >> &quats, const std::vector< value_t > &weights)
quat< value_t > operator*(const value_t alpha) const
dualquat< value_t > & operator*=(const dualquat< value_t > &other)