From 5087d36ab1987ccf661d2f21e0aa3f257a6899fb Mon Sep 17 00:00:00 2001 From: David Wisth Date: Mon, 15 Feb 2021 14:43:28 +0000 Subject: [PATCH] remove deprecated Unit3::error() which is replaced by Unit3::errorVector() --- gtsam/geometry/Unit3.cpp | 12 +----------- gtsam/geometry/Unit3.h | 4 ---- gtsam/geometry/tests/testUnit3.cpp | 28 ++-------------------------- gtsam/navigation/AttitudeFactor.cpp | 4 ++-- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/RotateFactor.h | 2 +- 6 files changed, 7 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 6f70d840e3..1756b055df 100644 --- 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 index 27d41a014d..158e3e9dd3 100644 --- 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 index 9929df21ae..35206b6abb 100644 --- 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( - boost::bind(&Unit3::error, &p, _1, boost::none), q); - p.error(q, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); - } - { - expected = numericalDerivative11( - 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 index 7f335152e3..512a257e70 100644 --- 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 index 185fae73f7..00f6b7b629 100644 --- 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 index 9e4091111c..3891bb7c9a 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -104,7 +104,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { /// vector of errors returns 2D vector Vector evaluateError(const Rot3& iRc, boost::optional 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);