diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index fa92905ff..f3e3a039f 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -60,28 +60,21 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& plane, +Vector3 OrientedPlane3::error(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - // Numerically calculate the derivative since this function doesn't provide - // one. + Vector2 n_error = -n_.localCoordinates(other.n_); + 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_); - ; + Matrix2 H_n_error_this = -numericalDerivative21(f, n_, other.n_); + *H1 << H_n_error_this, Z_2x1, 0, 0, 1; } if (H2) { - *H2 = -I_3x3; - H2->block<2, 2>(0, 0) = - -numericalDerivative22(f, n_, plane.n_); - ; + 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_ - plane.d_); + return Vector3(n_error(0), n_error(1), d_ - other.d_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index aaacca78a..2550761b1 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -96,9 +96,9 @@ public: /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. - * @param plane The other plane + * @param other The other plane */ - Vector3 error(const OrientedPlane3& plane, + Vector3 error(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; @@ -106,7 +106,7 @@ public: * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses * Unit3::localCoordinates. This one has correct derivatives. * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. - * @param the other plane + * @param other the other plane */ Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const;