12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Retract works

release/4.3a0
dellaert 2014-12-07 12:35:17 +01:00
parent 8db8cb54b0
commit 36da8702f9
1 changed files with 46 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -33,9 +33,19 @@ namespace manifold {
/// Chart for Eigen Quaternions /// Chart for Eigen Quaternions
template<typename S, int O> template<typename S, int O>
class QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>, struct QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
QuaternionChart<S, O> > { QuaternionChart<S, O> > {
typedef Eigen::Quaternion<S, O> Q;
typedef typename traits::TangentVector<Q>::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 { namespace traits {
@ -49,13 +59,13 @@ struct dimension<Eigen::Quaternion<S, O> > : public boost::integral_constant<
/// Define the trait that asserts Quaternion TangentVector is Vector3 /// Define the trait that asserts Quaternion TangentVector is Vector3
template<typename S, int O> template<typename S, int O>
struct TangentVector<Eigen::Quaternion<S, O> > { struct TangentVector<Eigen::Quaternion<S, O> > {
typedef Eigen::Matrix<S,3,1,O,3,1> type; typedef Eigen::Matrix<S, 3, 1, O, 3, 1> type;
}; };
/// Define the trait that asserts Quaternion TangentVector is Vector3 /// Define the trait that asserts Quaternion TangentVector is Vector3
template<typename S, int O> template<typename S, int O>
struct DefaultChart<Eigen::Quaternion<S, O> > { struct DefaultChart<Eigen::Quaternion<S, O> > {
typedef QuaternionChart<S,O> type; typedef QuaternionChart<S, O> type;
}; };
} // \namespace gtsam::manifold::traits } // \namespace gtsam::manifold::traits
@ -129,21 +139,45 @@ typedef Quaternion Q; // Typedef
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Concept) { TEST(Quaternion , Concept) {
// BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >)); BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >)); // not strictly needed
// BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >)); BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >)); // not strictly needed
// BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >)); BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
} }
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Constructor) { TEST(Quaternion , Constructor) {
Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
} }
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Invariants) { TEST(Quaternion , Invariants) {
Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
// group::check_invariants(g,h); Does not satisfy Testable concept (yet!) // 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<Q>::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<Q>::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) {
} }
//****************************************************************************** //******************************************************************************