gtsam 4.1.1 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
28namespace gtsam {
29
30// Define traits
31template<typename _Scalar, int _Options>
32struct 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
172typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
173
174} // \namespace gtsam
175
Base class and basic functions for Lie types.
3*3 matrix representation of SO(3)
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Group operator syntax flavors.
Definition: Group.h:37
tag to assert a type is a Lie group
Definition: Lie.h:164
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
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
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
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82