diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index ab4e589b4..f2ef91ad3 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -278,6 +278,29 @@ private: T g, h; }; +/** + * A Lie Group Chart + * Creates Local/Retract from exponential map and its inverse + * Assumes Expmap and Logmap defined in Derived + * TODO: Can we do this with a single Derived argument ? + */ +template +struct LieGroupChart { + + /// retract, composes with Exmpap around identity + static T Retract(const T& p, const TangentVector& omega) { + // TODO needs to be manifold::compose, with derivatives + return group::compose(p, Derived::Expmap(omega)); + } + + /// local is our own, as there is a slight bug in Eigen + static TangentVector Local(const T& q1, const T& q2) { + // TODO needs to be manifold::between, with derivatives + return Derived::Logmap(group::between(q1, q2)); + } + +}; + template class IsVectorSpace: public IsLieGroup { public: diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 8e6437541..bb27b1d0b 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,10 @@ namespace gtsam { /// Chart for Eigen Quaternions template -struct MakeQuaternionChart { +struct MakeQuaternionChart: LieGroupChart< + MakeQuaternionChart<_Scalar, _Options>, + Eigen::Quaternion<_Scalar, _Options>, + Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> > { // required typedef Eigen::Quaternion<_Scalar, _Options> ManifoldType; diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2c23a7f1d..a183c5792 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -21,7 +21,7 @@ namespace gtsam { -SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) { +SO3 Rodrigues(const double& theta, const Vector3& axis) { using std::cos; using std::sin; @@ -44,6 +44,16 @@ SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) { return R; } +/// simply convert omega to axis/angle representation +SO3 SO3Chart::Expmap(const Eigen::Ref& omega) { + if (omega.isZero()) + return SO3::Identity(); + else { + double angle = omega.norm(); + return Rodrigues(angle, omega / angle); + } +} + Vector3 SO3Chart::Logmap(const SO3& R) { // note switch to base 1 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 4bfa80754..ff290f995 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -11,7 +11,7 @@ /** * @file SO3.h - * @brief 3*3 matrix representation o SO(3) + * @brief 3*3 matrix representation of SO(3) * @author Frank Dellaert * @date December 2014 */ @@ -48,37 +48,18 @@ public: } }; -/// Default Chart for SO3 -struct SO3Chart { +/** + * Chart for SO3 comprising of exponential map and its inverse (log-map) + */ +struct SO3Chart: LieGroupChart { typedef SO3 ManifoldType; - /// Exponential map given axis/angle representation of Lie algebra - static SO3 Expmap(const double& theta, const Vector3& w); - - /// Exponential map, simply be converting omega to axis/angle representation - static SO3 Expmap(const Eigen::Ref& omega) { - if (omega.isZero()) - return SO3::Identity(); - else { - double angle = omega.norm(); - return Expmap(angle, omega / angle); - } - } - - /// retract, simply be converting omega to AngleAxis - static SO3 Retract(const SO3& p, const Eigen::Ref& omega) { - return p * Expmap(omega); - } + /// Exponential map + static SO3 Expmap(const Eigen::Ref& omega); /// We use our own Logmap, as there is a slight bug in Eigen static Vector3 Logmap(const SO3& R); - - /// local is our own, as there is a slight bug in Eigen - static Vector3 Local(const SO3& q1, const SO3& q2) { - return Logmap(q1.inverse() * q2); - } - }; #define SO3_TEMPLATE