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

Added Rot2 compose and between derivatives and unit tests

release/4.3a0
Richard Roberts 2011-08-30 19:08:22 +00:00
parent ca62706f55
commit 36ff110eef
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/Rot2.h
View File

@ -106,7 +106,13 @@ namespace gtsam {
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R1) const { return *this * R1;} inline Rot2 compose(const Rot2& R1,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(1);
if(H2) *H2 = eye(1);
return *this * R1;
}
/** Expmap around identity - create a rotation from an angle */ /** Expmap around identity - create a rotation from an angle */
static Rot2 Expmap(const Vector& v) { static Rot2 Expmap(const Vector& v) {
@ -126,7 +132,13 @@ namespace gtsam {
inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));} inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));}
/** Between using the default implementation */ /** Between using the default implementation */
inline Rot2 between(const Rot2& p2) const { return between_default(*this, p2); } inline Rot2 between(const Rot2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(1);
if(H2) *H2 = eye(1);
return between_default(*this, p2);
}
/** return 2*2 rotation matrix */ /** return 2*2 rotation matrix */
Matrix matrix() const; Matrix matrix() const;

17
gtsam/geometry/tests/testRot2.cpp
View File

@ -44,6 +44,23 @@ TEST( Rot2, compose)
{ {
CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.2)*Rot2::fromAngle(0.25))); CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.2)*Rot2::fromAngle(0.25)));
CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.25)*Rot2::fromAngle(0.2))); CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.25)*Rot2::fromAngle(0.2)));
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
}
/* ************************************************************************* */
TEST( Rot2, between)
{
CHECK(assert_equal(Rot2::fromAngle(0.05), Rot2::fromAngle(0.2).between(Rot2::fromAngle(0.25))));
CHECK(assert_equal(Rot2::fromAngle(-0.05), Rot2::fromAngle(0.25).between(Rot2::fromAngle(0.2))));
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(-eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
} }
/* ************************************************************************* */ /* ************************************************************************* */