Loading...
Searching...
No Matches
Quaternion.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#include <limits>
#include <iostream>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
namespace gtsam {
// Define traits
template<typename _Scalar, int _Options>
struct traits<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q;
typedef lie_group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
static Q Identity() {
return Q::Identity();
}
enum {
dimension = 3
};
typedef OptionalJacobian<3, 3> ChartJacobian;
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
static Q Compose(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
if (Hg) *Hg = h.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return g * h;
}
static Q Between(const Q &g, const Q & h,
ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
Q d = g.inverse() * h;
if (Hg) *Hg = -d.toRotationMatrix().transpose();
if (Hh) *Hh = I_3x3;
return d;
}
static Q Inverse(const Q &g,
ChartJacobian H = boost::none) {
if (H) *H = -g.toRotationMatrix();
return g.inverse();
}
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
using std::cos;
using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
_Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega;
return Q(cos(ha), vec.x(), vec.y(), vec.z());
} else {
// first order approximation sin(theta/2)/theta = 0.5
Vector3 vec = _Scalar(0.5) * omega;
return Q(1.0, vec.x(), vec.y(), vec.z());
}
}
static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
using std::acos;
using std::sqrt;
// define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
TangentVector omega;
const _Scalar qw = q.w();
// See Quaternion-Logmap.nb in doc for Taylor expansions
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
// (2 + 2 * (1-qw) / 3) * q.vec();
omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1
// (-2 - 2 * (1 + qw) / 3) * q.vec();
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else {
// Normal, away from zero case
if (qw > 0) {
_Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
omega = (angle / s) * q.vec();
} else {
// Make sure that we are using a canonical quaternion with w > 0
_Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
omega = (angle / s) * -q.vec();
}
}
if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
return omega;
}
static TangentVector Local(const Q& g, const Q& h,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
Q b = Between(g, h, H1, H2);
Matrix3 D_v_b;
TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
if (H1) *H1 = D_v_b * (*H1);
if (H2) *H2 = D_v_b * (*H2);
return v;
}
static Q Retract(const Q& g, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
Matrix3 D_h_v;
Q b = Expmap(v,H2 ? &D_h_v : 0);
Q h = Compose(g, b, H1, H2);
if (H2) *H2 = (*H2) * D_h_v;
return h;
}
static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0)
std::cout << "Eigen::Quaternion: ";
else
std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl;
}
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
}
};
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
} // \namespace gtsam
Lie.h
Base class and basic functions for Lie types.
SO3.h
3*3 matrix representation of SO(3)
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
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::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::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)