Merged in fix/Unit3 (pull request #318)

Fix/Unit3

Approved-by: Andrei Costinescu <andrei.costinescu@yahoo.com>

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<bool>(B_);
+  const bool cachedJacobian = static_cast<bool>(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_);

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<Vector6, Unit3>(
-                         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<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
+    Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector,
+                            Expression<Unit3>(U(i + 1)));
     graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
   }
 

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<Point2> intersections = circleCircleIntersection(
-        circle1.center, bestCircle2->center, best_fh);
+      auto bestCircleCenter = bestCircle2->center;
+      std::list<Point2> 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");
     }