diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp new file mode 100644 index 000000000..088a84243 --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.cpp + * @date Dec 19, 2013 + * @author Alex Trevor + * @brief A plane, represented by a normal direction and perpendicular distance + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +/// The print fuction +void OrientedPlane3::print(const std::string& s) const { + Vector coeffs = planeCoefficients(); + cout << s << " : " << coeffs << endl; +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, + OptionalJacobian<3, 3> Hp) { + Matrix n_hr; + Matrix n_hp; + Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); + + Vector n_unit = plane.n_.unitVector(); + Vector unit_vec = n_rotated.unitVector(); + double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; + OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), + pred_d); + + if (Hr) { + *Hr = gtsam::zeros(3, 6); + (*Hr).block<2, 3>(0, 0) = n_hr; + (*Hr).block<1, 3>(2, 3) = unit_vec; + } + if (Hp) { + Vector xrp = xr.translation().vector(); + Matrix n_basis = plane.n_.basis(); + Vector hpp = n_basis.transpose() * xrp; + *Hp = gtsam::zeros(3, 3); + (*Hp).block<2, 2>(0, 0) = n_hp; + (*Hp).block<1, 2>(2, 0) = hpp; + (*Hp)(2, 2) = 1; + } + + return (transformed_plane); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { + Vector2 n_error = -n_.localCoordinates(plane.n_); + double d_error = d_ - plane.d_; + Vector3 e; + e << n_error(0), n_error(1), d_error; + return (e); +} + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { + // Retract the Unit3 + Vector2 n_v(v(0), v(1)); + Unit3 n_retracted = n_.retract(n_v); + double d_retracted = d_ + v(2); + return OrientedPlane3(n_retracted, d_retracted); +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { + Vector n_local = n_.localCoordinates(y.n_); + double d_local = d_ - y.d_; + Vector3 e; + e << n_local(0), n_local(1), -d_local; + return e; +} + +/* ************************************************************************* */ +Vector3 OrientedPlane3::planeCoefficients() const { + Vector unit_vec = n_.unitVector(); + Vector3 a; + a << unit_vec[0], unit_vec[1], unit_vec[2], d_; + return a; +} + +/* ************************************************************************* */ + +} diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h new file mode 100644 index 000000000..28b67cb4e --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.h @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.h + * @date Dec 19, 2013 + * @author Alex Trevor + * @author Frank Dellaert + * @brief An infinite plane, represented by a normal direction and perpendicular distance + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/// Represents an infinite plane in 3D. +class OrientedPlane3: public DerivedValue { + +private: + + Unit3 n_; /// The direction of the planar normal + double d_; /// The perpendicular distance to this plane + +public: + enum { + dimension = 3 + }; + /// @name Constructors + /// @{ + + /// Default constructor + OrientedPlane3() : + n_(), d_(0.0) { + } + + /// Construct from a Unit3 and a distance + OrientedPlane3(const Unit3& s, double d) : + n_(s), d_(d) { + } + + OrientedPlane3(Vector vec) : + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + } + + /// Construct from a, b, c, d + OrientedPlane3(double a, double b, double c, double d) { + Point3 p(a, b, c); + n_ = Unit3(p); + d_ = d; + } + + /// The print fuction + void print(const std::string& s = std::string()) const; + + /// The equals function with tolerance + bool equals(const OrientedPlane3& s, double tol = 1e-9) const { + return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); + } + + /// Transforms a plane to the specified pose + static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + OptionalJacobian<3, 3> Hp = boost::none); + + /// Computes the error between two poses + Vector3 error(const gtsam::OrientedPlane3& plane) const; + + /// Dimensionality of tangent space = 3 DOF + inline static size_t Dim() { + return 3; + } + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { + return 3; + } + + /// The retract function + OrientedPlane3 retract(const Vector& v) const; + + /// The local coordinates function + Vector3 localCoordinates(const OrientedPlane3& s) const; + + /// Returns the plane coefficients (a, b, c, d) + Vector3 planeCoefficients() const; + + inline Unit3 normal() const { + return n_; + } + + /// @} +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 611cf7cde..415410aa4 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -108,7 +108,7 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); Vector2 xi = Bt * q.p_.vector(); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 50ffb55b7..7d145c213 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -38,7 +38,9 @@ private: public: - enum { dimension = 2 }; + enum { + dimension = 2 + }; /// @name Constructors /// @{ @@ -65,8 +67,8 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = - boost::none); + static Unit3 FromPoint3(const Point3& point, // + OptionalJacobian<2, 3> H = boost::none); /// Random direction, using boost::uniform_on_sphere static Unit3 Random(boost::mt19937 & rng); @@ -99,24 +101,29 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { + const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { if (H) *H = basis(); return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector()); + } + /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { return s * d.p_; } /// Signed, vector-valued error between two directions - Vector2 error(const Unit3& q, - OptionalJacobian<2,2> H = boost::none) const; + Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions - double distance(const Unit3& q, - OptionalJacobian<1,2> H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// @} @@ -153,25 +160,27 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(p_); - // homebrew serialize Eigen Matrix - ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); - ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); - ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); - ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); - ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); - ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); - } + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(p_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); + } /// @} }; // Define GTSAM traits -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp new file mode 100644 index 000000000..14a387102 --- /dev/null +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3 class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +//******************************************************************************* +TEST (OrientedPlane3, transform) +{ + // Test transforming a plane to a pose + gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); + OrientedPlane3 plane (-1 , 0, 0, 5); + OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3); + OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none); + EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); + + // Test the jacobians of transform + Matrix actualH1, expectedH1, actualH2, expectedH2; + { + expectedH1 = numericalDerivative11(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); + EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + } + { + expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); + EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + } + +} + +//******************************************************************************* +// Returns a random vector -- copied from testUnit3.cpp +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +TEST(OrientedPlane3, localCoordinates_retract) { + + size_t numIterations = 10000; + gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + minPlaneLimit << -1.0, -1.0, -1.0, 0.01; + maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; + + Vector minXiLimit(3),maxXiLimit(3); + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; + for (size_t i = 0; i < numIterations; i++) { + + sleep(0); + + // Create a Plane + OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.segment<3>(0).norm () > M_PI) + v12.segment<3>(0) = v12.segment<3>(0) / M_PI; + OrientedPlane3 p2 = p1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = p1.localCoordinates(p2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + OrientedPlane3 actual_p2 = p1.retract(actual_v12); + EXPECT(assert_equal(p2, actual_p2, 1e-3)); + } +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7aa210dad..098af8b6d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -886,6 +886,7 @@ namespace gtsam { typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; + typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp new file mode 100644 index 000000000..7ec72825b --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -0,0 +1,54 @@ +/* + * OrientedPlane3Factor.cpp + * + * Created on: Jan 29, 2014 + * Author: Natesh Srinivasan + */ + + +#include "OrientedPlane3Factor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** + +void OrientedPlane3DirectionPrior::print(const string& s) const { + cout << "Prior Factor on " << landmarkKey_ << "\n"; + measured_p_.print("Measured Plane"); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** + +bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); +} + +//*************************************************************************** + +Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, + boost::optional H) const { + +if(H) { + Matrix H_p; + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q,H_p); + H->resize(2,3); + H->block <2,2>(0,0) << H_p; + H->block <2,1>(0,2) << Matrix::Zero(2, 1); + return e; +} else { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q); + return e; +} + +} +} + diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h new file mode 100644 index 000000000..7827a5c2c --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -0,0 +1,95 @@ +/* + * @file OrientedPlane3Factor.cpp + * @brief OrientedPlane3 Factor class + * @author Alex Trevor + * @date December 22, 2013 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * Factor to measure a planar landmark from a given pose + */ +class OrientedPlane3Factor: public NoiseModelFactor2 { + +protected: + Symbol poseSymbol_; + Symbol landmarkSymbol_; + Vector measured_coeffs_; + OrientedPlane3 measured_p_; + + typedef NoiseModelFactor2 Base; + +public: + + /// Constructor + OrientedPlane3Factor () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol + OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, + const Symbol& pose, + const Symbol& landmark) + : Base (noiseModel, pose, landmark), + poseSymbol_ (pose), + landmarkSymbol_ (landmark), + measured_coeffs_ (z) + { + measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + } + + /// print + void print(const std::string& s="PlaneFactor") const; + 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_); + return (err); + }; +}; + +// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior +class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { +protected: + OrientedPlane3 measured_p_; /// measured plane parameters + Key landmarkKey_; + typedef NoiseModelFactor1 Base; +public: + + typedef OrientedPlane3DirectionPrior This; + /// Constructor + OrientedPlane3DirectionPrior () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol + OrientedPlane3DirectionPrior (Key key, const Vector&z, + const SharedGaussian& noiseModel) + : Base (noiseModel, key), + landmarkKey_ (key) + { + measured_p_ = OrientedPlane3 (Unit3 (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/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp new file mode 100644 index 000000000..fa16a0f91 --- /dev/null +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +TEST (OrientedPlane3, lm_translation_error) +{ + // Tests one pose, two measurements of the landmark that differ in range only. + // 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; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + gtsam::Symbol init_sym ('x', 0); + gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), + gtsam::Point3 (0.0, 0.0, 0.0)); + gtsam::Vector sigmas(6); + sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; + gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) ); + new_values.insert (init_sym, init_pose); + new_graph.add (pose_prior); + + // Add two landmark measurements, differing in range + new_values.insert (lm_sym, test_lm0); + gtsam::Vector sigmas3(3); + sigmas3 << 0.1, 0.1, 0.1; + gtsam::Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); + new_graph.add (test_meas0); + gtsam::Vector test_meas1_mean(4); + test_meas1_mean << -1.0, 0.0, 0.0, 1.0; + gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); + new_graph.add (test_meas1); + + // Optimize + gtsam::ISAM2Result result = isam2.update (new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate (); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0); + EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); +} + +TEST (OrientedPlane3, lm_rotation_error) +{ + // Tests one pose, two measurements of the landmark that differ in angle only. + // 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; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + gtsam::Symbol init_sym ('x', 0); + gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), + gtsam::Point3 (0.0, 0.0, 0.0)); + gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); + new_values.insert (init_sym, init_pose); + new_graph.add (pose_prior); + +// // Add two landmark measurements, differing in angle + new_values.insert (lm_sym, test_lm0); + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << 0.0, -1.0, 0.0, 3.0; + gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas1); + + // Optimize + gtsam::ISAM2Result result = isam2.update (new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate (); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0); + 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) + + Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin + + // Factor + Key key(1); + SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); + OrientedPlane3DirectionPrior factor(key, planeOrientation, model); + + // Create a linearization point at the zero-error point + Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); + Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0); + Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); + + + OrientedPlane3 T1(theta1); + OrientedPlane3 T2(theta2); + OrientedPlane3 T3(theta3); + + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1); + + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); + + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2, actualH3; + factor.evaluateError(T1, actualH1); + factor.evaluateError(T2, actualH2); + factor.evaluateError(T3, actualH3); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); + EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */