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

Point2 loses its mojo (superfluous Lie/Manifold stuff)

release/4.3a0
dellaert 2014-12-22 16:05:16 +01:00
parent 5473550eea
commit 65ae450abd
2 changed files with 7 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
gtsam/geometry/Point2.h
View File

@ -107,109 +107,17 @@ public:
/// @{
/// identity
inline static Point2 identity() {
return Point2();
}
/// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity()
inline Point2 inverse(OptionalJacobian<2,2> H=boost::none) const {
if (H) *H = -I_2x2;
return Point2(-x_, -y_);
}
inline static Point2 identity() {return Point2();}
/// syntactic sugar for inverse, i.e., -p == inverse(p)
inline Point2 operator- () const {return Point2(-x_,-y_);}
/// "Compose", just adds the coordinates of two points. With optional derivatives
inline Point2 compose(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = I_2x2;
if(H2) *H2 = I_2x2;
return *this + q;
}
/// syntactic sugar for adding two points, i.e., p+q == compose(p,q)
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
/// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q)
inline Point2 between(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = -I_2x2;
if(H2) *H2 = I_2x2;
return q - (*this);
}
/// syntactic sugar for subtracting points, i.e., q-p == between(p,q)
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return 2; }
/// Dimensionality of tangent space = 2 DOF
inline size_t dim() const { return 2; }
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const {
return *this + Point2(v);
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const {
Point2 dp = t2 - *this;
return Vector2(dp.x(), dp.y());
}
/// @}
/// @name Lie Group
/// @{
/// Exponential map around identity - just create a Point2 from a vector
static Point2 Expmap(const Vector2& v,
OptionalJacobian<2, 2> H = boost::none) {
if (H) *H = I_2x2;
return Point2(v);
}
/// Logmap around identity
static inline Vector2 Logmap(const Point2& dp,
OptionalJacobian<2, 2> H = boost::none) {
if (H) *H = I_2x2;
return Vector2(dp.x(), dp.y());
}
/// Left-trivialized derivative of the exponential map
static Matrix2 dexpL(const Vector2& v) {
return I_2x2;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix2 dexpInvL(const Vector2& v) {
return I_2x2;
}
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v, OptionalJacobian<2, 2> H1,
OptionalJacobian<2, 2> H2 = boost::none) const {
if (H1) *H1 = I_2x2;
if (H2) *H2 = I_2x2;
return *this + Point2(v);
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2, 2> H1,
OptionalJacobian<2, 2> H2 = boost::none) const {
if (H1) *H1 = - I_2x2;
if (H2) *H2 = I_2x2;
Point2 dp = t2 - *this;
return Vector2(dp.x(), dp.y());
}
/// @}
/// @name Vector Space
/// @{
@ -285,5 +193,5 @@ inline Point2 operator*(double s, const Point2& p) {return p*s;}
template<>
struct traits_x<Point2> : public internal::VectorSpace<Point2> {};
}
} // \ namespace gtsam

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

@ -66,16 +66,16 @@ TEST(Point2, Lie) {
Point2 p1(1, 2), p2(4, 5);
Matrix H1, H2;
EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(Point2(5,7), traits_x<Point2>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(Point2(3,3), traits_x<Point2>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point2(5,7), traits_x<Point2>::Retract(p1, Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), traits_x<Point2>::Local(p1,p2)));
}
/* ************************************************************************* */
@ -83,7 +83,7 @@ TEST( Point2, expmap) {
Vector d(2);
d(0) = 1;
d(1) = -1;
Point2 a(4, 5), b = a.retract(d), c(5, 4);
Point2 a(4, 5), b = traits_x<Point2>::Retract(a,d), c(5, 4);
EXPECT(assert_equal(b,c));
}