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) {
m.print();
}
void Equals(const ManifoldType& m1,
bool Equals(const ManifoldType& m1,
const ManifoldType& m2,
double tol = 1e-8) {
return m1.equals(m2, tol);
@ -116,7 +116,7 @@ struct LieGroup {
void Print(const ManifoldType& m) {
m.print();
}
void Equals(const ManifoldType& m1,
bool Equals(const ManifoldType& m1,
const ManifoldType& m2,
double tol = 1e-8) {
return m1.equals(m2, tol);
@ -201,6 +201,7 @@ struct LieGroup {
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) {
return ManifoldType::Expmap(v, Hv);
}
};
} // namespace internal
@ -256,11 +257,17 @@ public:
// and the versions with Jacobians.
v = traits_x<M>::Local(p,q,Hp,Hq);
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:
ManifoldType p,q;
ChartJacobian Hp,Hq,Hv;
TangentVector v;
bool b;
};
/**
@ -277,12 +284,17 @@ public:
BOOST_STATIC_ASSERT_MSG(
(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)");
e = traits_x<G>::identity;
e = traits_x<G>::Identity();
e = traits_x<G>::Compose(g, h);
e = traits_x<G>::Between(g, h);
e = traits_x<G>::Inverse(g);
operator_usage(flavor);
// 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:
@ -298,14 +310,15 @@ private:
flavor_tag flavor;
G e, g, h;
bool b;
};
/// Check invariants
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits_x<G>::identity;
return traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::inverse(a)), e, tol)
G e = traits_x<G>::Identity();
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>::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) {
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.");
typedef DefaultChart<Y> ChartY;
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.");
static const int N = traits::dimension<X>::value;
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 > {
typedef group_tag structure_category;
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

5
gtsam/geometry/Quaternion.h
View File

@ -29,8 +29,11 @@ struct traits_x<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q;
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
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;

29
gtsam/geometry/Unit3.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
@ -28,7 +29,7 @@
namespace gtsam {
/// Represents a 3D point on a unit sphere.
class GTSAM_EXPORT Unit3{
class GTSAM_EXPORT Unit3 {
private:
@ -37,6 +38,8 @@ private:
public:
enum { dimension = 2 };
/// @name Constructors
/// @{
@ -160,24 +163,14 @@ private:
};
// Define GTSAM traits
namespace traits {
template <> struct traits_x<Unit3> : public internal::Manifold<Unit3> {};
template<>
struct GTSAM_EXPORT is_manifold<Unit3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Unit3> : public boost::integral_constant<int, 2>{
};
template<>
struct GTSAM_EXPORT zero<Unit3> {
static Unit3 value() {
return Unit3();
}
};
}
//template<>
//struct GTSAM_EXPORT zero<Unit3> {
// static Unit3 value() {
// return Unit3();
// }
//};
} // 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) {
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) {
EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2)));
EXPECT_LONGS_EQUAL(0, traits_x<G>::Compose(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits_x<G>::Compose(G(0),G(1)));
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(0, group::compose(G(2),G(1)));
EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(2)));
EXPECT_LONGS_EQUAL(2, traits_x<G>::Compose(G(2),G(0)));
EXPECT_LONGS_EQUAL(0, traits_x<G>::Compose(G(2),G(1)));
EXPECT_LONGS_EQUAL(1, traits_x<G>::Compose(G(2),G(2)));
}
//******************************************************************************
TEST(Cyclic, Between) {
EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2)));
EXPECT_LONGS_EQUAL(0, traits_x<G>::Between(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits_x<G>::Between(G(0),G(1)));
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(2, group::between(G(2),G(1)));
EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2)));
EXPECT_LONGS_EQUAL(1, traits_x<G>::Between(G(2),G(0)));
EXPECT_LONGS_EQUAL(2, traits_x<G>::Between(G(2),G(1)));
EXPECT_LONGS_EQUAL(0, traits_x<G>::Between(G(2),G(2)));
}
//******************************************************************************
TEST(Cyclic, Ivnverse) {
EXPECT_LONGS_EQUAL(0, group::inverse(G(0)));
EXPECT_LONGS_EQUAL(2, group::inverse(G(1)));
EXPECT_LONGS_EQUAL(1, group::inverse(G(2)));
EXPECT_LONGS_EQUAL(0, traits_x<G>::Inverse(G(0)));
EXPECT_LONGS_EQUAL(2, traits_x<G>::Inverse(G(1)));
EXPECT_LONGS_EQUAL(1, traits_x<G>::Inverse(G(2)));
}
//******************************************************************************
TEST(Cyclic , Invariants) {
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;
typedef Quaternion Q; // Typedef
typedef traits<Q>::ChartJacobian QuaternionJacobian;
typedef traits_x<Q>::ChartJacobian QuaternionJacobian;
//******************************************************************************
TEST(Quaternion , Concept) {

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

@ -69,13 +69,13 @@ TEST (RotateFactor, test) {
Matrix actual, expected;
// 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);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Vector,Rot3>(
expected = numericalDerivative11<Vector3,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));