diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index db570e830..a07afdf6f 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -59,5 +59,38 @@ namespace gtsam { }; }; + class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { + + protected: + OrientedPlane3 measured_p_; /// measured plane parameters + Symbol landmarkSymbol_; + + typedef NoiseModelFactor1 Base; + + public: + + /// Constructor + OrientedPlane3DirectionPrior () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol + OrientedPlane3DirectionPrior (const Symbol& landmark, const Vector&z, + const SharedGaussian& noiseModel) + : Base (noiseModel, landmark), + landmarkSymbol_ (landmark) + { + measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3)); + } + + /// print + void print(const std::string& s) const; + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + virtual Vector evaluateError(const OrientedPlane3& plane, + boost::optional H = boost::none) const; + }; + } // gtsam diff --git a/gtsam/slam/tests/testOrientedPlane3DirectionPrior.cpp b/gtsam/slam/tests/testOrientedPlane3DirectionPrior.cpp deleted file mode 100644 index aeeb95b15..000000000 --- a/gtsam/slam/tests/testOrientedPlane3DirectionPrior.cpp +++ /dev/null @@ -1,10 +0,0 @@ -/* - * testOrientedPlane3DirectionPrior.cpp - * - * Created on: Jan 29, 2014 - * Author: nsrinivasan7 - */ - - - - diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 0b8f0331a..d274b5354 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -85,7 +85,7 @@ TEST (OrientedPlane3, lm_rotation_error) // Normal along -x, 3m away gtsam::Symbol lm_sym ('p', 0); gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); - + gtsam::ISAM2 isam2; gtsam::Values new_values; gtsam::NonlinearFactorGraph new_graph; @@ -115,6 +115,36 @@ TEST (OrientedPlane3, lm_rotation_error) EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); } +// ************************************************************************* +TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { + + // Example: pitch and roll of aircraft in an ENU Cartesian frame. + // If pitch and roll are zero for an aerospace frame, + // that means Z is pointing down, i.e., direction of Z = (0,0,-1) + + Sphere2 planeOrientation(0, 0, 1); // body Z axis + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + OrientedPlane3DirectionPriorFactor factor(key, planeOrientation, model); + + // Create a linearization point at the zero-error point + Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); + EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ int main() { srand(time(NULL));