125 lines
4.0 KiB
C++
125 lines
4.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file Quaternion.h
|
|
* @brief Lie Group wrapper for Eigen Quaternions
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include <gtsam/base/concepts.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
|
|
#define QUATERNION_TEMPLATE typename _Scalar, int _Options
|
|
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
|
|
|
|
namespace gtsam {
|
|
|
|
// Define group traits
|
|
template<QUATERNION_TEMPLATE>
|
|
struct traits_x<QUATERNION_TYPE> {
|
|
typedef QUATERNION_TYPE ManifoldType;
|
|
typedef QUATERNION_TYPE Q;
|
|
typedef lie_group_tag structure_category;
|
|
|
|
static const Q identity = Q::Identity();
|
|
|
|
// Define manifold traits
|
|
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
|
|
typedef OptionalJacobian<3, 3> ChartJacobian;
|
|
|
|
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) {
|
|
if (Hg) {
|
|
//TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
|
|
*Hg = h.toRotationMatrix().transpose();
|
|
}
|
|
if (Hh) {
|
|
//TODO : check Jacobian consistent with chart ( I(3)? )
|
|
*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) {
|
|
//TODO : check Jacobian consistent with chart
|
|
*Hg = -d.toRotationMatrix().transpose();
|
|
}
|
|
if (Hh) {
|
|
//TODO : check Jacobian consistent with chart (my guess I(3) )
|
|
*Hh = I_3x3;
|
|
}
|
|
return d;
|
|
}
|
|
static Q Inverse(const Q &g, ChartJacobian H=boost::none) {
|
|
if (H) {
|
|
//TODO : check Jacobian consistent with chart
|
|
*H = -g.toRotationMatrix();
|
|
}
|
|
return g.inverse();
|
|
}
|
|
|
|
/// Exponential map, simply be converting omega to axis/angle representation
|
|
// TODO: implement Jacobian
|
|
static Q Expmap(const Eigen::Ref<const TangentVector >& omega, ChartJacobian H=boost::none) {
|
|
if (omega.isZero())
|
|
return Q::Identity();
|
|
else {
|
|
_Scalar angle = omega.norm();
|
|
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
|
}
|
|
}
|
|
|
|
/// We use our own Logmap, as there is a slight bug in Eigen
|
|
// TODO: implement Jacobian
|
|
static TangentVector Logmap(const Q& q, ChartJacobian H=boost::none) {
|
|
using std::acos;
|
|
using std::sqrt;
|
|
static const double twoPi = 2.0 * M_PI,
|
|
// define these compile time constants to avoid std::abs:
|
|
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
|
|
|
|
const double qw = q.w();
|
|
if (qw > NearlyOne) {
|
|
// Taylor expansion of (angle / s) at 1
|
|
//return (2 + 2 * (1-qw) / 3) * q.vec();
|
|
return (8./3. - 2./3. * qw) * q.vec();
|
|
} else if (qw < NearlyNegativeOne) {
|
|
// Taylor expansion of (angle / s) at -1
|
|
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
|
|
return (-8./3 + 2./3 * qw) * q.vec();
|
|
} else {
|
|
// Normal, away from zero case
|
|
double 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;
|
|
return (angle / s) * q.vec();
|
|
}
|
|
}
|
|
|
|
static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none) {
|
|
return Logmap(Between(origin,other,Horigin,Hother));
|
|
// TODO: incorporate Jacobian of Logmap
|
|
}
|
|
static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin, ChartJacobian Hv) {
|
|
return Compose(origin, Expmap(v), Horigin, Hv);
|
|
// TODO : incorporate Jacobian of Expmap
|
|
}
|
|
};
|
|
|
|
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
|
|
|
|
} // \namespace gtsam
|
|
|