gtsam  4.1.0
gtsam
Quaternion.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
20 #include <gtsam/base/Lie.h>
21 #include <gtsam/base/concepts.h>
22 #include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
23 #include <limits>
24 #include <iostream>
25 
26 #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
27 
28 namespace gtsam {
29 
30 // Define traits
31 template<typename _Scalar, int _Options>
32 struct traits<QUATERNION_TYPE> {
33  typedef QUATERNION_TYPE ManifoldType;
34  typedef QUATERNION_TYPE Q;
35 
38 
41  static Q Identity() {
42  return Q::Identity();
43  }
44 
48  enum {
49  dimension = 3
50  };
52  typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
53 
57  static Q Compose(const Q &g, const Q & h,
58  ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
59  if (Hg) *Hg = h.toRotationMatrix().transpose();
60  if (Hh) *Hh = I_3x3;
61  return g * h;
62  }
63 
64  static Q Between(const Q &g, const Q & h,
65  ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
66  Q d = g.inverse() * h;
67  if (Hg) *Hg = -d.toRotationMatrix().transpose();
68  if (Hh) *Hh = I_3x3;
69  return d;
70  }
71 
72  static Q Inverse(const Q &g,
73  ChartJacobian H = boost::none) {
74  if (H) *H = -g.toRotationMatrix();
75  return g.inverse();
76  }
77 
79  static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
80  ChartJacobian H = boost::none) {
81  using std::cos;
82  using std::sin;
83  if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
84  _Scalar theta2 = omega.dot(omega);
85  if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
86  _Scalar theta = std::sqrt(theta2);
87  _Scalar ha = _Scalar(0.5) * theta;
88  Vector3 vec = (sin(ha) / theta) * omega;
89  return Q(cos(ha), vec.x(), vec.y(), vec.z());
90  } else {
91  // first order approximation sin(theta/2)/theta = 0.5
92  Vector3 vec = _Scalar(0.5) * omega;
93  return Q(1.0, vec.x(), vec.y(), vec.z());
94  }
95  }
96 
98  static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
99  using std::acos;
100  using std::sqrt;
101 
102  // define these compile time constants to avoid std::abs:
103  static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
104  NearlyNegativeOne = -1.0 + 1e-10;
105 
106  TangentVector omega;
107 
108  const _Scalar qw = q.w();
109  // See Quaternion-Logmap.nb in doc for Taylor expansions
110  if (qw > NearlyOne) {
111  // Taylor expansion of (angle / s) at 1
112  // (2 + 2 * (1-qw) / 3) * q.vec();
113  omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
114  } else if (qw < NearlyNegativeOne) {
115  // Taylor expansion of (angle / s) at -1
116  // (-2 - 2 * (1 + qw) / 3) * q.vec();
117  omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
118  } else {
119  // Normal, away from zero case
120  _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
121  // Important: convert to [-pi,pi] to keep error continuous
122  if (angle > M_PI)
123  angle -= twoPi;
124  else if (angle < -M_PI)
125  angle += twoPi;
126  omega = (angle / s) * q.vec();
127  }
128 
129  if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
130  return omega;
131  }
132 
136 
137  static TangentVector Local(const Q& g, const Q& h,
138  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
139  Q b = Between(g, h, H1, H2);
140  Matrix3 D_v_b;
141  TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
142  if (H1) *H1 = D_v_b * (*H1);
143  if (H2) *H2 = D_v_b * (*H2);
144  return v;
145  }
146 
147  static Q Retract(const Q& g, const TangentVector& v,
148  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
149  Matrix3 D_h_v;
150  Q b = Expmap(v,H2 ? &D_h_v : 0);
151  Q h = Compose(g, b, H1, H2);
152  if (H2) *H2 = (*H2) * D_h_v;
153  return h;
154  }
155 
159  static void Print(const Q& q, const std::string& str = "") {
160  if (str.size() == 0)
161  std::cout << "Eigen::Quaternion: ";
162  else
163  std::cout << str << " ";
164  std::cout << q.vec().transpose() << std::endl;
165  }
166  static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
167  return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
168  }
170 };
171 
172 typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
173 
174 } // \namespace gtsam
175 
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:84
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition: Lie.h:163
gtsam::traits< QUATERNION_TYPE >::Logmap
static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none)
We use our own Logmap, as there is a slight bug in Eigen.
Definition: Quaternion.h:98
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition: Group.h:37
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::traits< QUATERNION_TYPE >::Expmap
static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian H=boost::none)
Exponential map, using the inlined code from Eigen's conversion from axis/angle.
Definition: Quaternion.h:79
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:74
SO3.h
3*3 matrix representation of SO(3)
Lie.h
Base class and basic functions for Lie types.