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

fixing little things... far from done

release/4.3a0
Mike Bosse 2014-12-12 21:12:17 +01:00
parent d383b44cc6
commit df8dc1f99c
8 changed files with 58 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/base/concepts.h
View File

@ -66,7 +66,7 @@ struct Manifold {
void Print(const ManifoldType& m) { void Print(const ManifoldType& m) {
m.print(); m.print();
} }
void Equals(const ManifoldType& m1, bool Equals(const ManifoldType& m1,
const ManifoldType& m2, const ManifoldType& m2,
double tol = 1e-8) { double tol = 1e-8) {
return m1.equals(m2, tol); return m1.equals(m2, tol);
@ -116,7 +116,7 @@ struct LieGroup {
void Print(const ManifoldType& m) { void Print(const ManifoldType& m) {
m.print(); m.print();
} }
void Equals(const ManifoldType& m1, bool Equals(const ManifoldType& m1,
const ManifoldType& m2, const ManifoldType& m2,
double tol = 1e-8) { double tol = 1e-8) {
return m1.equals(m2, tol); return m1.equals(m2, tol);
@ -201,6 +201,7 @@ struct LieGroup {
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) {
return ManifoldType::Expmap(v, Hv); return ManifoldType::Expmap(v, Hv);
} }
}; };
} // namespace internal } // namespace internal
@ -256,11 +257,17 @@ public:
// and the versions with Jacobians. // and the versions with Jacobians.
v = traits_x<M>::Local(p,q,Hp,Hq); v = traits_x<M>::Local(p,q,Hp,Hq);
q = traits_x<M>::Retract(p,v,Hp,Hv); q = traits_x<M>::Retract(p,v,Hp,Hv);
traits_x<M>::Print(p);
traits_x<M>::Print(p, "p");
b = traits_x<M>::Equals(p,q);
b = traits_x<M>::Equals(p,q,1e-9);
} }
private: private:
ManifoldType p,q; ManifoldType p,q;
ChartJacobian Hp,Hq,Hv; ChartJacobian Hp,Hq,Hv;
TangentVector v; TangentVector v;
bool b;
}; };
/** /**
@ -277,12 +284,17 @@ public:
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, structure_category_tag>::value), (boost::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)"); "This type's structure_category trait does not assert it as a group (or derived)");
e = traits_x<G>::identity; e = traits_x<G>::Identity();
e = traits_x<G>::Compose(g, h); e = traits_x<G>::Compose(g, h);
e = traits_x<G>::Between(g, h); e = traits_x<G>::Between(g, h);
e = traits_x<G>::Inverse(g); e = traits_x<G>::Inverse(g);
operator_usage(flavor); operator_usage(flavor);
// todo: how do we test the act concept? or do we even need to? // todo: how do we test the act concept? or do we even need to?
traits_x<G>::Print(g);
traits_x<G>::Print(g, "g");
b = traits_x<G>::Equals(g,h);
b = traits_x<G>::Equals(g,h,1e-9);
} }
private: private:
@ -298,14 +310,15 @@ private:
flavor_tag flavor; flavor_tag flavor;
G e, g, h; G e, g, h;
bool b;
}; };
/// Check invariants /// Check invariants
template<typename G> template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) // BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) { check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits_x<G>::identity; G e = traits_x<G>::Identity();
return traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::inverse(a)), e, tol) return traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::Inverse(a)), e, tol)
&& traits_x<G>::Equals(traits_x<G>::Between(a, b), traits_x<G>::Compose(traits_x<G>::Inverse(a), b), tol) && traits_x<G>::Equals(traits_x<G>::Between(a, b), traits_x<G>::Compose(traits_x<G>::Inverse(a), b), tol)
&& traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::Between(a, b)), b, tol); && traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::Between(a, b)), b, tol);
} }

4
gtsam/base/numericalDerivative.h
View File

@ -111,12 +111,12 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) { double delta = 1e-5) {
using namespace traits; using namespace traits;
BOOST_STATIC_ASSERT_MSG( (typename boost::is_same< typename traits_x<X>::structure_category, gtsam::manifold_tag>::value), BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
"Template argument Y must be a manifold type."); "Template argument Y must be a manifold type.");
typedef DefaultChart<Y> ChartY; typedef DefaultChart<Y> ChartY;
typedef typename ChartY::vector TangentY; typedef typename ChartY::vector TangentY;
BOOST_STATIC_ASSERT_MSG( (typename boost::is_same<traits_x<X>::structure_category, gtsam::manifold_tag>::value), BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X>::structure_category>::value),
"Template argument X must be a manifold type."); "Template argument X must be a manifold type.");
static const int N = traits::dimension<X>::value; static const int N = traits::dimension<X>::value;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");

4
gtsam/geometry/Cyclic.h
View File

@ -70,7 +70,9 @@ template <CYCLIC_TEMPLATE>
struct traits_x<CYCLIC_TYPE > { struct traits_x<CYCLIC_TYPE > {
typedef group_tag structure_category; typedef group_tag structure_category;
GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE); GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE);
static const CYCLIC_TYPE identity = CYCLIC_TYPE::Identity(); static CYCLIC_TYPE Identity() { return CYCLIC_TYPE::Identity(); }
static bool Equals(const CYCLIC_TYPE& a, const CYCLIC_TYPE& b, double tol=1e-9) { return a.equals(b,tol); }
static void Print(const CYCLIC_TYPE& c, const std::string &s="") { c.print(s); }
}; };
} // \namespace gtsam } // \namespace gtsam

5
gtsam/geometry/Quaternion.h
View File

@ -29,8 +29,11 @@ struct traits_x<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType; typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q; typedef QUATERNION_TYPE Q;
typedef lie_group_tag structure_category; typedef lie_group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
static const Q identity = Q::Identity(); enum { dimension = 3 };
static Q Identity() { return Q::Identity(); }
// Define manifold traits // Define manifold traits
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;

27
gtsam/geometry/Unit3.h
View File

@ -20,6 +20,7 @@
#pragma once #pragma once
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp> #include <boost/random/mersenne_twister.hpp>
@ -37,6 +38,8 @@ private:
public: public:
enum { dimension = 2 };
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -160,24 +163,14 @@ private:
}; };
// Define GTSAM traits // Define GTSAM traits
namespace traits { template <> struct traits_x<Unit3> : public internal::Manifold<Unit3> {};
template<> //template<>
struct GTSAM_EXPORT is_manifold<Unit3> : public boost::true_type{ //struct GTSAM_EXPORT zero<Unit3> {
}; // static Unit3 value() {
// return Unit3();
template<> // }
struct GTSAM_EXPORT dimension<Unit3> : public boost::integral_constant<int, 2>{ //};
};
template<>
struct GTSAM_EXPORT zero<Unit3> {
static Unit3 value() {
return Unit3();
}
};
}
} // namespace gtsam } // namespace gtsam

34
gtsam/geometry/tests/testCyclic.cpp
View File

@ -27,7 +27,7 @@ typedef Cyclic<3> G; // Let's use the cyclic group of order 3
//****************************************************************************** //******************************************************************************
TEST(Cyclic, Concept) { TEST(Cyclic, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); BOOST_CONCEPT_ASSERT((IsGroup<G>));
EXPECT_LONGS_EQUAL(0, group::traits::identity<G>::value); EXPECT_LONGS_EQUAL(0, traits_x<G>::Identity());
} }
//****************************************************************************** //******************************************************************************
@ -37,37 +37,37 @@ TEST(Cyclic, Constructor) {
//****************************************************************************** //******************************************************************************
TEST(Cyclic, Compose) { TEST(Cyclic, Compose) {
EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); EXPECT_LONGS_EQUAL(0, traits_x<G>::Compose(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); EXPECT_LONGS_EQUAL(1, traits_x<G>::Compose(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); EXPECT_LONGS_EQUAL(2, traits_x<G>::Compose(G(0),G(2)));
EXPECT_LONGS_EQUAL(2, group::compose(G(2),G(0))); EXPECT_LONGS_EQUAL(2, traits_x<G>::Compose(G(2),G(0)));
EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(1))); EXPECT_LONGS_EQUAL(0, traits_x<G>::Compose(G(2),G(1)));
EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(2))); EXPECT_LONGS_EQUAL(1, traits_x<G>::Compose(G(2),G(2)));
} }
//****************************************************************************** //******************************************************************************
TEST(Cyclic, Between) { TEST(Cyclic, Between) {
EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0))); EXPECT_LONGS_EQUAL(0, traits_x<G>::Between(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1))); EXPECT_LONGS_EQUAL(1, traits_x<G>::Between(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2))); EXPECT_LONGS_EQUAL(2, traits_x<G>::Between(G(0),G(2)));
EXPECT_LONGS_EQUAL(1, group::between(G(2),G(0))); EXPECT_LONGS_EQUAL(1, traits_x<G>::Between(G(2),G(0)));
EXPECT_LONGS_EQUAL(2, group::between(G(2),G(1))); EXPECT_LONGS_EQUAL(2, traits_x<G>::Between(G(2),G(1)));
EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2))); EXPECT_LONGS_EQUAL(0, traits_x<G>::Between(G(2),G(2)));
} }
//****************************************************************************** //******************************************************************************
TEST(Cyclic, Ivnverse) { TEST(Cyclic, Ivnverse) {
EXPECT_LONGS_EQUAL(0, group::inverse(G(0))); EXPECT_LONGS_EQUAL(0, traits_x<G>::Inverse(G(0)));
EXPECT_LONGS_EQUAL(2, group::inverse(G(1))); EXPECT_LONGS_EQUAL(2, traits_x<G>::Inverse(G(1)));
EXPECT_LONGS_EQUAL(1, group::inverse(G(2))); EXPECT_LONGS_EQUAL(1, traits_x<G>::Inverse(G(2)));
} }
//****************************************************************************** //******************************************************************************
TEST(Cyclic , Invariants) { TEST(Cyclic , Invariants) {
G g(2), h(5); G g(2), h(5);
// group::check_invariants(g,h); check_group_invariants(g,h);
} }
//****************************************************************************** //******************************************************************************

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

@ -22,7 +22,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef Quaternion Q; // Typedef typedef Quaternion Q; // Typedef
typedef traits<Q>::ChartJacobian QuaternionJacobian; typedef traits_x<Q>::ChartJacobian QuaternionJacobian;
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Concept) { TEST(Quaternion , Concept) {

4
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -69,13 +69,13 @@ TEST (RotateFactor, test) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Vector,Rot3>( expected = numericalDerivative11<Vector3,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual); f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Vector,Rot3>( expected = numericalDerivative11<Vector3,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual); f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));