From aa1e5962c91e3934c83a51f691f9e63cc0364089 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sat, 27 Jun 2015 17:49:45 -0400 Subject: [PATCH 1/8] fix: correct some inappropriate floating point error, and its test --- gtsam/geometry/Unit3.cpp | 8 +-- gtsam/geometry/tests/testUnit3.cpp | 106 +++++++++++++++-------------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a74e39f47..5196b9477 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -148,12 +148,11 @@ Unit3 Unit3::retract(const Vector2& v) const { // Compute the 3D xi_hat vector Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - double xi_hat_norm = xi_hat.norm(); // Avoid nan - if (xi_hat_norm == 0.0) { - if (v.norm() == 0.0) + if (xi_hat_norm < 1e-16) { + if (v.norm() < 1e-16) return Unit3(point3()); else return Unit3(-point3()); @@ -162,13 +161,12 @@ Unit3 Unit3::retract(const Vector2& v) const { Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); - } /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector3 p = p_.vector(), q = y.p_.vector(); + Vector3 p = p_.vector(), q = y.unitVector(); double dot = p.dot(q); // Check for special cases diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 1d0c28ded..a30e816ed 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -66,7 +66,7 @@ TEST(Unit3, rotate) { Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; - // Use numerical derivatives to calculate the expected Jacobian + { expectedH = numericalDerivative21(rotate_, R, p); R.rotate(p, actualH, boost::none); @@ -90,8 +90,8 @@ 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 { expectedH = numericalDerivative21(unrotate_, R, p); R.unrotate(p, actualH, boost::none); @@ -113,7 +113,6 @@ TEST(Unit3, error) { EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; - // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); @@ -153,31 +152,29 @@ TEST(Unit3, distance) { } //******************************************************************************* -TEST(Unit3, localCoordinates0) { - Unit3 p; - Vector actual = p.localCoordinates(p); - 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, 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)); +TEST(Unit3, localCoordinates) { + { + Unit3 p; + Vector2 actual = p.localCoordinates(p); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(1, 6.12385e-21, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(-1, 0, 0); + Vector2 expected(M_PI, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(expected, actual, 1e-8)); + } } //******************************************************************************* TEST(Unit3, basis) { Unit3 p; - Matrix expected(3, 2); + Matrix32 expected; expected << 0, 0, 0, -1, 1, 0; Matrix actual = p.basis(); EXPECT(assert_equal(expected, actual, 1e-8)); @@ -185,20 +182,27 @@ TEST(Unit3, basis) { //******************************************************************************* 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)); @@ -228,9 +232,11 @@ inline static Vector randomVector(const Vector& minLimits, 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); + Vector3 minSphereLimit(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit(1.0, 1.0, 1.0); + Vector2 minXiLimit(-1.0, -1.0); + Vector2 maxXiLimit(1.0, 1.0); + for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -246,9 +252,9 @@ TEST(Unit3, localCoordinates_retract) { // 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)); + EXPECT(assert_equal(v12, actual_v12, 1e-6)); Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); + EXPECT(assert_equal(s2, actual_s2, 1e-6)); } } @@ -258,30 +264,26 @@ TEST(Unit3, localCoordinates_retract) { 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(); + Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0); + Vector2 minXiLimit = Vector2(-M_PI, -M_PI); + Vector2 maxXiLimit = Vector2(M_PI, M_PI); + for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). // boost::this_thread::sleep(boost::posix_time::milliseconds(0)); - - // Create the two Unit3s. - // Unlike the above case, we can use any two Unit3's. Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); // Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); - Vector v12 = randomVector(minXiLimit, maxXiLimit); + Vector v = 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); + if (v.norm() > M_PI) + v = v / M_PI; + Unit3 s2 = s1.retract(v); - // 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)); + EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6)); + EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6)); } } From ab81c98ac27d57391ca8317f5061cf38cc335b28 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sat, 27 Jun 2015 17:49:45 -0400 Subject: [PATCH 2/8] fix: correct some inappropriate floating point error, and its test --- gtsam/geometry/Unit3.cpp | 8 +-- gtsam/geometry/tests/testUnit3.cpp | 106 +++++++++++++++-------------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a74e39f47..5196b9477 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -148,12 +148,11 @@ Unit3 Unit3::retract(const Vector2& v) const { // Compute the 3D xi_hat vector Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - double xi_hat_norm = xi_hat.norm(); // Avoid nan - if (xi_hat_norm == 0.0) { - if (v.norm() == 0.0) + if (xi_hat_norm < 1e-16) { + if (v.norm() < 1e-16) return Unit3(point3()); else return Unit3(-point3()); @@ -162,13 +161,12 @@ Unit3 Unit3::retract(const Vector2& v) const { Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); - } /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector3 p = p_.vector(), q = y.p_.vector(); + Vector3 p = p_.vector(), q = y.unitVector(); double dot = p.dot(q); // Check for special cases diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 1d0c28ded..a30e816ed 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -66,7 +66,7 @@ TEST(Unit3, rotate) { Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-8)); Matrix actualH, expectedH; - // Use numerical derivatives to calculate the expected Jacobian + { expectedH = numericalDerivative21(rotate_, R, p); R.rotate(p, actualH, boost::none); @@ -90,8 +90,8 @@ 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 { expectedH = numericalDerivative21(unrotate_, R, p); R.unrotate(p, actualH, boost::none); @@ -113,7 +113,6 @@ TEST(Unit3, error) { EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; - // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); @@ -153,31 +152,29 @@ TEST(Unit3, distance) { } //******************************************************************************* -TEST(Unit3, localCoordinates0) { - Unit3 p; - Vector actual = p.localCoordinates(p); - 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, 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)); +TEST(Unit3, localCoordinates) { + { + Unit3 p; + Vector2 actual = p.localCoordinates(p); + EXPECT(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(1, 6.12385e-21, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(zero(2), actual, 1e-8)); + } + { + Unit3 p, q(-1, 0, 0); + Vector2 expected(M_PI, 0); + Vector2 actual = p.localCoordinates(q); + CHECK(assert_equal(expected, actual, 1e-8)); + } } //******************************************************************************* TEST(Unit3, basis) { Unit3 p; - Matrix expected(3, 2); + Matrix32 expected; expected << 0, 0, 0, -1, 1, 0; Matrix actual = p.basis(); EXPECT(assert_equal(expected, actual, 1e-8)); @@ -185,20 +182,27 @@ TEST(Unit3, basis) { //******************************************************************************* 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)); @@ -228,9 +232,11 @@ inline static Vector randomVector(const Vector& minLimits, 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); + Vector3 minSphereLimit(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit(1.0, 1.0, 1.0); + Vector2 minXiLimit(-1.0, -1.0); + Vector2 maxXiLimit(1.0, 1.0); + for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -246,9 +252,9 @@ TEST(Unit3, localCoordinates_retract) { // 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)); + EXPECT(assert_equal(v12, actual_v12, 1e-6)); Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-3)); + EXPECT(assert_equal(s2, actual_s2, 1e-6)); } } @@ -258,30 +264,26 @@ TEST(Unit3, localCoordinates_retract) { 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(); + Vector3 minSphereLimit = Vector3(-1.0, -1.0, -1.0); + Vector3 maxSphereLimit = Vector3(1.0, 1.0, 1.0); + Vector2 minXiLimit = Vector2(-M_PI, -M_PI); + Vector2 maxXiLimit = Vector2(M_PI, M_PI); + for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). // boost::this_thread::sleep(boost::posix_time::milliseconds(0)); - - // Create the two Unit3s. - // Unlike the above case, we can use any two Unit3's. Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); // Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); - Vector v12 = randomVector(minXiLimit, maxXiLimit); + Vector v = 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); + if (v.norm() > M_PI) + v = v / M_PI; + Unit3 s2 = s1.retract(v); - // 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)); + EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6)); + EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6)); } } From 4e9f365a7ed4cdd6d8bfc489210c0540a540a634 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Mon, 29 Jun 2015 18:28:07 -0400 Subject: [PATCH 3/8] test: add test for twist at singular value --- gtsam/geometry/tests/testUnit3.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a30e816ed..16c7cd0e7 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -41,6 +41,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) @@ -152,6 +153,7 @@ TEST(Unit3, distance) { } //******************************************************************************* + TEST(Unit3, localCoordinates) { { Unit3 p; @@ -169,6 +171,20 @@ TEST(Unit3, localCoordinates) { Vector2 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) + } } //******************************************************************************* @@ -252,9 +268,9 @@ TEST(Unit3, localCoordinates_retract) { // Check if the local coordinates and retract return the same results. Vector actual_v12 = s1.localCoordinates(s2); - EXPECT(assert_equal(v12, actual_v12, 1e-6)); + EXPECT(assert_equal(v12, actual_v12, 1e-8)); Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-6)); + EXPECT(assert_equal(s2, actual_s2, 1e-8)); } } From 3f5f0e852d1e7b1d5bc21c64174a884ca09fd9a2 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Mon, 29 Jun 2015 18:29:13 -0400 Subject: [PATCH 4/8] change: small value approximation for retract --- gtsam/geometry/Unit3.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5196b9477..1a08b7fcc 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -150,12 +150,9 @@ Unit3 Unit3::retract(const Vector2& v) const { Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); - // Avoid nan - if (xi_hat_norm < 1e-16) { - if (v.norm() < 1e-16) - return Unit3(point3()); - else - return Unit3(-point3()); + // When v is the so small and approximate as a direction + if (xi_hat_norm < 1e-8) { + return Unit3(cos(xi_hat_norm) * p + xi_hat); } Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p From 440a9557101e33e0dfad8dd896376b0f99d98bd1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 3 Jul 2015 11:33:30 -0700 Subject: [PATCH 5/8] Added missing includes --- gtsam/geometry/Unit3.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f8cea092e..e20b77739 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,11 +20,16 @@ #pragma once -#include #include +#include +#include +#include -#include #include +#include +#include + +#include namespace gtsam { From d6ffe54fd2cf744e5e741b5dad9ea4ed559eccc6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 3 Jul 2015 11:34:18 -0700 Subject: [PATCH 6/8] Cleaned up basis a bit --- gtsam/geometry/Unit3.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 1a08b7fcc..b760c8459 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -15,12 +15,12 @@ * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor + * @author Zhaoyang Lv * @brief The Unit3 class - basically a point on a unit sphere */ #include #include -#include #include // for GTSAM_USE_TBB #ifdef __clang__ @@ -85,26 +85,24 @@ const Matrix32& Unit3::basis() const { return *B_; // Get the axis of rotation with the minimum projected length of the point - Point3 axis; + Vector3 axis; double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); if ((mx <= my) && (mx <= mz)) - axis = Point3(1.0, 0.0, 0.0); + axis = Vector3(1.0, 0.0, 0.0); else if ((my <= mx) && (my <= mz)) - axis = Point3(0.0, 1.0, 0.0); + axis = Vector3(0.0, 1.0, 0.0); else if ((mz <= mx) && (mz <= my)) - axis = Point3(0.0, 0.0, 1.0); + axis = Vector3(0.0, 0.0, 1.0); else assert(false); // Create the two basis vectors - Point3 b1 = p_.cross(axis); - b1 = b1 / b1.norm(); - Point3 b2 = p_.cross(b1); - b2 = b2 / b2.norm(); + Vector3 b1 = p_.vector().cross(axis).normalized(); + Vector3 b2 = p_.vector().cross(b1).normalized(); // Create the basis matrix B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + (*B_) << b1, b2; return *B_; } From f0d10397718b97bec1192e3cc8dad0af2a527ce9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 3 Jul 2015 11:38:31 -0700 Subject: [PATCH 7/8] Removed temporaries --- gtsam/geometry/Unit3.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index b760c8459..82f6af9c1 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -120,10 +120,9 @@ Matrix3 Unit3::skew() const { /* ************************************************************************* */ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix23 Bt = basis().transpose(); - Vector2 xi = Bt * q.p_.vector(); + Vector2 xi = basis().transpose() * q.p_.vector(); if (H) - *H = Bt * q.basis(); + *H = basis().transpose() * q.basis(); return xi; } @@ -142,10 +141,9 @@ Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix Vector3 p = p_.vector(); - Matrix32 B = basis(); // Compute the 3D xi_hat vector - Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = basis() * v; double xi_hat_norm = xi_hat.norm(); // When v is the so small and approximate as a direction From 51983c30a66b68e951293978028069f7e40ca2f7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 3 Jul 2015 11:46:51 -0700 Subject: [PATCH 8/8] Switched to Vector3 altogether --- gtsam/geometry/Unit3.cpp | 32 +++++++++++++------------------- gtsam/geometry/Unit3.h | 28 ++++++++++++++-------------- 2 files changed, 27 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 82f6af9c1..a4153643e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -62,12 +62,10 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). - boost::variate_generator > - generator(rng, randomDirection); + boost::variate_generator > generator( + rng, randomDirection); vector d = generator(); - Unit3 result; - result.p_ = Point3(d[0], d[1], d[2]); - return result; + return Unit3(d[0], d[1], d[2]); } #ifdef GTSAM_USE_TBB @@ -97,8 +95,8 @@ const Matrix32& Unit3::basis() const { assert(false); // Create the two basis vectors - Vector3 b1 = p_.vector().cross(axis).normalized(); - Vector3 b2 = p_.vector().cross(b1).normalized(); + Vector3 b1 = p_.cross(axis).normalized(); + Vector3 b2 = p_.cross(b1).normalized(); // Create the basis matrix B_.reset(Matrix32()); @@ -120,7 +118,7 @@ Matrix3 Unit3::skew() const { /* ************************************************************************* */ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Vector2 xi = basis().transpose() * q.p_.vector(); + Vector2 xi = basis().transpose() * q.p_; if (H) *H = basis().transpose() * q.basis(); return xi; @@ -139,19 +137,16 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - // Get the vector form of the point and the basis matrix - Vector3 p = p_.vector(); - // Compute the 3D xi_hat vector Vector3 xi_hat = basis() * v; double xi_hat_norm = xi_hat.norm(); // When v is the so small and approximate as a direction if (xi_hat_norm < 1e-8) { - return Unit3(cos(xi_hat_norm) * p + xi_hat); + return Unit3(cos(xi_hat_norm) * p_ + xi_hat); } - Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_ + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } @@ -159,18 +154,17 @@ Unit3 Unit3::retract(const Vector2& v) const { /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector3 p = p_.vector(), q = y.unitVector(); - double dot = p.dot(q); + double dot = p_.dot(y.p_); // Check for special cases if (std::abs(dot - 1.0) < 1e-16) - return Vector2(0, 0); + return Vector2(0.0, 0.0); else if (std::abs(dot + 1.0) < 1e-16) - return Vector2(M_PI, 0); + return Vector2(M_PI, 0.0); else { // no special case - double theta = acos(dot); - Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); + const double theta = acos(dot); + Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index e20b77739..9d0444844 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -38,7 +38,7 @@ class GTSAM_EXPORT Unit3 { private: - Point3 p_; ///< The location of the point on the unit sphere + Vector3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis public: @@ -57,18 +57,18 @@ public: /// Construct from point explicit Unit3(const Point3& p) : - p_(p / p.norm()) { + p_(p.vector().normalized()) { } /// Construct from a vector3 explicit Unit3(const Vector3& p) : - p_(p / p.norm()) { + p_(p.normalized()) { } /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { - p_ = p_ / p_.norm(); + p_.normalize(); } /// Named constructor from Point3 with optional Jacobian @@ -88,7 +88,7 @@ public: /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { - return p_.equals(s.p_, tol); + return equal_with_abs_tol(p_, s.p_, tol); } /// @} @@ -106,22 +106,22 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const { + if (H) + *H = basis(); + return Point3(p_); + } + + /// Return unit-norm Vector + const Vector3& unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); return p_; } - /// Return unit-norm Vector - Vector3 unitVector(boost::optional H = boost::none) const { - if (H) - *H = basis(); - return (p_.vector()); - } - /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { - return s * d.p_; + return Point3(s * d.p_); } /// Signed, vector-valued error between two directions