diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index f3e3a039f..156f7e03f 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -17,9 +17,7 @@ * @brief A plane, represented by a normal direction and perpendicular distance */ -#include #include -#include #include @@ -59,24 +57,6 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } -/* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2) const { - Vector2 n_error = -n_.localCoordinates(other.n_); - - const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); - if (H1) { - Matrix2 H_n_error_this = -numericalDerivative21(f, n_, other.n_); - *H1 << H_n_error_this, Z_2x1, 0, 0, 1; - } - if (H2) { - Matrix H_n_error_other = -numericalDerivative22(f, n_, other.n_); - *H2 << H_n_error_other, Z_2x1, 0, 0, -1; - } - return Vector3(n_error(0), n_error(1), d_ - other.d_); -} - /* ************************************************************************* */ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 2550761b1..1ff21cd9c 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -20,13 +20,9 @@ #pragma once -#include #include #include -#include -#include - namespace gtsam { /** @@ -94,14 +90,6 @@ public: OptionalJacobian<3, 3> Hp = boost::none, OptionalJacobian<3, 6> Hr = boost::none) const; - /** Computes the error between two planes. - * The error is a norm 1 difference in tangent space. - * @param other The other plane - */ - Vector3 error(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; - /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index e164382d7..6f62ab4e2 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -144,27 +144,6 @@ TEST (OrientedPlane3, errorVector) { EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } -//******************************************************************************* -TEST (OrientedPlane3, error) { - // Hard-coded regression values, to ensure the result doesn't change. - OrientedPlane3 plane1(-1, 0.1, 0.2, 5); - OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); - - // Test the jacobians of transform - Matrix33 actualH1, expectedH1, actualH2, expectedH2; - Vector3 actual = plane1.error(plane2, actualH1, actualH2); - - EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8)); - EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5)); - - boost::function f = // - boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none); - expectedH1 = numericalDerivative21(f, plane1, plane2); - expectedH2 = numericalDerivative22(f, plane1, plane2); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); -} - //******************************************************************************* TEST (OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5);