diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index ff48ae5dc..c682e4aa4 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; @@ -58,8 +59,22 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { +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. + 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_);; + } + if (H2) { + *H2 = -I_3x3; + H2->block<2,2>(0,0) = -numericalDerivative22(f, n_, plane.n_);; + } return Vector3(n_error(0), n_error(1), d_ - plane.d_); } @@ -67,8 +82,8 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { 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.normal(), H1 ? &H_n_error_this : 0, - H2 ? &H_n_error_other : 0); + Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0, + H2 ? &H_n_error_other : 0); double d_error = d_ - other.d_; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 024fa2778..446fc83c7 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -56,8 +56,8 @@ public: } /// Construct from a Unit3 and a distance - OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + OrientedPlane3(const Unit3& n, double d) : + n_(n), d_(d) { } /// Construct from a vector of plane coefficients @@ -66,8 +66,7 @@ public: /// Construct from four numbers of plane coeffcients (a, b, c, d) OrientedPlane3(double a, double b, double c, double d) { - Point3 p(a, b, c); - n_ = Unit3(p); + n_ = Unit3(a, b, c); d_ = d; } @@ -112,9 +111,15 @@ public: /** Computes the error between two planes. * The error is a norm 1 difference in tangent space. - * @param the other plane + * @param plane The other plane */ - Vector3 error(const OrientedPlane3& plane) const; + 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); + } /** 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 dc5851319..36e566dca 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -138,7 +138,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { } //******************************************************************************* -TEST (OrientedPlane3, error2) { +TEST (OrientedPlane3, errorVector) { OrientedPlane3 plane1(-1, 0.1, 0.2, 5); OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); @@ -161,6 +161,27 @@ TEST (OrientedPlane3, error2) { 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); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index ddf60a256..9929df21a 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -501,6 +501,7 @@ TEST(actualH, Serialization) { EXPECT(serializationTestHelpers::equalsBinary(p)); } + /* ************************************************************************* */ int main() { srand(time(nullptr)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index cfefe1f22..7c86e68cc 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -2,7 +2,7 @@ * OrientedPlane3Factor.cpp * * Created on: Jan 29, 2014 - * Author: Natesh Srinivasan + * Author: Natesh Srinivasan */ #include "OrientedPlane3Factor.h" @@ -19,6 +19,28 @@ void OrientedPlane3Factor::print(const string& s, this->noiseModel_->print(" noise model: "); } +//*************************************************************************** +Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, + const OrientedPlane3& plane, boost::optional H1, + boost::optional H2) const { + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, + H1 ? &predicted_H_pose : nullptr); + Vector3 err = predicted_plane.error( + measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = error_H_predicted * predicted_H_pose; + } + if (H2) { + *H2 = error_H_predicted * predicted_H_plane; + } + + return err; +} + //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index c42ed222a..a6ab5528b 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -47,11 +47,8 @@ public: /// evaluateError Vector evaluateError( const Pose3& pose, const OrientedPlane3& plane, - boost::optional Hpose = boost::none, - boost::optional Hplane = boost::none) const override { - auto predicted_plane = plane.transform(pose, Hplane, Hpose); - return predicted_plane.error(measured_p_); - } + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 9f94ab3ac..b0592709f 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -87,7 +87,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away Symbol lm_sym('p', 0); - OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0); ISAM2 isam2; Values new_values; @@ -126,6 +126,36 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +TEST( OrientedPlane3Factor, Derivatives ) { + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); + + // Linearisation point + OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7); + gtsam::Point3 pointLin = gtsam::Point3(1, 2, -4); + gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI); + Pose3 poseLin(rotationLin, pointLin); + + // Factor + Key planeKey(1), poseKey(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); + + // Calculate numerical derivatives + boost::function f = boost::bind( + &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); + Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(poseLin, pLin, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); +} + // ************************************************************************* TEST( OrientedPlane3DirectionPrior, Constructor ) {