geometry/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>
34  typedef QUATERNION_TYPE Q;
35 
38 
41  static Q Identity() {
42  return Q::Identity();
43  }
44 
48  enum {
49  dimension = 3
50  };
53 
57  static Q Compose(const Q &g, const Q & h,
58  ChartJacobian Hg = {}, ChartJacobian Hh = {}) {
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 = {}, ChartJacobian Hh = {}) {
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 = {}) {
74  if (H) *H = -g.toRotationMatrix();
75  return g.inverse();
76  }
77 
80  ChartJacobian H = {}) {
81  using std::cos;
82  using std::sin;
83  if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
84  _Scalar theta2 = omega.dot(omega);
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 = {}) {
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  if (qw > 0) {
121  _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
122  // Important: convert to [-pi,pi] to keep error continuous
123  if (angle > M_PI)
124  angle -= twoPi;
125  else if (angle < -M_PI)
126  angle += twoPi;
127  omega = (angle / s) * q.vec();
128  } else {
129  // Make sure that we are using a canonical quaternion with w > 0
130  _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
131  if (angle > M_PI)
132  angle -= twoPi;
133  else if (angle < -M_PI)
134  angle += twoPi;
135  omega = (angle / s) * -q.vec();
136  }
137  }
138 
139  if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
140  return omega;
141  }
142 
146 
147  static TangentVector Local(const Q& g, const Q& h,
148  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
149  Q b = Between(g, h, H1, H2);
150  Matrix3 D_v_b;
151  TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
152  if (H1) *H1 = D_v_b * (*H1);
153  if (H2) *H2 = D_v_b * (*H2);
154  return v;
155  }
156 
157  static Q Retract(const Q& g, const TangentVector& v,
158  ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
159  Matrix3 D_h_v;
160  Q b = Expmap(v,H2 ? &D_h_v : 0);
161  Q h = Compose(g, b, H1, H2);
162  if (H2) *H2 = (*H2) * D_h_v;
163  return h;
164  }
165 
169  static void Print(const Q& q, const std::string& str = "") {
170  if (str.size() == 0)
171  std::cout << "Eigen::Quaternion: ";
172  else
173  std::cout << str << " ";
174  std::cout << q.vec().transpose() << std::endl;
175  }
176  static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
177  return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
178  }
180 };
181 
183 
184 } // \namespace gtsam
185 
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Quaternion Q
Eigen::Vector3d Vector3
Definition: Vector.h:43
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian H={})
Exponential map, using the inlined code from Eigen&#39;s conversion from axis/angle.
tag to assert a type is a Lie group
Definition: Lie.h:164
OptionalJacobian< 3, 3 > ChartJacobian
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82
#define M_PI
Definition: main.h:106
Jet< T, N > acos(const Jet< T, N > &f)
Definition: jet.h:432
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose2_ Expmap(const Vector3_ &xi)
Eigen::Matrix< _Scalar, 3, 1, _Options, 3, 1 > TangentVector
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
3*3 matrix representation of SO(3)
void g(const string &key, int i)
Definition: testBTree.cpp:41
static TangentVector Local(const Q &g, const Q &h, ChartJacobian H1={}, ChartJacobian H2={})
static double epsilon
Definition: testRot3.cpp:37
static void Print(const Q &q, const std::string &str="")
Definition: pytypes.h:1403
Group operator syntax flavors.
Definition: Group.h:40
static Q Compose(const Q &g, const Q &h, ChartJacobian Hg={}, ChartJacobian Hh={})
Array< int, Dynamic, 1 > v
BetweenFactor< Rot3 > Between
static bool Equals(const Q &q1, const Q &q2, double tol=1e-8)
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
static Q Between(const Q &g, const Q &h, ChartJacobian Hg={}, ChartJacobian Hh={})
const G & b
Definition: Group.h:86
Base class and basic functions for Lie types.
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
traits
Definition: chartTesting.h:28
const double h
multiplicative_group_tag group_flavor
The quaternion class used to represent 3D orientations and rotations.
static Q Inverse(const Q &g, ChartJacobian H={})
static Q Retract(const Q &g, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
#define QUATERNION_TYPE
static TangentVector Logmap(const Q &q, ChartJacobian H={})
We use our own Logmap, as there is a slight bug in Eigen.
The matrix class, also used for vectors and row-vectors.
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:29