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

Added derivatives to Point3::between and added several missing unit tests

release/4.3a0
Richard Roberts 2011-08-27 00:58:43 +00:00
parent 690a2c59fd
commit 02a6a8caad
2 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Point3.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H2=boost::none) const {
if (H1) *H1 = eye(3); if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3); if (H2) *H2 = eye(3);
return *this+p2; return *this + p2;
} }
/** Exponential map at identity - just create a Point3 from x,y,z */ /** Exponential map at identity - just create a Point3 from x,y,z */
@ -81,7 +81,13 @@ namespace gtsam {
inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);} inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);}
/** Between using the default implementation */ /** Between using the default implementation */
inline Point3 between(const Point3& p2) const { return between_default(*this, p2); } inline Point3 between(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(3);
if(H2) *H2 = eye(3);
return p2 - *this;
}
/** return vectorized form (column-wise)*/ /** return vectorized form (column-wise)*/
Vector vector() const { Vector vector() const {

18
gtsam/geometry/tests/testPoint3.cpp
View File

@ -21,6 +21,24 @@ using namespace gtsam;
Point3 P(0.2,0.7,-2); Point3 P(0.2,0.7,-2);
/* ************************************************************************* */
TEST(Point3, Lie) {
Point3 p1(1,2,3);
Point3 p2(4,5,6);
Matrix H1, H2;
EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(p2)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, arithmetic) TEST( Point3, arithmetic)
{ {