diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h new file mode 100644 index 000000000..dce47beca --- /dev/null +++ b/gtsam/geometry/Quaternion.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 Unit tests for unit quaternions + * @author Frank Dellaert + **/ + +#include + +namespace gtsam { + +namespace traits { + +/// Define Eigen::Quaternion to be a model of the Lie Group concept +template +struct structure_category > { + typedef lie_group_tag type; +}; + +} // \namespace gtsam::traits + +namespace manifold { + +/// Chart for Eigen Quaternions +template +struct QuaternionChart { + + // required + typedef Eigen::Quaternion ManifoldType; + + // internal + typedef ManifoldType Q; + typedef typename traits::TangentVector::type Omega; + + /// Exponential map, simply be converting omega to AngleAxis + static Q Expmap(const Omega& omega) { + double theta = omega.norm(); + if (std::abs(theta) < 1e-10) + return Q::Identity(); + return Q(Eigen::AngleAxisd(theta, omega / theta)); + } + + /// retract, simply be converting omega to AngleAxis + static Q Retract(const Q& p, const Omega& omega) { + return p * Expmap(omega); + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static Omega Logmap(const Q& q) { + 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 * (qw - 1) / 3) * q.vec(); + } else if (qw < NearlyNegativeOne) { + // Angle is zero, return zero vector + return Vector3::Zero(); + } 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(); + } + } + + /// local is our own, as there is a slight bug in Eigen + static Omega Local(const Q& q1, const Q& q2) { + return Logmap(q1.inverse() * q2); + } +}; + +namespace traits { + +/// Define the trait that asserts Quaternion manifold has dimension 3 +template +struct dimension > : public boost::integral_constant< + int, 3> { +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct TangentVector > { + typedef Eigen::Matrix type; +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct DefaultChart > { + typedef QuaternionChart type; +}; + +} // \namespace gtsam::manifold::traits +} // \namespace gtsam::manifold + +GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) + +} // \namespace gtsam + +/** + * GSAM typedef to an Eigen::Quaternion, we disable alignment because + * geometry objects are stored in boost pool allocators, in Values containers, + * and and these pool allocators do not support alignment. + */ +typedef Eigen::Quaternion Quaternion; + diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index eaef53230..534fe155a 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -10,127 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file Quaternion.h - * @brief Unit tests for unit quaternions + * @file testQuaternion.cpp + * @brief Unit tests for Quaternion, as a GTSAM-adapted Lie Group * @author Frank Dellaert **/ -#include - -namespace gtsam { - -namespace traits { - -/// Define Eigen::Quaternion to be a model of the Lie Group concept -template -struct structure_category > { - typedef lie_group_tag type; -}; - -} // \namespace gtsam::traits - -namespace manifold { - -/// Chart for Eigen Quaternions -template -struct QuaternionChart { - - // required - typedef Eigen::Quaternion ManifoldType; - - // internal - typedef ManifoldType Q; - typedef typename traits::TangentVector::type Omega; - - /// Exponential map, simply be converting omega to AngleAxis - static Q Expmap(const Omega& omega) { - double theta = omega.norm(); - if (std::abs(theta) < 1e-10) - return Q::Identity(); - return Q(Eigen::AngleAxisd(theta, omega / theta)); - } - - /// retract, simply be converting omega to AngleAxis - static Q Retract(const Q& p, const Omega& omega) { - return p * Expmap(omega); - } - - /// We use our own Logmap, as there is a slight bug in Eigen - static Omega Logmap(const Q& q) { - 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 * (qw - 1) / 3) * q.vec(); - } else if (qw < NearlyNegativeOne) { - // Angle is zero, return zero vector - return Vector3::Zero(); - } 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(); - } - } - - /// local is our own, as there is a slight bug in Eigen - static Omega Local(const Q& q1, const Q& q2) { - return Logmap(q1.inverse() * q2); - } -}; - -namespace traits { - -/// Define the trait that asserts Quaternion manifold has dimension 3 -template -struct dimension > : public boost::integral_constant< - int, 3> { -}; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct TangentVector > { - typedef Eigen::Matrix type; -}; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct DefaultChart > { - typedef QuaternionChart type; -}; - -} // \namespace gtsam::manifold::traits -} // \namespace gtsam::manifold - -GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) - -} // \namespace gtsam - -/** - * GSAM typedef to an Eigen::Quaternion, we disable alignment because - * geometry objects are stored in boost pool allocators, in Values containers, - * and and these pool allocators do not support alignment. - */ -typedef Eigen::Quaternion Quaternion; - -/** - * @file testCyclic.cpp - * @brief Unit tests for cyclic group - * @author Frank Dellaert - **/ - -//#include -#include -#include +#include #include using namespace std;