diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index c682e4aa4..fa92905ff 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -17,11 +17,11 @@ * @brief A plane, represented by a normal direction and perpendicular distance */ +#include #include #include #include -#include using namespace std; @@ -34,8 +34,9 @@ void OrientedPlane3::print(const string& s) const { } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp, - OptionalJacobian<3, 6> Hr) const { +OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, + OptionalJacobian<3, 3> Hp, + OptionalJacobian<3, 6> Hr) const { Matrix23 D_rotated_plane; Matrix22 D_rotated_pose; Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); @@ -60,38 +61,44 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> /* ************************************************************************* */ Vector3 OrientedPlane3::error(const OrientedPlane3& plane, - OptionalJacobian<3,3> H1, - OptionalJacobian<3,3> H2) const { - // Numerically calculate the derivative since this function doesn't provide one. + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + // Numerically calculate the derivative since this function doesn't provide + // one. const auto f = boost::bind(&Unit3::localCoordinates, _1, _2); Vector2 n_error = -n_.localCoordinates(plane.n_); if (H1) { *H1 = I_3x3; - H1->block<2,2>(0,0) = -numericalDerivative21(f, n_, plane.n_);; + H1->block<2, 2>(0, 0) = + -numericalDerivative21(f, n_, plane.n_); + ; } if (H2) { *H2 = -I_3x3; - H2->block<2,2>(0,0) = -numericalDerivative22(f, n_, plane.n_);; + H2->block<2, 2>(0, 0) = + -numericalDerivative22(f, n_, plane.n_); + ; } return Vector3(n_error(0), n_error(1), d_ - plane.d_); } /* ************************************************************************* */ -Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, +Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { Matrix22 H_n_error_this, H_n_error_other; Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0, - H2 ? &H_n_error_other : 0); + H2 ? &H_n_error_other : 0); double d_error = d_ - other.d_; if (H1) { - *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1; + *H1 << H_n_error_this, Z_2x1, 0, 0, 1; } if (H2) { - *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1; + *H2 << H_n_error_other, Z_2x1, 0, 0, -1; } return Vector3(n_error(0), n_error(1), d_error); @@ -103,7 +110,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector3& v, Matrix22 H_n; Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); if (H) { - *H << H_n, Vector2::Zero(), 0, 0, 1; + *H << H_n, Z_2x1, 0, 0, 1; } return OrientedPlane3(n_retract, d_ + v(2)); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 446fc83c7..aaacca78a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -91,35 +91,16 @@ public: * @return the transformed plane */ OrientedPlane3 transform(const Pose3& xr, - OptionalJacobian<3, 3> Hp = boost::none, - OptionalJacobian<3, 6> Hr = boost::none) const; - - /** - * @deprecated the static method has wrong Jacobian order, - * please use the member method transform() - * @param The raw plane - * @param xr a transformation in current coordiante - * @param Hr optional jacobian wrpt the pose transformation - * @param Hp optional Jacobian wrpt the destination plane - * @return the transformed plane - */ - static OrientedPlane3 Transform(const OrientedPlane3& plane, - const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = boost::none) { - return plane.transform(xr, Hp, Hr); - } + 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 plane The other plane */ Vector3 error(const OrientedPlane3& plane, - OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2 = boost::none) const; - - static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { - return plane1.error(plane2); - } + 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 diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 36e566dca..e164382d7 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) { //******************************************************************************* -OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) { - return OrientedPlane3::Transform(plane, xr); -} - OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { return plane.transform(xr); } -TEST (OrientedPlane3, transform) { +TEST(OrientedPlane3, transform) { gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), - gtsam::Point3(2.0, 3.0, 4.0)); + gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); - OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, - none, none); - OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); - EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5)); - EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5)); + OrientedPlane3 transformedPlane = plane.transform(pose, none, none); + EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; - { - // because the Transform class uses a wrong order of Jacobians in interface - OrientedPlane3::Transform(plane, pose, actualH1, none); - expectedH1 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - OrientedPlane3::Transform(plane, pose, none, actualH2); - expectedH2 = numericalDerivative21(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); - } - { - plane.transform(pose, actualH1, none); - expectedH1 = numericalDerivative21(transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); - plane.transform(pose, none, actualH2); - expectedH2 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); - } + expectedH1 = numericalDerivative21(transform_, plane, pose); + plane.transform(pose, actualH1, none); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + + expectedH2 = numericalDerivative22(transform_, plane, pose); + plane.transform(pose, none, actualH2); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } //*******************************************************************************