diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index c53ff16bf..aaf0aa953 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,9 +21,6 @@ #include #include -#include // GTSAM_USE_TBB - -#include #include // for GTSAM_USE_TBB #ifdef __clang__ @@ -45,14 +42,13 @@ namespace gtsam { /* ************************************************************************* */ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { - Unit3 direction(point); - if (H) { - // 3*3 Derivative of representation with respect to point is 3*3: - Matrix3 D_p_point; - point.normalize(D_p_point); // TODO, this calculates norm a second time :-( - // Calculate the 2*3 Jacobian + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix3 D_p_point; + Point3 normalized = point.normalize(H ? &D_p_point : 0); + Unit3 direction; + direction.p_ = normalized.vector(); + if (H) *H << direction.basis().transpose() * D_p_point; - } return direction; } @@ -90,19 +86,20 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Get the unit vector and derivative wrt this. // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - Point3 n(p_); + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point Point3 axis; - double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); - if ((mx <= my) && (mx <= mz)) + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); - else if ((my <= mx) && (my <= mz)) + } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); - else if ((mz <= mx) && (mz <= my)) + } else if ((mz <= mx) && (mz <= my)) { axis = Point3(0.0, 0.0, 1.0); - else + } else { assert(false); + } // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. @@ -148,7 +145,7 @@ Point3 Unit3::point3(OptionalJacobian<3, 2> H) const { Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const { if (H) *H = basis(); - return (p_); + return p_; } /* ************************************************************************* */ @@ -171,10 +168,10 @@ Matrix3 Unit3::skew() const { double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; - const Point3& pn = point3(H_p ? &H_pn_p : 0); + Point3 pn = point3(H_p ? &H_pn_p : 0); Matrix32 H_qn_q; - const Point3& qn = q.point3(H_q ? &H_qn_q : 0); + Point3 qn = q.point3(H_q ? &H_qn_q : 0); // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; @@ -206,7 +203,7 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; - const Point3& qn = q.point3(H_q ? &H_qn_q : 0); + Point3 qn = q.point3(H_q ? &H_qn_q : 0); // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index e8fe24c9e..b7d359f40 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,12 +20,17 @@ #pragma once -#include #include #include +#include +#include +#include -#include #include +#include +#include + +#include #ifdef GTSAM_USE_TBB #include @@ -76,6 +81,12 @@ public: p_.normalize(); } + /// Construct from 2D point in plane at focal length f + /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point + explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + p_.normalize(); + } + /// Named constructor from Point3 with optional Jacobian static Unit3 FromPoint3(const Point3& point, // OptionalJacobian<2, 3> H = boost::none); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 2f8bf378f..bfc5caad7 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -18,19 +18,20 @@ * @brief Tests the Unit3 class */ -#include -#include #include #include +#include +#include +#include #include #include #include -#include #include -#include #include + #include + #include #include #include @@ -49,6 +50,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(Unit3) Point3 point3_(const Unit3& p) { return p.point3(); } + TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) @@ -98,6 +100,7 @@ TEST(Unit3, unrotate) { 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 { @@ -237,19 +240,66 @@ TEST(Unit3, localCoordinates0) { EXPECT(assert_equal(zero(2), actual, 1e-8)); } -//******************************************************************************* -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(Unit3, localCoordinates) { + { + Unit3 p, q; + Vector2 expected = Vector2::Zero(); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(1, 6.12385e-21, 0); + Vector2 expected = Vector2::Zero(); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(-1, 0, 0); + Vector2 expected(M_PI, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(0, 1, 0); + Vector2 expected(0,-M_PI_2); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p, q(0, -1, 0); + Vector2 expected(0, M_PI_2); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + } + { + Unit3 p(0,1,0), q(0,-1,0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } + { + Unit3 p(0,0,1), q(0,0,-1); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } -//******************************************************************************* -TEST(Unit3, localCoordinates2) { - Unit3 p, q(-1, 0, 0); - Vector expected = (Vector(2) << M_PI, 0).finished(); - Vector actual = p.localCoordinates(q); - CHECK(assert_equal(expected, actual, 1e-8)); + double twist = 1e-4; + { + Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(actual(0) < 1e-2); + EXPECT(actual(1) > M_PI - 1e-2) + } + { + Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0); + Vector2 actual = p.localCoordinates(q); + EXPECT(actual(0) < 1e-2); + EXPECT(actual(1) < -M_PI + 1e-2) + } } //******************************************************************************* @@ -296,98 +346,33 @@ TEST(Unit3, basis_derivatives) { //******************************************************************************* TEST(Unit3, retract) { - Unit3 p; - Vector v(2); - v << 0.5, 0; - 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)); + { + Unit3 p; + Vector2 v(0.5, 0); + 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)); + } + { + Unit3 p; + Vector2 v(0, 0); + Unit3 actual = p.retract(v); + EXPECT(assert_equal(p, actual, 1e-6)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + } } //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p; - Vector v(2); - v << (M_PI / 2.0), 0; + Vector2 v((M_PI / 2.0), 0); 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)); } -//******************************************************************************* -/// Returns a random vector -inline static Vector randomVector(const Vector& minLimits, - const Vector& maxLimits) { - - // Get the number of dimensions and create the return vector - size_t numDims = dim(minLimits); - Vector vector = zero(numDims); - - // Create the random vector - for (size_t i = 0; i < numDims; i++) { - double range = maxLimits(i) - minLimits(i); - vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); - } - return vector; -} - -//******************************************************************************* -// Let x and y be two Unit3's. -// The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Unit3, localCoordinates_retract) { - - size_t numIterations = 10000; - Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = - Vector3(1.0, 1.0, 1.0); - Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); - for (size_t i = 0; i < numIterations; i++) { - - // 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))); - Vector v12 = randomVector(minXiLimit, maxXiLimit); - 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)); - Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); - } -} - -//******************************************************************************* -// Let x and y be two Unit3's. -// The equality x.localCoordinates(x.retract(v)) == v should hold. -TEST(Unit3, localCoordinates_retract_expmap) { - - size_t numIterations = 10000; - Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = - Vector3(1.0, 1.0, 1.0); - Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); - for (size_t i = 0; i < numIterations; i++) { - - // Create the two Unit3s. - // Unlike the above case, we can use any two Unit3's. - Unit3 s1(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; - 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)); - Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); - } -} - //******************************************************************************* TEST(Unit3, Random) { boost::mt19937 rng(42); @@ -399,6 +384,26 @@ TEST(Unit3, Random) { EXPECT(assert_equal(expectedMean,actualMean,0.1)); } +//******************************************************************************* +// New test that uses Unit3::Random +TEST(Unit3, localCoordinates_retract) { + boost::mt19937 rng(42); + size_t numIterations = 10000; + + for (size_t i = 0; i < numIterations; i++) { + // Create two random Unit3s + const Unit3 s1 = Unit3::Random(rng); + const Unit3 s2 = Unit3::Random(rng); + // Check that they are not at opposite ends of the sphere, which is ill defined + if (s1.unitVector().dot(s2.unitVector())<-0.9) continue; + + // Check if the local coordinates and retract return consistent results. + Vector v12 = s1.localCoordinates(s2); + Unit3 actual_s2 = s1.retract(v12); + EXPECT(assert_equal(s2, actual_s2, 1e-9)); + } +} + //************************************************************************* TEST (Unit3, FromPoint3) { Matrix actualH; @@ -450,6 +455,14 @@ TEST(Unit3, ErrorBetweenFactor) { } } +/* ************************************************************************* */ +TEST(actualH, Serialization) { + Unit3 p(0, 1, 0); + EXPECT(serializationTestHelpers::equalsObj(p)); + EXPECT(serializationTestHelpers::equalsXML(p)); + EXPECT(serializationTestHelpers::equalsBinary(p)); +} + /* ************************************************************************* */ int main() { srand(time(NULL));