diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 22a96f4a3..8c8f9354f 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -33,9 +33,19 @@ namespace manifold { /// Chart for Eigen Quaternions template -class QuaternionChart: public manifold::Chart, +struct QuaternionChart: public manifold::Chart, QuaternionChart > { - + typedef Eigen::Quaternion Q; + typedef typename traits::TangentVector::type V; + static V local(const Q& p, const Q& q) { + return V(); + } + static Q retract(const Q& p, const V& v) { + double theta = v.norm(); + if (std::abs(theta) < 1e-10) + return p; + return p * Q(Eigen::AngleAxisd(theta, v / theta)); + } }; namespace traits { @@ -49,13 +59,13 @@ struct dimension > : public boost::integral_constant< /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct TangentVector > { - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; }; /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct DefaultChart > { - typedef QuaternionChart type; + typedef QuaternionChart type; }; } // \namespace gtsam::manifold::traits @@ -129,21 +139,45 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - // BOOST_CONCEPT_ASSERT((IsGroup)); - // BOOST_CONCEPT_ASSERT((IsManifold)); - // BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsManifold)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** TEST(Quaternion , Constructor) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** TEST(Quaternion , Invariants) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); - Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - // group::check_invariants(g,h); Does not satisfy Testable concept (yet!) + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) +} + +//****************************************************************************** +TEST(Quaternion , Local) { + Vector3 z_axis(0,0,1); + Q q1(Eigen::AngleAxisd(0,z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 expected(0,0,0.1); + Vector3 actual = Chart::Local(q1,q2); + cout << expected << endl; + cout << actual << endl; + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(Quaternion , Retract) { + Vector3 z_axis(0,0,1); + Q q(Eigen::AngleAxisd(0,z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 v(0,0,0.1); + Q actual = Chart::Retract(q,v); + EXPECT(actual.isApprox(expected)); } //****************************************************************************** @@ -155,7 +189,7 @@ TEST(Quaternion , Between) { } //****************************************************************************** -TEST(Quaternion , Ivnverse) { +TEST(Quaternion , Inverse) { } //******************************************************************************