diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp old mode 100644 new mode 100755 index e0e0ecb56..08f14c829 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -65,69 +65,76 @@ 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 - // 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 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 - // Return cached basis if available and the Jacobian isn't needed. - if (B_ && !H) { - return *B_; - } - - // Return cached basis and derivatives if available. - if (B_ && H && H_B_) { - *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(); + const bool cachedBasis = static_cast(B_); + const bool cachedJacobian = static_cast(H_B_); 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; + 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. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; + // 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_; } @@ -257,7 +264,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_); 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); } diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 3164b360e..a844ee823 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"); }