diff --git a/gtsam.h b/gtsam.h index 73a626a12..d6da9b164 100644 --- a/gtsam.h +++ b/gtsam.h @@ -563,15 +563,15 @@ virtual class Pose3 : gtsam::Value { void serialize() const; }; -#include -virtual class Sphere2 : gtsam::Value { +#include +virtual class Unit3 : gtsam::Value { // Standard Constructors - Sphere2(); - Sphere2(const gtsam::Point3& pose); + Unit3(); + Unit3(const gtsam::Point3& pose); // Testable void print(string s) const; - bool equals(const gtsam::Sphere2& pose, double tol) const; + bool equals(const gtsam::Unit3& pose, double tol) const; // Other functionality Matrix basis() const; @@ -580,14 +580,14 @@ virtual class Sphere2 : gtsam::Value { // Manifold static size_t Dim(); size_t dim() const; - gtsam::Sphere2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Sphere2& s) const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; }; #include virtual class EssentialMatrix : gtsam::Value { // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb); + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); // Testable void print(string s) const; @@ -601,7 +601,7 @@ virtual class EssentialMatrix : gtsam::Value { // Other methods: gtsam::Rot3 rotation() const; - gtsam::Sphere2 direction() const; + gtsam::Unit3 direction() const; Matrix matrix() const; double error(Vector vA, Vector vB); }; diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cccedc5bb..636b5b7a2 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -19,14 +19,14 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, const Point3& _1T2_ = _1P2_.translation(); if (!H) { // just make a direction out of translation and create E - Sphere2 direction(_1T2_); + Unit3 direction(_1T2_); return EssentialMatrix(_1R2_, direction); } else { // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation - // First get 2*3 derivative from Sphere2::FromPoint3 + // First get 2*3 derivative from Unit3::FromPoint3 Matrix D_direction_1T2; - Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2); + Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); H->resize(5, 6); H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left @@ -49,7 +49,7 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { Vector3 omega(sub(xi, 0, 3)); Vector2 z(sub(xi, 3, 5)); Rot3 R = aRb_.retract(omega); - Sphere2 t = aTb_.retract(z); + Unit3 t = aTb_.retract(z); return EssentialMatrix(R, t); } @@ -85,12 +85,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, if (!HE && !HR) { // Rotate translation direction and return - Sphere2 c1Tc2 = cRb * aTb_; + Unit3 c1Tc2 = cRb * aTb_; return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 - Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) { *HE = zeros(5, 5); HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 @@ -126,7 +126,7 @@ double EssentialMatrix::error(const Vector& vA, const Vector& vB, // /* ************************************************************************* */ ostream& operator <<(ostream& os, const EssentialMatrix& E) { Rot3 R = E.rotation(); - Sphere2 d = E.direction(); + Unit3 d = E.direction(); os.precision(10); os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; return os; @@ -140,7 +140,7 @@ istream& operator >>(istream& is, EssentialMatrix& E) { // Create EssentialMatrix from rotation and translation Rot3 rot = Rot3::RzRyRx(rx, ry, rz); - Sphere2 dt = Sphere2(dx, dy, dz); + Unit3 dt = Unit3(dx, dy, dz); E = EssentialMatrix(rot, dt); return is; diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index a7270fff0..7de68c7a1 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -8,7 +8,7 @@ #pragma once #include -#include +#include #include #include @@ -17,7 +17,7 @@ namespace gtsam { /** * An essential matrix is like a Pose3, except with translation up to scale * It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision, - * but here we choose instead to parameterize it as a (Rot3,Sphere2) pair. + * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ class EssentialMatrix: public DerivedValue { @@ -25,7 +25,7 @@ class EssentialMatrix: public DerivedValue { private: Rot3 aRb_; ///< Rotation between a and b - Sphere2 aTb_; ///< translation direction from a to b + Unit3 aTb_; ///< translation direction from a to b Matrix3 E_; ///< Essential matrix public: @@ -44,7 +44,7 @@ public: } /// Construct from rotation and translation - EssentialMatrix(const Rot3& aRb, const Sphere2& aTb) : + EssentialMatrix(const Rot3& aRb, const Unit3& aTb) : aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } @@ -52,10 +52,10 @@ public: static EssentialMatrix FromPose3(const Pose3& _1P2_, boost::optional H = boost::none); - /// Random, using Rot3::Random and Sphere2::Random + /// Random, using Rot3::Random and Unit3::Random template static EssentialMatrix Random(Engine & rng) { - return EssentialMatrix(Rot3::Random(rng), Sphere2::Random(rng)); + return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng)); } /// @} @@ -103,7 +103,7 @@ public: } /// Direction - inline const Sphere2& direction() const { + inline const Unit3& direction() const { return aTb_; } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ef82e9369..12fec368d 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -40,14 +40,14 @@ Rot3 Rot3::rodriguez(const Point3& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { +Rot3 Rot3::rodriguez(const Unit3& w, double theta) { return rodriguez(w.point3(),theta); } /* ************************************************************************* */ Rot3 Rot3::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( - Sphere2 w = Sphere2::Random(rng); + Unit3 w = Unit3::Random(rng); boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); double angle = randomAngle(rng); return rodriguez(w,angle); @@ -71,9 +71,9 @@ Point3 Rot3::operator*(const Point3& p) const { } /* ************************************************************************* */ -Sphere2 Rot3::rotate(const Sphere2& p, +Unit3 Rot3::rotate(const Unit3& p, boost::optional HR, boost::optional Hp) const { - Sphere2 q = rotate(p.point3(Hp)); + Unit3 q = rotate(p.point3(Hp)); if (Hp) (*Hp) = q.basis().transpose() * matrix() * (*Hp); if (HR) @@ -82,9 +82,9 @@ Sphere2 Rot3::rotate(const Sphere2& p, } /* ************************************************************************* */ -Sphere2 Rot3::unrotate(const Sphere2& p, +Unit3 Rot3::unrotate(const Unit3& p, boost::optional HR, boost::optional Hp) const { - Sphere2 q = unrotate(p.point3(Hp)); + Unit3 q = unrotate(p.point3(Hp)); if (Hp) (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); if (HR) @@ -93,7 +93,7 @@ Sphere2 Rot3::unrotate(const Sphere2& p, } /* ************************************************************************* */ -Sphere2 Rot3::operator*(const Sphere2& p) const { +Unit3 Rot3::operator*(const Unit3& p) const { return rotate(p); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d302a3502..cface1751 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -177,7 +177,7 @@ namespace gtsam { * @param theta rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Sphere2& w, double theta); + static Rot3 rodriguez(const Unit3& w, double theta); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -324,19 +324,19 @@ namespace gtsam { boost::optional H2 = boost::none) const; /// @} - /// @name Group Action on Sphere2 + /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, + Unit3 rotate(const Unit3& p, boost::optional HR = boost::none, boost::optional Hp = boost::none) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Sphere2 unrotate(const Sphere2& p, boost::optional HR = boost::none, + Unit3 unrotate(const Unit3& 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; + Unit3 operator*(const Unit3& p) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Unit3.cpp similarity index 83% rename from gtsam/geometry/Sphere2.cpp rename to gtsam/geometry/Unit3.cpp index 91f1158cd..43b29f0bf 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /* - * @file Sphere2.h + * @file Unit3.h * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor - * @brief The Sphere2 class - basically a point on a unit sphere + * @brief The Unit3 class - basically a point on a unit sphere */ -#include +#include #include #include #include @@ -29,8 +29,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional H) { - Sphere2 direction(point); +Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { + Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix D_p_point; @@ -43,17 +43,17 @@ Sphere2 Sphere2::FromPoint3(const Point3& point, boost::optional H) { } /* ************************************************************************* */ -Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { +Unit3 Unit3::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( boost::random::uniform_on_sphere randomDirection(3); vector d = randomDirection(rng); - Sphere2 result; + Unit3 result; result.p_ = Point3(d[0], d[1], d[2]); return result; } /* ************************************************************************* */ -Matrix Sphere2::basis() const { +Matrix Unit3::basis() const { // Return cached version if exists if (B_.rows() == 3) @@ -85,17 +85,17 @@ Matrix Sphere2::basis() const { /* ************************************************************************* */ /// The print fuction -void Sphere2::print(const std::string& s) const { +void Unit3::print(const std::string& s) const { cout << s << ":" << p_ << endl; } /* ************************************************************************* */ -Matrix Sphere2::skew() const { +Matrix Unit3::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } /* ************************************************************************* */ -Vector Sphere2::error(const Sphere2& q, boost::optional H) const { +Vector Unit3::error(const Unit3& q, boost::optional H) const { Matrix Bt = basis().transpose(); Vector xi = Bt * q.p_.vector(); if (H) @@ -104,7 +104,7 @@ Vector Sphere2::error(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -double Sphere2::distance(const Sphere2& q, boost::optional H) const { +double Unit3::distance(const Unit3& q, boost::optional H) const { Vector xi = error(q, H); double theta = xi.norm(); if (H) @@ -113,7 +113,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -Sphere2 Sphere2::retract(const Vector& v) const { +Unit3 Unit3::retract(const Vector& v) const { // Get the vector form of the point and the basis matrix Vector p = Point3::Logmap(p_); @@ -127,19 +127,19 @@ Sphere2 Sphere2::retract(const Vector& v) const { // Avoid nan if (xi_hat_norm == 0.0) { if (v.norm() == 0.0) - return Sphere2(point3()); + return Unit3(point3()); else - return Sphere2(-point3()); + return Unit3(-point3()); } Vector exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); - return Sphere2(exp_p_xi_hat); + return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector Sphere2::localCoordinates(const Sphere2& y) const { +Vector Unit3::localCoordinates(const Unit3& y) const { Vector p = Point3::Logmap(p_); Vector q = Point3::Logmap(y.p_); diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Unit3.h similarity index 83% rename from gtsam/geometry/Sphere2.h rename to gtsam/geometry/Unit3.h index 1f155a08c..e6e7c8e95 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Unit3.h @@ -10,12 +10,12 @@ * -------------------------------------------------------------------------- */ /* - * @file Sphere2.h + * @file Unit3.h * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor - * @brief Develop a Sphere2 class - basically a point on a unit sphere + * @brief Develop a Unit3 class - basically a point on a unit sphere */ #pragma once @@ -38,7 +38,7 @@ typedef mersenne_twister_engine { +class Unit3: public DerivedValue { private: @@ -51,27 +51,27 @@ public: /// @{ /// Default constructor - Sphere2() : + Unit3() : p_(1.0, 0.0, 0.0) { } /// Construct from point - Sphere2(const Point3& p) : + Unit3(const Point3& p) : p_(p / p.norm()) { } /// Construct from x,y,z - Sphere2(double x, double y, double z) : + Unit3(double x, double y, double z) : p_(x, y, z) { p_ = p_ / p_.norm(); } /// Named constructor from Point3 with optional Jacobian - static Sphere2 FromPoint3(const Point3& point, boost::optional H = + static Unit3 FromPoint3(const Point3& point, boost::optional H = boost::none); /// Random direction, using boost::uniform_on_sphere - static Sphere2 Random(boost::random::mt19937 & rng); + static Unit3 Random(boost::random::mt19937 & rng); /// @} @@ -82,7 +82,7 @@ public: void print(const std::string& s = std::string()) const; /// The equals function with tolerance - bool equals(const Sphere2& s, double tol = 1e-9) const { + bool equals(const Unit3& s, double tol = 1e-9) const { return p_.equals(s.p_, tol); } /// @} @@ -108,16 +108,16 @@ public: } /// Return scaled direction as Point3 - friend Point3 operator*(double s, const Sphere2& d) { + friend Point3 operator*(double s, const Unit3& d) { return s * d.p_; } /// Signed, vector-valued error between two directions - Vector error(const Sphere2& q, + Vector error(const Unit3& q, boost::optional H = boost::none) const; /// Distance between two directions - double distance(const Sphere2& q, + double distance(const Unit3& q, boost::optional H = boost::none) const; /// @} @@ -141,10 +141,10 @@ public: }; /// The retract function - Sphere2 retract(const Vector& v) const; + Unit3 retract(const Vector& v) const; /// The local coordinates function - Vector localCoordinates(const Sphere2& s) const; + Vector localCoordinates(const Unit3& s) const; /// @} }; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index b19c950cd..207b75211 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -70,7 +70,7 @@ TEST (EssentialMatrix, retract1) { //************************************************************************* TEST (EssentialMatrix, retract2) { EssentialMatrix expected(c1Rc2, - Sphere2(c1Tc2).retract((Vector(2) << 0.1, 0))); + Unit3(c1Tc2).retract((Vector(2) << 0.1, 0))); EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0)); EXPECT(assert_equal(expected, actual)); } @@ -85,7 +85,7 @@ TEST (EssentialMatrix, transform_to) { * Rot3::roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, aTb2); - //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); + //EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; E.transform_to(P, actH1, actH2); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index c78b2bf91..92f9a9758 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /* - * @file testSphere2.cpp + * @file testUnit3.cpp * @date Feb 03, 2012 * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor - * @brief Tests the Sphere2 class + * @brief Tests the Unit3 class */ -#include +#include #include #include #include @@ -33,36 +33,36 @@ using namespace boost::assign; using namespace gtsam; using namespace std; -GTSAM_CONCEPT_TESTABLE_INST(Sphere2) -GTSAM_CONCEPT_MANIFOLD_INST(Sphere2) +GTSAM_CONCEPT_TESTABLE_INST(Unit3) +GTSAM_CONCEPT_MANIFOLD_INST(Unit3) //******************************************************************************* -Point3 point3_(const Sphere2& p) { +Point3 point3_(const Unit3& p) { return p.point3(); } -TEST(Sphere2, point3) { +TEST(Unit3, point3) { 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); - expectedH = numericalDerivative11(point3_, s); + Unit3 s(p); + expectedH = numericalDerivative11(point3_, s); EXPECT(assert_equal(p, s.point3(actualH), 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-9)); } } //******************************************************************************* -static Sphere2 rotate_(const Rot3& R, const Sphere2& p) { +static Unit3 rotate_(const Rot3& R, const Unit3& p) { return R * p; } -TEST(Sphere2, rotate) { +TEST(Unit3, rotate) { Rot3 R = Rot3::yaw(0.5); - Sphere2 p(1, 0, 0); - Sphere2 expected = Sphere2(R.column(1)); - Sphere2 actual = R * p; + Unit3 p(1, 0, 0); + Unit3 expected = Unit3(R.column(1)); + Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian @@ -79,15 +79,15 @@ TEST(Sphere2, rotate) { } //******************************************************************************* -static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { +static Unit3 unrotate_(const Rot3& R, const Unit3& p) { return R.unrotate(p); } -TEST(Sphere2, unrotate) { +TEST(Unit3, unrotate) { Rot3 R = Rot3::yaw(-M_PI / 4.0); - Sphere2 p(1, 0, 0); - Sphere2 expected = Sphere2(1, 1, 0); - Sphere2 actual = R.unrotate(p); + Unit3 p(1, 0, 0); + Unit3 expected = Unit3(1, 1, 0); + Unit3 actual = R.unrotate(p); EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian @@ -104,8 +104,8 @@ TEST(Sphere2, unrotate) { } //******************************************************************************* -TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // +TEST(Unit3, error) { + Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // r = p.retract((Vector(2) << 0.8, 0)); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5)); @@ -114,22 +114,22 @@ TEST(Sphere2, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( - boost::bind(&Sphere2::error, &p, _1, boost::none), q); + expected = numericalDerivative11( + boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( - boost::bind(&Sphere2::error, &p, _1, boost::none), r); + expected = numericalDerivative11( + boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } } //******************************************************************************* -TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // +TEST(Unit3, distance) { + Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // r = p.retract((Vector(2) << 0.8, 0)); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); @@ -138,44 +138,44 @@ TEST(Sphere2, distance) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalGradient( - boost::bind(&Sphere2::distance, &p, _1, boost::none), q); + expected = numericalGradient( + boost::bind(&Unit3::distance, &p, _1, boost::none), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalGradient( - boost::bind(&Sphere2::distance, &p, _1, boost::none), r); + expected = numericalGradient( + boost::bind(&Unit3::distance, &p, _1, boost::none), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } } //******************************************************************************* -TEST(Sphere2, localCoordinates0) { - Sphere2 p; +TEST(Unit3, localCoordinates0) { + Unit3 p; Vector actual = p.localCoordinates(p); EXPECT(assert_equal(zero(2), actual, 1e-8)); } //******************************************************************************* -TEST(Sphere2, localCoordinates1) { - Sphere2 p, q(1, 6.12385e-21, 0); +TEST(Unit3, localCoordinates1) { + Unit3 p, q(1, 6.12385e-21, 0); Vector actual = p.localCoordinates(q); CHECK(assert_equal(zero(2), actual, 1e-8)); } //******************************************************************************* -TEST(Sphere2, localCoordinates2) { - Sphere2 p, q(-1, 0, 0); +TEST(Unit3, localCoordinates2) { + Unit3 p, q(-1, 0, 0); Vector expected = (Vector(2) << M_PI, 0); Vector actual = p.localCoordinates(q); CHECK(assert_equal(expected, actual, 1e-8)); } //******************************************************************************* -TEST(Sphere2, basis) { - Sphere2 p; +TEST(Unit3, basis) { + Unit3 p; Matrix expected(3, 2); expected << 0, 0, 0, -1, 1, 0; Matrix actual = p.basis(); @@ -183,23 +183,23 @@ TEST(Sphere2, basis) { } //******************************************************************************* -TEST(Sphere2, retract) { - Sphere2 p; +TEST(Unit3, retract) { + Unit3 p; Vector v(2); v << 0.5, 0; - Sphere2 expected(0.877583, 0, 0.479426); - Sphere2 actual = p.retract(v); + Unit3 expected(0.877583, 0, 0.479426); + Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } //******************************************************************************* -TEST(Sphere2, retract_expmap) { - Sphere2 p; +TEST(Unit3, retract_expmap) { + Unit3 p; Vector v(2); v << (M_PI / 2.0), 0; - Sphere2 expected(Point3(0, 0, 1)); - Sphere2 actual = p.retract(v); + Unit3 expected(Point3(0, 0, 1)); + Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); } @@ -222,9 +222,9 @@ inline static Vector randomVector(const Vector& minLimits, } //******************************************************************************* -// Let x and y be two Sphere2's. +// Let x and y be two Unit3's. // The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Sphere2, localCoordinates_retract) { +TEST(Unit3, localCoordinates_retract) { size_t numIterations = 10000; Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = @@ -235,26 +235,26 @@ TEST(Sphere2, localCoordinates_retract) { // Sleep for the random number generator (TODO?: Better create all of them first). sleep(0); - // Create the two Sphere2s. - // NOTE: You can not create two totally random Sphere2's because you cannot always compute - // between two any Sphere2's. (For instance, they might be at the different sides of the circle). - Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); -// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + // Create the two Unit3s. + // NOTE: You can not create two totally random Unit3's because you cannot always compute + // between two any Unit3's. (For instance, they might be at the different sides of the circle). + Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); Vector v12 = randomVector(minXiLimit, maxXiLimit); - Sphere2 s2 = s1.retract(v12); + Unit3 s2 = s1.retract(v12); // Check if the local coordinates and retract return the same results. Vector actual_v12 = s1.localCoordinates(s2); EXPECT(assert_equal(v12, actual_v12, 1e-3)); - Sphere2 actual_s2 = s1.retract(actual_v12); + Unit3 actual_s2 = s1.retract(actual_v12); EXPECT(assert_equal(s2, actual_s2, 1e-3)); } } //******************************************************************************* -// Let x and y be two Sphere2's. +// Let x and y be two Unit3's. // The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Sphere2, localCoordinates_retract_expmap) { +TEST(Unit3, localCoordinates_retract_expmap) { size_t numIterations = 10000; Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = @@ -266,21 +266,21 @@ TEST(Sphere2, localCoordinates_retract_expmap) { // Sleep for the random number generator (TODO?: Better create all of them first). sleep(0); - // Create the two Sphere2s. + // Create the two Unit3s. // Unlike the above case, we can use any two sphers. - Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); -// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); Vector v12 = randomVector(minXiLimit, maxXiLimit); // Magnitude of the rotation can be at most pi if (v12.norm() > M_PI) v12 = v12 / M_PI; - Sphere2 s2 = s1.retract(v12); + Unit3 s2 = s1.retract(v12); // Check if the local coordinates and retract return the same results. Vector actual_v12 = s1.localCoordinates(s2); EXPECT(assert_equal(v12, actual_v12, 1e-3)); - Sphere2 actual_s2 = s1.retract(actual_v12); + Unit3 actual_s2 = s1.retract(actual_v12); EXPECT(assert_equal(s2, actual_s2, 1e-3)); } } @@ -326,28 +326,28 @@ TEST(Sphere2, localCoordinates_retract_expmap) { //} //******************************************************************************* -TEST(Sphere2, Random) { +TEST(Unit3, Random) { boost::random::mt19937 rng(42); // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); - Point3 actual = Sphere2::Random(rng).point3(); + Point3 actual = Unit3::Random(rng).point3(); EXPECT(assert_equal(expected,actual,1e-5)); // Check that means are all zero at least Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) - actualMean = actualMean + Sphere2::Random(rng).point3(); + actualMean = actualMean + Unit3::Random(rng).point3(); actualMean = actualMean / 100; EXPECT(assert_equal(expectedMean,actualMean,0.1)); } //************************************************************************* -TEST (Sphere2, FromPoint3) { +TEST (Unit3, FromPoint3) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point - Sphere2 expected(point); - EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - boost::bind(Sphere2::FromPoint3, _1, boost::none), point); + Unit3 expected(point); + EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(Unit3::FromPoint3, _1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index fd5e92883..7eba995d3 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -27,13 +27,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb, boost::optional H) const { if (H) { Matrix D_nRef_R, D_e_nRef; - Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R); + Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); Vector e = nZ_.error(nRef, D_e_nRef); H->resize(2, 3); H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; return e; } else { - Sphere2 nRef = nRb * bRef_; + Unit3 nRef = nRb * bRef_; return nZ_.error(nRef); } } diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 5290fc1e5..44dd4183a 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace gtsam { @@ -35,7 +35,7 @@ class AttitudeFactor { protected: - const Sphere2 nZ_, bRef_; ///< Position measurement in + const Unit3 nZ_, bRef_; ///< Position measurement in public: @@ -48,7 +48,7 @@ public: * @param nZ measured direction in navigation frame * @param bRef reference direction in body frame (default Z-axis) */ - AttitudeFactor(const Sphere2& nZ, const Sphere2& bRef = Sphere2(0, 0, 1)) : + AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) : nZ_(nZ), bRef_(bRef) { } @@ -87,8 +87,8 @@ public: * @param model Gaussian noise model * @param bRef reference direction in body frame (default Z-axis) */ - Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, - const Sphere2& bRef = Sphere2(0, 0, 1)) : + Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, + const Unit3& bRef = Unit3(0, 0, 1)) : Base(model, key), AttitudeFactor(nZ, bRef) { } @@ -156,8 +156,8 @@ public: * @param model Gaussian noise model * @param bRef reference direction in body frame (default Z-axis) */ - Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, - const Sphere2& bRef = Sphere2(0, 0, 1)) : + Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, + const Unit3& bRef = Unit3(0, 0, 1)) : Base(model, key), AttitudeFactor(nZ, bRef) { } diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 6c47523a6..79677c0c6 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -39,7 +39,7 @@ public: /** Constructor */ MagFactor(Key key, const Point3& measured, double scale, - const Sphere2& direction, const Point3& bias, + const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { @@ -85,7 +85,7 @@ public: /** Constructor */ MagFactor1(Key key, const Point3& measured, double scale, - const Sphere2& direction, const Point3& bias, + const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : NoiseModelFactor1(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { @@ -154,7 +154,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactor3 { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -164,7 +164,7 @@ public: /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactor3(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } @@ -179,12 +179,12 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(const LieScalar& scale, const Sphere2& direction, + Vector evaluateError(const LieScalar& scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { // measured bM = nRbÕ * nM + b, where b is unknown bias - Sphere2 rotated = bRn_.rotate(direction, boost::none, H2); + Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) *H1 = rotated.point3().vector(); diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 9edb2bbf5..c4155ea16 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -30,8 +30,8 @@ TEST( Rot3AttitudeFactor, Constructor ) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Sphere2 bZ(0, 0, 1); // reference direction is body Z axis - Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bZ(0, 0, 1); // reference direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" // Factor Key key(1); @@ -63,8 +63,8 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Sphere2 bZ(0, 0, 1); // reference direction is body Z axis - Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bZ(0, 0, 1); // reference direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" // Factor Key key(1); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 7e6b940c3..1875e4f1c 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -49,7 +49,7 @@ Point3 scaled = scale * nM; Point3 measured = nRb.inverse() * (scale * nM) + bias; LieScalar s(scale * nM.norm()); -Sphere2 dir(nM); +Unit3 dir(nM); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); @@ -97,7 +97,7 @@ TEST( MagFactor, Factors ) { EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 0b6dde95d..3b40a0ba6 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -66,7 +66,7 @@ public: */ class RotateDirectionsFactor: public NoiseModelFactor1 { - Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z + Unit3 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; typedef RotateDirectionsFactor This; @@ -74,7 +74,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { public: /// Constructor - RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z, + RotateDirectionsFactor(Key key, const Unit3& p, const Unit3& z, const SharedNoiseModel& model) : Base(model, key), p_(p), z_(z) { } @@ -96,7 +96,7 @@ public: /// vector of errors returns 2D vector Vector evaluateError(const Rot3& R, boost::optional H = boost::none) const { - Sphere2 q = R * z_; + Unit3 q = R * z_; Vector e = p_.error(q, H); if (H) { Matrix DR; diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index eb87100f6..8fcb5b6e5 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -34,7 +34,7 @@ TEST( EssentialMatrixConstraint, test ) { Key poseKey2(2); Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20); Point3 trueTranslation(+0.5, -1.0, +1.0); - Sphere2 trueDirection(trueTranslation); + Unit3 trueDirection(trueTranslation); EssentialMatrix measurement(trueRotation, trueDirection); SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25); EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 715b72b74..ca924aaae 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -28,7 +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); +Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), // i2Ri3 = Rot3::rodriguez(p2, 1), // i3Ri4 = Rot3::rodriguez(p3, 1); @@ -39,7 +39,7 @@ c2Zc3 = iRc.inverse() * i2Ri3 * iRc, // c3Zc4 = iRc.inverse() * i3Ri4 * iRc; // The corresponding rotated directions in the camera frame -Sphere2 z1 = iRc.inverse() * p1, // +Unit3 z1 = iRc.inverse() * p1, // z2 = iRc.inverse() * p2, // z3 = iRc.inverse() * p3;