diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 5cfa80779..76a569ffa 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; @@ -58,7 +59,14 @@ 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. + auto f = boost::bind(&OrientedPlane3::Error, _1, _2); + if (H1) *H1 = numericalDerivative21(f, *this, plane); + if (H2) *H2 = numericalDerivative22(f, *this, plane); + Vector2 n_error = -n_.localCoordinates(plane.n_); return Vector3(n_error(0), n_error(1), d_ - plane.d_); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index ad6053234..e56e650ab 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -112,7 +112,9 @@ public: * The error is a norm 1 difference in tangent space. * @param 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); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 56968301a..699148e92 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -19,6 +19,33 @@ 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 { + Vector err(3); + + if (H1 || H2) { + Matrix36 predicted_H_pose; + Matrix33 predicted_H_plane, error_H_predicted; + + OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); + err << predicted_plane.error(measured_p_, error_H_predicted); + + // 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; + } + } else { + OrientedPlane3 predicted_plane = plane.transform(pose); + err << predicted_plane.error(measured_p_); + } + 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 d1f2329c4..282873b4e 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,7 +7,6 @@ #pragma once -#include #include #include @@ -48,31 +47,7 @@ public: /// evaluateError virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - Vector err(3); - - 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); - } - ; + boost::none) const; }; // 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 65491648f..6744dcd50 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -122,40 +122,34 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -double randDouble(double max = 1) { - return static_cast(rand()) / RAND_MAX * max; -} - TEST( OrientedPlane3Factor, Derivatives ) { - for (int i=0; i<100; i++) { - // Random measurement - OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble()); + // Measurement + OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); - // Random linearisation point - OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble()); - gtsam::Point3 pointLin = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100)); - gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI)); - Pose3 poseLin(rotationLin, pointLin); + // 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); + // 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); + // 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); + // 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-7)); - EXPECT(assert_equal(numericalH2, actualH2, 1e-7)); - } + // Verify we get the expected error + EXPECT(assert_equal(numericalH1, actualH1, 1e-8)); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); } // *************************************************************************