From 285e2da5a808c6b88d2d0f389d0ba3d0bab6d159 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:55:49 -0400 Subject: [PATCH 1/9] Fixed lint errors --- 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 00edc1ad4..e0e0ecb56 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -257,7 +257,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From e1466b2609daa9ca06bfef9e1732a6f8a9dde540 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:45:42 -0400 Subject: [PATCH 2/9] Fixed uninitialized problem --- gtsam_unstable/slam/SmartRangeFactor.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..2170c8599 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -83,6 +88,7 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { //gttic_(triangulate); @@ -96,7 +102,7 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles for(const Circle2& it: circles) { @@ -111,13 +117,14 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points + if (bestCircle2 && best_fh) { std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + circle1.center, bestCircle2->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; @@ -127,7 +134,10 @@ public: error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + } else { + throw std::runtime_error("triangulate failed"); + } + // gttoc_(triangulate); } /** From a34a9b8ff1512e62f738c6a276cd38204ec32f49 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:46:30 -0400 Subject: [PATCH 3/9] Fixed remaining lint errors --- gtsam_unstable/slam/SmartRangeFactor.h | 31 ++++++++++++-------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 2170c8599..3164b360e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -28,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -40,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -53,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -91,7 +89,7 @@ public: * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -105,11 +103,11 @@ public: boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -129,7 +127,7 @@ public: // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { error1 += distance2(it.center, p1); error2 += distance2(it.center, p2); } @@ -147,19 +145,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -178,8 +177,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam From 767c5d41ee8ef3914ce855997a984849702988ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 09:11:50 -0400 Subject: [PATCH 4/9] Showed compiler that B_ is always initialized --- gtsam/geometry/Unit3.cpp | 100 +++++++++++++++++++-------------------- 1 file changed, 49 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index e0e0ecb56..2c49f72e7 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -74,61 +74,59 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached basis if available and the Jacobian isn't needed. if (B_ && !H) { + // Return cached basis if available and the Jacobian isn't needed. return *B_; - } - - // Return cached basis and derivatives if available. - if (B_ && H && H_B_) { + } else if (B_ && H && H_B_) { + // Return cached basis and derivatives if available. *H = *H_B_; + return *B_; + } else { + B_.reset(Matrix32()); + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3 n(p_); + + // Get the axis of rotation with the minimum projected length of the point + Point3 axis(0, 0, 1); + 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)) { + axis = Point3(0.0, 1.0, 0.0); + } + + // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with + // the chosen axis. + Matrix33 H_B1_n; + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Matrix33 H_b2_n, H_b2_b1; + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + if (H) { + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } - - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - 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)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; - *H = *H_B_; - } - - return *B_; } /* ************************************************************************* */ @@ -257,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 6ec0ca798299a001f6cb8c87066c4275d27d71d4 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Fri, 12 Oct 2018 10:32:41 +0000 Subject: [PATCH 5/9] Fixed maybe-uninitialized warnings in Unit3::basis function --- gtsam/geometry/Unit3.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 2c49f72e7..21f66604e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -82,7 +82,6 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { *H = *H_B_; return *B_; } else { - B_.reset(Matrix32()); // Get the unit vector and derivative wrt this. // NOTE(hayk): We can't call point3(), because it would recursively call basis(). const Point3 n(p_); @@ -111,7 +110,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); // Create the basis by stacking b1 and b2. - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); if (H) { // Chain rule tomfoolery to compute the derivative. @@ -120,8 +121,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); *H = *H_B_; } From 6c09d8681c73cd3b21ab1bb8e587bd1633e777b9 Mon Sep 17 00:00:00 2001 From: AndreiCostinescu Date: Fri, 12 Oct 2018 19:10:18 -0400 Subject: [PATCH 6/9] Fixed warning in SmartRangeFactor.h --- gtsam_unstable/slam/SmartRangeFactor.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 3164b360e..08f6bf6ce 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor { // use best fh to find actual intersection points if (bestCircle2 && best_fh) { - std::list intersections = circleCircleIntersection( - circle1.center, bestCircle2->center, best_fh); + auto bestCircleCenter = bestCircle2->center; + std::list intersections = circleCircleIntersection( + circle1.center, bestCircleCenter, best_fh); - // pick winner based on other measurements - double error1 = 0, error2 = 0; - Point2 p1 = intersections.front(), p2 = intersections.back(); - for (const Circle2& it : circles) { - error1 += distance2(it.center, p1); - error2 += distance2(it.center, p2); - } - return (error1 < error2) ? p1 : p2; + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + for (const Circle2& it : circles) { + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); + } + return (error1 < error2) ? p1 : p2; } else { throw std::runtime_error("triangulate failed"); } From 540be68b803aac74be823933075d9d09571fd429 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Oct 2018 23:30:47 -0400 Subject: [PATCH 7/9] Refactored code paths to cover all 8 cases of H, B_, H_B_ with minimal calculation. Previous version was a bit hard to parse. Assign directly to B (formerly stacked) and jacobian (formerly derivative). --- gtsam/geometry/Unit3.cpp | 89 +++++++++++++++++++++------------------- 1 file changed, 47 insertions(+), 42 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 21f66604e..df1152487 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -68,67 +68,72 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I - // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or - // there is still a latent bug to watch out for. + // NOTE(hayk): At some point it seemed like this reproducably resulted in + // deadlock. However, I can't see reason why and I can no longer reproduce it. + // It may have been a red herring, or there is still a latent bug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - - if (B_ && !H) { - // Return cached basis if available and the Jacobian isn't needed. - return *B_; - } else if (B_ && H && H_B_) { - // Return cached basis and derivatives if available. - *H = *H_B_; - return *B_; - } else { - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); + Point3 n, axis; + if (!B_ || (H && !H_B_)) { + // Get the unit vector + // NOTE(hayk): can't call point3(), because would recursively call basis(). + n = Point3(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); + axis = Point3(0, 0, 1); 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)) { axis = Point3(0.0, 1.0, 0.0); } + } - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + if (H) { + if (!H_B_) { + // Compute Jacobian. Possibly recomputes B_ - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Matrix33 H_B1_n; + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix32 B; + Matrix33 H_b1_B1; + B.col(0) = normalize(B1, &H_b1_B1); - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); + // Get the second basis vector b2, which is orthogonal to n and b1. + Matrix33 H_b2_n, H_b2_b1; + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + // Chain rule tomfoolery to compute the jacobian. + Matrix62 jacobian; + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); - *H = *H_B_; + // Cache the result and jacobian + B_.reset(B); + H_B_.reset(jacobian); } - return *B_; + // Return cached jacobian, possibly computed just above + *H = *H_B_; } + + if (!B_) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + + return *B_; } /* ************************************************************************* */ From 88c4bd0a330043f4c4b3f50ced55fd793b6f3c48 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Sat, 13 Oct 2018 06:56:04 -0400 Subject: [PATCH 8/9] Second attempt at a logical refactor of Unit3::basis method --- gtsam/geometry/Unit3.cpp | 83 ++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 45 deletions(-) mode change 100644 => 100755 gtsam/geometry/Unit3.cpp diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp old mode 100644 new mode 100755 index 61bcbc457..d5ddc115b --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -69,70 +69,63 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB // NOTE(hayk): At some point it seemed like this reproducably resulted in - // deadlock. However, I can't see reason why and I can no longer reproduce it. - // It may have been a red herring, or there is still a latent bug. + // deadlock. However, I don't know why and I can no longer reproduce it. + // It either was a red herring or there is still a latent bug left to debug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - Point3 n, axis; - if (!B_ || (H && !H_B_)) { + + bool cachedBasis = static_cast(B_); + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; + + if (!cachedBasis) { // Get the unit vector - // NOTE(hayk): can't call point3(), because would recursively call basis(). - n = Point3(p_); + // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - axis = Point3(0, 0, 1); + Point3 axis(0, 0, 1); 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)) { axis = Point3(0.0, 1.0, 0.0); } + + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, through the cross-product of n and b1. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Point3 b2 = + gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); } if (H) { - if (!H_B_) { - // Compute Jacobian. Possibly recomputes B_ + if (!cachedBasis || !H_B_) { + // If Jacobian not cached or the basis was not cached, recompute it. + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Matrix33 H_B1_n; - const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix32 B; - Matrix33 H_b1_B1; - B.col(0) = normalize(B1, &H_b1_B1); - - // Get the second basis vector b2, which is orthogonal to n and b1. - Matrix33 H_b2_n, H_b2_b1; - B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - - // Chain rule tomfoolery to compute the jacobian. - Matrix62 jacobian; - const Matrix32& H_n_p = B; - jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; - auto H_b1_p = jacobian.block<3, 2>(0, 0); - jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the result and jacobian - B_.reset(B); - H_B_.reset(jacobian); + // Cache the derivative and fill the result. + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); } - // Return cached jacobian, possibly computed just above *H = *H_B_; } - if (!B_) { - // Same calculation as above, without derivatives. - // Done after H block, as that possibly computes B_ for the first time - Matrix32 B; - const Point3 B1 = gtsam::cross(n, axis); - B.col(0) = normalize(B1); - B.col(1) = gtsam::cross(n, B.col(0)); - B_.reset(B); - } - return *B_; } @@ -262,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 35d4bb1a7618aa70c7aeccbc31998f85699bdaf3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 12:18:25 -0400 Subject: [PATCH 9/9] After discussion with Andrei, a final version of logic, and new tests to check more cases. Tested with both typedef and old Point3 configs. --- gtsam/geometry/Unit3.cpp | 97 ++++++++++++++++-------------- gtsam/geometry/tests/testUnit3.cpp | 24 +++++--- 2 files changed, 70 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index d5ddc115b..08f14c829 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -65,6 +65,19 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return Unit3(d[0], d[1], d[2]); } +/* ************************************************************************* */ +// Get the axis of rotation with the minimum projected length of the point +static Point3 CalculateBestAxis(const Point3& n) { + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + return Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + return Point3(0.0, 1.0, 0.0); + } else { + return Point3(0, 0, 1); + } +} + /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB @@ -74,58 +87,54 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - bool cachedBasis = static_cast(B_); - Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - - if (!cachedBasis) { - // Get the unit vector - // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - 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)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, through the cross-product of n and b1. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Point3 b2 = - gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); - } + const bool cachedBasis = static_cast(B_); + const bool cachedJacobian = static_cast(H_B_); if (H) { - if (!cachedBasis || !H_B_) { - // If Jacobian not cached or the basis was not cached, recompute it. - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + if (!cachedJacobian) { + // Compute Jacobian. Recomputes B_ + Matrix32 B; + Matrix62 jacobian; + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + B.col(0) = normalize(B1, &H_b1_B1); + + // Get the second basis vector b2, which is orthogonal to n and b1. + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); + + // Chain rule tomfoolery to compute the jacobian. + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the result and jacobian + H_B_.reset(jacobian); + B_.reset(B); } + // Return cached jacobian, possibly computed just above *H = *H_B_; } + if (!cachedBasis) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + return *B_; } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 71093d668..542aca038 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -314,15 +314,24 @@ TEST(Unit3, basis) { Unit3 p(0.1, -0.2, 0.9); Matrix expected(3, 2); - expected << 0.0, -0.994169047, 0.97618706, - -0.0233922129, 0.216930458, 0.105264958; + expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958; Matrix62 actualH; - Matrix actual = p.basis(actualH); - EXPECT(assert_equal(expected, actual, 1e-6)); - Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + boost::bind(BasisTest, _1, boost::none), p); + + // without H, first time + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // without H, cached + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // with H, first time + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + + // with H, cached + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } @@ -432,7 +441,8 @@ TEST(Unit3, ErrorBetweenFactor) { // Add process factors using the dot product error function. SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); for (size_t i = 0; i < data.size() - 1; i++) { - Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + Expression exp(Expression(U(i)), &Unit3::errorVector, + Expression(U(i + 1))); graph.addExpressionFactor(R_process, Vector2::Zero(), exp); }