diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 61d8a30d2..ad6053234 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -114,6 +114,10 @@ public: */ Vector3 error(const OrientedPlane3& plane) 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 * Unit3::localCoordinates. This one has correct derivatives. diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 1f56adc45..d1f2329c4 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -48,10 +49,27 @@ public: virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, - H2); Vector err(3); - err << predicted_plane.error(measured_p_); + + if (H1 || H2) { + Matrix H1_1, H2_1; + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1); + err << predicted_plane.error(measured_p_); + // Numerically calculate the derivative since this function doesn't provide one. + auto f = boost::bind(&OrientedPlane3::Error, _1, _2); + Matrix H1_2 = numericalDerivative21(f, predicted_plane, measured_p_); + + // Apply the chain rule to calculate the derivatives. + if (H1) { + *H1 = H1_2 * H1_1; + } + if (H2) { + *H2 = H1_2 * H2_1; + } + } else { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); + err << predicted_plane.error(measured_p_); + } return (err); } ; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 36ae57201..65491648f 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -83,7 +83,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; @@ -153,8 +153,8 @@ TEST( OrientedPlane3Factor, Derivatives ) { 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)); + EXPECT(assert_equal(numericalH1, actualH1, 1e-7)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); } }