From 5c406dc7c72d2ac4c77f90b8c2c254147fa0b565 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Jul 2015 20:00:41 -0700 Subject: [PATCH 01/10] fixed serialization (no need to save B!) --- gtsam/geometry/Unit3.h | 9 +-------- gtsam/geometry/tests/testUnit3.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9d0444844..b7e243419 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -147,7 +147,7 @@ public: enum CoordinatesMode { EXPMAP, ///< Use the exponential map to retract - RENORM ///< Retract with vector addtion and renormalize. + RENORM ///< Retract with vector addition and renormalize. }; /// The retract function @@ -167,13 +167,6 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); - // homebrew serialize Eigen Matrix - ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); - ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); - ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); - ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); - ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); - ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); } /// @} diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 16c7cd0e7..b2f387d67 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -22,11 +22,13 @@ #include #include #include +#include + #include + #include #include #include -//#include #include #include @@ -365,6 +367,14 @@ TEST (Unit3, FromPoint3) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +/* ************************************************************************* */ +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)); From c8cf14d4b9a92b40f9c08df2e329f0f7bfd05b7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Jul 2015 20:00:56 -0700 Subject: [PATCH 02/10] Moved mutex around smaller block --- gtsam/geometry/Unit3.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a4153643e..aad128816 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -69,18 +69,13 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } #ifdef GTSAM_USE_TBB -tbb::mutex unit3BasisMutex; +static tbb::mutex unit3BasisMutex; #endif /* ************************************************************************* */ const Matrix32& Unit3::basis() const { -#ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); -#endif - // Return cached version if exists - if (B_) - return *B_; + if (B_) return *B_; // Get the axis of rotation with the minimum projected length of the point Vector3 axis; @@ -99,8 +94,13 @@ const Matrix32& Unit3::basis() const { Vector3 b2 = p_.cross(b1).normalized(); // Create the basis matrix - B_.reset(Matrix32()); - (*B_) << b1, b2; + { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(unit3BasisMutex); +#endif + B_.reset(Matrix32()); + (*B_) << b1, b2; + } return *B_; } From 48169e7776cc54fbb2e6894da673a834b4cd711b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Jul 2015 21:14:45 -0700 Subject: [PATCH 03/10] restored mutex to original scope --- gtsam/geometry/Unit3.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aad128816..922b1e940 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -74,6 +74,10 @@ static tbb::mutex unit3BasisMutex; /* ************************************************************************* */ const Matrix32& Unit3::basis() const { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(unit3BasisMutex); +#endif + // Return cached version if exists if (B_) return *B_; @@ -94,13 +98,8 @@ const Matrix32& Unit3::basis() const { Vector3 b2 = p_.cross(b1).normalized(); // Create the basis matrix - { -#ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); -#endif - B_.reset(Matrix32()); - (*B_) << b1, b2; - } + B_.reset(Matrix32()); + (*B_) << b1, b2; return *B_; } From 84950b3b4d2a11e1e83e84469839b9308a0fa4bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 05:17:23 +0000 Subject: [PATCH 04/10] Unit3.cpp edited online with Bitbucket --- gtsam/geometry/Unit3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 922b1e940..d70c17f4b 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -69,13 +69,13 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } #ifdef GTSAM_USE_TBB -static tbb::mutex unit3BasisMutex; +tbb::mutex unit3BasisMutex; #endif /* ************************************************************************* */ const Matrix32& Unit3::basis() const { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); + tbb::mutex::scoped_lock lock(unit3BasisMutex); #endif // Return cached version if exists From fcbceaf746ff2a5d70e3683f6894afb5a3903958 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 00:12:06 -0700 Subject: [PATCH 05/10] More tests --- gtsam/geometry/tests/testUnit3.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index b2f387d67..3f5f32e1d 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -158,20 +158,39 @@ TEST(Unit3, distance) { TEST(Unit3, localCoordinates) { { - Unit3 p; - Vector2 actual = p.localCoordinates(p); + 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); - CHECK(assert_equal(zero(2), actual, 1e-8)); + 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); - CHECK(assert_equal(expected, actual, 1e-8)); + 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)); } double twist = 1e-4; From 3245a2304eeecaa0de08a789658829bfd0cddf49 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 10:29:55 -0700 Subject: [PATCH 06/10] Improved small angle behavior of retract --- gtsam/geometry/Unit3.cpp | 43 +++++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 922b1e940..5142faeb5 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -38,6 +38,7 @@ #include #include +#include using namespace std; @@ -135,37 +136,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - // Compute the 3D xi_hat vector Vector3 xi_hat = basis() * v; - double xi_hat_norm = xi_hat.norm(); + double theta = 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); + // Treat case of very small v differently + if (theta < std::numeric_limits::epsilon()) { + return Unit3(cos(theta) * p_ + xi_hat); } - Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p_ - + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + Vector3 exp_p_xi_hat = + cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector2 Unit3::localCoordinates(const Unit3& y) const { - - double dot = p_.dot(y.p_); - - // Check for special cases - if (std::abs(dot - 1.0) < 1e-16) - return Vector2(0.0, 0.0); - else if (std::abs(dot + 1.0) < 1e-16) - return Vector2(M_PI, 0.0); - else { +Vector2 Unit3::localCoordinates(const Unit3& other) const { + const double x = p_.dot(other.p_); + // Crucial quantitity here is y = theta/sin(theta) with theta=acos(x) + // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) + // We treat the special caes 1 and -1 below + const double x2 = x * x; + const double z = 1 - x2; + double y; + if (z < std::numeric_limits::epsilon()) { + if (x > 0) // expand at x=1 + y = 1.0 - (x - 1.0) / 3.0; + else // cop out + return Vector2(M_PI, 0.0); + } else { // no special case - const double theta = acos(dot); - Vector3 result_hat = (theta / sin(theta)) * (y.p_ - p_ * dot); - return basis().transpose() * result_hat; + y = acos(x) / sqrt(z); } + return basis().transpose() * y * (other.p_ - x * p_); } /* ************************************************************************* */ From 297f8fcc9bff72226885c630e95450a8bf1fe2b0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 10:30:30 -0700 Subject: [PATCH 07/10] Replaced flaky random tests with more compact one that excludes degeneracies explicitly --- gtsam/geometry/tests/testUnit3.cpp | 138 +++++------------------------ 1 file changed, 20 insertions(+), 118 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 3f5f32e1d..a4227acdf 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -246,124 +246,6 @@ TEST(Unit3, retract_expmap) { 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; - 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). -// boost::this_thread::sleep(boost::posix_time::milliseconds(0)); - - // 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); - 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-8)); - Unit3 actual_s2 = s1.retract(actual_v12); - EXPECT(assert_equal(s2, actual_s2, 1e-8)); - } -} - -//******************************************************************************* -// 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; - 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)); - Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); -// Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); - Vector v = randomVector(minXiLimit, maxXiLimit); - - // Magnitude of the rotation can be at most pi - if (v.norm() > M_PI) - v = v / M_PI; - Unit3 s2 = s1.retract(v); - - EXPECT(assert_equal(v, s1.localCoordinates(s1.retract(v)), 1e-6)); - EXPECT(assert_equal(s2, s1.retract(s1.localCoordinates(s2)), 1e-6)); - } -} - -//******************************************************************************* -//TEST( Pose2, between ) -//{ -// // < -// // -// // ^ -// // -// // *--0--*--* -// Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y -// Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x -// -// Matrix actualH1,actualH2; -// Pose2 expected(M_PI/2.0, Point2(2,2)); -// Pose2 actual1 = gT1.between(gT2); -// Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); -// EXPECT(assert_equal(expected,actual1)); -// EXPECT(assert_equal(expected,actual2)); -// -// Matrix expectedH1 = (Matrix(3,3) << -// 0.0,-1.0,-2.0, -// 1.0, 0.0,-2.0, -// 0.0, 0.0,-1.0 -// ); -// Matrix numericalH1 = numericalDerivative21(testing::between, gT1, gT2); -// EXPECT(assert_equal(expectedH1,actualH1)); -// EXPECT(assert_equal(numericalH1,actualH1)); -// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx -// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); -// -// Matrix expectedH2 = (Matrix(3,3) << -// 1.0, 0.0, 0.0, -// 0.0, 1.0, 0.0, -// 0.0, 0.0, 1.0 -// ); -// Matrix numericalH2 = numericalDerivative22(testing::between, gT1, gT2); -// EXPECT(assert_equal(expectedH2,actualH2)); -// EXPECT(assert_equal(numericalH2,actualH2)); -// -//} - //******************************************************************************* TEST(Unit3, Random) { boost::mt19937 rng(42); @@ -375,6 +257,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; From 3a3088856edb0f2f3462439fca633d2a535f6dda Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 10:37:35 -0700 Subject: [PATCH 08/10] Merge remote-tracking branch 'origin/fix/Unit3Serialization' into fix/Unit3Serialization --- gtsam/geometry/Unit3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5142faeb5..c191e4fbe 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -76,7 +76,7 @@ static tbb::mutex unit3BasisMutex; /* ************************************************************************* */ const Matrix32& Unit3::basis() const { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(unit3BasisMutex); + tbb::mutex::scoped_lock lock(unit3BasisMutex); #endif // Return cached version if exists From 05a55b262a8a4c483eee25347a319030e68ccdf1 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sun, 12 Jul 2015 14:37:03 -0400 Subject: [PATCH 09/10] fix: type error in comments --- gtsam/geometry/Unit3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index e86121537..7729bd354 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -153,14 +153,14 @@ Unit3 Unit3::retract(const Vector2& v) const { /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); - // Crucial quantitity here is y = theta/sin(theta) with theta=acos(x) + // Crucial quantity here is y = theta/sin(theta) with theta=acos(x) // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) - // We treat the special caes 1 and -1 below + // We treat the special case 1 and -1 below const double x2 = x * x; const double z = 1 - x2; double y; if (z < std::numeric_limits::epsilon()) { - if (x > 0) // expand at x=1 + if (x > 0) // first order expansion at x=1 y = 1.0 - (x - 1.0) / 3.0; else // cop out return Vector2(M_PI, 0.0); From 6b1c411f9e485716e7abe0b0204c5e7852f8dce1 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sun, 12 Jul 2015 14:37:40 -0400 Subject: [PATCH 10/10] feature: add two small unit tests for local coordinate --- gtsam/geometry/tests/testUnit3.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a4227acdf..e55caaa3c 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -192,6 +192,16 @@ TEST(Unit3, localCoordinates) { 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)); + } double twist = 1e-4; {