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

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

@ -27,6 +27,34 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_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) {
Point2 p1(1, 2), p2 = p1;