diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 53895d7bb..b86a3e899 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace gtsam { @@ -293,6 +294,17 @@ namespace gtsam { Point3 unrotate(const Point3& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + /// @} + /// @name Group Action on Sphere2 + /// @{ + + /// rotate 3D direction from rotated coordinate frame to world frame + Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + + /// rotate 3D direction from rotated coordinate frame to world frame + Sphere2 operator*(const Sphere2& p) const; + /// @} /// @name Standard Interface /// @{ @@ -303,11 +315,12 @@ namespace gtsam { /** return 3*3 transpose (inverse) rotation matrix */ Matrix3 transpose() const; - /** returns column vector specified by index */ + /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; - Point3 r1() const; - Point3 r2() const; - Point3 r3() const; + + Point3 r1() const; ///< first column + Point3 r2() const; ///< second column + Point3 r3() const; ///< third column /** * Use RQ to calculate xyz angle representation diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index ab5b7f909..327d5feb0 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -201,6 +201,22 @@ Point3 Rot3::rotate(const Point3& p, return Point3(rot_ * p.vector()); } +/* ************************************************************************* */ +Sphere2 Rot3::rotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q(rotate(p.point3(Hp))); + if (Hp) + (*Hp) = q.basis().transpose() * matrix() * (*Hp); + if (HR) + (*HR) = -q.basis().transpose() * matrix() * p.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::operator*(const Sphere2& p) const { + return rotate(p); +} + /* ************************************************************************* */ // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, @@ -346,7 +362,14 @@ Matrix3 Rot3::transpose() const { /* ************************************************************************* */ Point3 Rot3::column(int index) const{ - return Point3(rot_.col(index)); + if(index == 3) + return r3(); + else if(index == 2) + return r2(); + else if(index == 1) + return r1(); // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 356fb8a36..e2ee24cab 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -67,22 +67,6 @@ Matrix Sphere2::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } -/* ************************************************************************* */ -Sphere2 Sphere2::Rotate(const Rot3& R, const Sphere2& p, - boost::optional HR, boost::optional Hp) { - Sphere2 q(R * p.p_); - if (Hp) - (*Hp) = q.basis().transpose() * R.matrix() * p.basis(); - if (HR) - (*HR) = -q.basis().transpose() * R.matrix() * p.skew(); - return q; -} - -/* ************************************************************************* */ -Sphere2 operator*(const Rot3& R, const Sphere2& p) { - return Sphere2::Rotate(R, p); -} - /* ************************************************************************* */ Vector Sphere2::error(const Sphere2& q, boost::optional H) const { Matrix Bt = basis().transpose(); diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 29decde1c..5d1bbd7b2 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include namespace gtsam { @@ -83,14 +83,6 @@ public: return p_; } - /// Rotate - static Sphere2 Rotate(const Rot3& R, const Sphere2& p, - boost::optional HR = boost::none, boost::optional Hp = - boost::none); - - /// Rotate sugar - friend Sphere2 operator*(const Rot3& R, const Sphere2& p); - /// Signed, vector-valued error between two directions Vector error(const Sphere2& q, boost::optional H = boost::none) const; diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 0041b1289..af435bcea 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -36,8 +37,9 @@ Point3 point3_(const Sphere2& p) { return p.point3(); } TEST(Sphere2, point3) { - vector < Point3 > ps; - ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)/sqrt(2); + vector ps; + ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) + / sqrt(2); Matrix actualH, expectedH; BOOST_FOREACH(Point3 p,ps) { Sphere2 s(p); @@ -48,6 +50,10 @@ TEST(Sphere2, point3) { } //******************************************************************************* +static Sphere2 rotate_(const Rot3& R, const Sphere2& p) { + return R * p; +} + TEST(Sphere2, rotate) { Rot3 R = Rot3::yaw(0.5); Sphere2 p(1, 0, 0); @@ -57,15 +63,13 @@ TEST(Sphere2, rotate) { Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian { - expectedH = numericalDerivative11( - boost::bind(&Sphere2::Rotate, _1, p, boost::none, boost::none), R); - Sphere2::Rotate(R, p, actualH, boost::none); + expectedH = numericalDerivative21(rotate_,R,p); + R.rotate(p, actualH, boost::none); EXPECT(assert_equal(expectedH, actualH, 1e-9)); } { - expectedH = numericalDerivative11( - boost::bind(&Sphere2::Rotate, R, _1, boost::none, boost::none), p); - Sphere2::Rotate(R, p, boost::none, actualH); + expectedH = numericalDerivative22(rotate_,R,p); + R.rotate(p, boost::none, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-9)); } } diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 428f60680..83e89b713 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -8,7 +8,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -44,7 +44,7 @@ public: Vector e = p_.error(q, H); if (H) { Matrix DR; - Sphere2::Rotate(R, z_, DR); + R.rotate(z_, DR); *H = (*H) * DR; } return e; @@ -57,7 +57,7 @@ public: double e = p_.distance(q, H); if (H) { Matrix DR; - Sphere2::Rotate(R, z_, DR); + R.rotate(z_, DR); *H = (*H) * DR; } return (Vector(1) << e); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index c5992d3de..56ea09b5d 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -28,6 +28,7 @@ Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); Rot3 iRc(cameraX, cameraY, cameraZ); // Now, let's create some rotations around IMU frame Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); +Rot3 gRi1 = Rot3::Expmap(Vector3(1, 0, 0)); // The corresponding rotations in the camera frame Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, // z3 = iRc.inverse() * p3;