remove deprecated Unit3::error() which is replaced by Unit3::errorVec…

…tor()
borglab · Feb 15, 2021 · 5087d36 · 5087d36
1 parent 0ad488c
commit 5087d36
Show file tree

Hide file tree

Showing 6 changed files with 7 additions and 45 deletions.
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
@@ -187,16 +187,6 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
   return d;
 }
 
-/* ************************************************************************* */
-Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
-  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
-  const Vector2 xi = basis().transpose() * q.p_;
-  if (H_q) {
-    *H_q = basis().transpose() * q.basis();
-  }
-  return xi;
-}
-
 /* ************************************************************************* */
 Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
                            OptionalJacobian<2, 2> H_q) const {
@@ -236,7 +226,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
 /* ************************************************************************* */
 double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
   Matrix2 H_xi_q;
-  const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
+  const Vector2 xi = errorVector(q, boost::none, H ? &H_xi_q : nullptr);
   const double theta = xi.norm();
   if (H)
     *H = (xi.transpose() / theta) * H_xi_q;

diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
@@ -152,10 +152,6 @@ class Unit3 {
   GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
                              OptionalJacobian<1,2> H2 = boost::none) const;
 
-  /// Signed, vector-valued error between two directions
-  /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
-  GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
-
   /// Signed, vector-valued error between two directions
   /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
   GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //

diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
@@ -145,30 +145,6 @@ TEST(Unit3, dot) {
   }
 }
 
-//*******************************************************************************
-TEST(Unit3, error) {
-  Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
-  r = p.retract(Vector2(0.8, 0));
-  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
-  EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
-  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<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), q);
-    p.error(q, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
-  }
-  {
-    expected = numericalDerivative11<Vector2,Unit3>(
-        boost::bind(&Unit3::error, &p, _1, boost::none), r);
-    p.error(r, actual);
-    EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
-  }
-}
-
 //*******************************************************************************
 TEST(Unit3, error2) {
   Unit3 p(0.1, -0.2, 0.8);
@@ -487,10 +463,10 @@ TEST(Unit3, ErrorBetweenFactor) {
 TEST(Unit3, CopyAssign) {
   Unit3 p{1, 0.2, 0.3};
 
-  EXPECT(p.error(p).isZero());
+  EXPECT(p.errorVector(p).isZero());
 
   p = Unit3{-1, 2, 8};
-  EXPECT(p.error(p).isZero());
+  EXPECT(p.errorVector(p).isZero());
 }
 
 /* ************************************************************************* */

diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp
@@ -29,13 +29,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
     Matrix23 D_nRef_R;
     Matrix22 D_e_nRef;
     Unit3 nRef = nRb.rotate(bRef_, D_nRef_R);
-    Vector e = nZ_.error(nRef, D_e_nRef);
+    Vector e = nZ_.errorVector(nRef, boost::none, D_e_nRef);
 
     (*H) = D_e_nRef * D_nRef_R;
     return e;
   } else {
     Unit3 nRef = nRb * bRef_;
-    return nZ_.error(nRef);
+    return nZ_.errorVector(nRef);
   }
 }
 

diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp
@@ -67,7 +67,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(
   Unit3 n_hat_p = measured_p_.normal();
   Unit3 n_hat_q = plane.normal();
   Matrix2 H_p;
-  Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr);
+  Vector e = n_hat_p.errorVector(n_hat_q, boost::none, H ? &H_p : nullptr);
   if (H) {
     H->resize(2, 3);
     *H << H_p, Z_2x1;

diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h
@@ -104,7 +104,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
   /// vector of errors returns 2D vector
   Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
     Unit3 i_q = iRc * c_z_;
-    Vector error = i_p_.error(i_q, H);
+    Vector error = i_p_.errorVector(i_q, boost::none, H);
     if (H) {
       Matrix DR;
       iRc.rotate(c_z_, DR);