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

re-formatted ScalarTraits while checking correctness (seems correct)

release/4.3a0
dellaert 2014-12-21 16:46:20 +01:00
parent 505c876588
commit 88a83fe9e7
2 changed files with 65 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
gtsam/base/concepts.h
View File

@ -231,129 +231,77 @@ struct ScalarTraits {
typedef additive_group_tag group_flavor; typedef additive_group_tag group_flavor;
typedef Scalar ManifoldType; typedef Scalar ManifoldType;
enum { dimension = 1 }; enum { dimension = 1 };
typedef Eigen::Matrix<double, dimension, 1> TangentVector; typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<1, 1> ChartJacobian;
// For Testable // For Testable
static void Print(Scalar m, const std::string& str = "") { static void Print(Scalar m, const std::string& str = "") {
gtsam::print(m,str); gtsam::print(m, str);
} }
static bool Equals(Scalar m1, static bool Equals(Scalar m1, Scalar m2, double tol = 1e-8) {
Scalar m2,
double tol = 1e-8) {
return fabs(m1 - m2) < tol; return fabs(m1 - m2) < tol;
} }
static TangentVector Local(Scalar origin, static TangentVector Local(Scalar origin, Scalar other) {
Scalar other) {
return TangentVector(other - origin); return TangentVector(other - origin);
} }
static Scalar Retract(Scalar origin, static Scalar Retract(Scalar origin, const TangentVector& v) {
const TangentVector& v) {
return origin + v[0]; return origin + v[0];
} }
static TangentVector Local(Scalar origin, static TangentVector Local(Scalar origin, Scalar other, ChartJacobian Horigin,
Scalar other,
ChartJacobian Horigin,
ChartJacobian Hother = boost::none) { ChartJacobian Hother = boost::none) {
if(Horigin) { if (Horigin) (*Horigin)[0] = -1.0;
(*Horigin)[0] = -1.0; if (Hother) (*Hother)[0] = 1.0;
} return TangentVector(other - origin);
if(Hother) {
(*Hother)[0] = 1.0;
}
return Local(origin, other);
} }
static Scalar Retract(Scalar origin, static Scalar Retract(Scalar origin, const TangentVector& v,
const TangentVector& v, ChartJacobian Horigin, ChartJacobian Hv = boost::none) {
ChartJacobian Horigin, if (Horigin) (*Horigin)[0] = 1.0;
ChartJacobian Hv = boost::none) { if (Hv) (*Hv)[0] = 1.0;
if(Horigin) { return origin + v[0];
(*Horigin)[0] = 1.0;
}
if(Hv) {
(*Hv)[0] = 1.0;
}
return Retract(origin, v);
} }
static int GetDimension(Scalar m){ return 1; } static int GetDimension(Scalar m) { return 1; }
// For Group. Only implemented for groups // For Group. Only implemented for groups
static Scalar Compose(Scalar m1, static Scalar Compose(Scalar m1, Scalar m2) { return m1 + m2;}
Scalar m2) { static Scalar Between(Scalar m1, Scalar m2) { return m2 - m1;}
static Scalar Inverse(Scalar m) { return -m;}
static Scalar Compose(Scalar m1, Scalar m2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = 1.0;
if (H2) (*H2)[0] = 1.0;
return m1 + m2; return m1 + m2;
} }
static Scalar Between(Scalar m1, static Scalar Between(Scalar m1, Scalar m2, ChartJacobian H1,
Scalar m2) { ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = -1.0;
if (H2) (*H2)[0] = 1.0;
return m2 - m1; return m2 - m1;
} }
static Scalar Inverse(Scalar m) { static Scalar Inverse(Scalar m, ChartJacobian H) {
if (H) (*H)[0] = -1;
return -m; return -m;
} }
static Scalar Compose(Scalar m1, static Scalar Identity() { return 0; }
Scalar m2, static TangentVector Logmap(Scalar m) {return TangentVector(m);}
ChartJacobian H1, static Scalar Expmap(const TangentVector& v) { return v[0];}
ChartJacobian H2 = boost::none) {
if(H1) {
(*H1)[0] = 1.0;
}
if(H2) {
(*H2)[0] = 1.0;
}
return Compose(m1, m2);
}
static Scalar Between(Scalar m1, static TangentVector Logmap(Scalar m, ChartJacobian Hm) {
Scalar m2, if (Hm) (*Hm)[0] = 1.0;
ChartJacobian H1,
ChartJacobian H2 = boost::none) {
if(H1) {
(*H1)[0] = -1.0;
}
if(H2) {
(*H2)[0] = 1.0;
}
return Between(m1, m2);
}
static Scalar Inverse(Scalar m, ChartJacobian H) {
if(H) {
(*H)[0] = -1;
}
return Inverse(m);
}
static Scalar Identity() {
return 0;
}
static TangentVector Logmap(Scalar m) {
return TangentVector(m); return TangentVector(m);
} }
static Scalar Expmap(const TangentVector& v) {
return v[0];
}
static TangentVector Logmap(Scalar m, ChartJacobian Hm) {
if(Hm) {
(*Hm)[0] = 1.0;
}
return Scalar::Logmap(m);
}
static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) {
if(Hv) { if (Hv) (*Hv)[0] = 1.0;
(*Hv)[0] = 1.0; return v[0];
}
return Scalar::Expmap(v);
} }
}; };

28
gtsam/geometry/tests/testPoint2.cpp
View File

@ -27,6 +27,34 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2)
//******************************************************************************
TEST(Double , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<double>));
BOOST_CONCEPT_ASSERT((IsManifold<double>));
BOOST_CONCEPT_ASSERT((IsLieGroup<double>));
}
//******************************************************************************
TEST(Double , Invariants) {
double p1(2), p2(5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
}
//******************************************************************************
TEST(Point2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point2>));
BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Point2>));
}
//******************************************************************************
TEST(Point2 , Invariants) {
Point2 p1(1, 2), p2(4, 5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Point2, constructor) { TEST(Point2, constructor) {
Point2 p1(1, 2), p2 = p1; Point2 p1(1, 2), p2 = p1;