diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 9ee58233c..e4d2092ea 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -33,49 +33,48 @@ void OrientedPlane3::print(const std::string& s) const { /* ************************************************************************* */ - OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane, +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); + OptionalJacobian<3, 3> Hp) { + Matrix n_hr; + Matrix n_hp; + Unit3 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp); - 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); + 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); - } +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 { @@ -96,13 +95,13 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { } /* ************************************************************************* */ - Vector3 OrientedPlane3::planeCoefficients () const - { - Vector unit_vec = n_.unitVector (); - Vector3 a; - a << unit_vec[0], unit_vec[1], unit_vec[2], d_; - return a; - } +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 index 28b37638a..f5bd0ec2e 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -31,8 +31,8 @@ class OrientedPlane3: public DerivedValue { private: - Unit3 n_; /// The direction of the planar normal - double d_; /// The perpendicular distance to this plane + Unit3 n_; /// The direction of the planar normal + double d_; /// The perpendicular distance to this plane public: enum { dimension = 3 }; @@ -46,16 +46,16 @@ public: } /// Construct from a Unit3 and a distance - OrientedPlane3 (const Unit3& s, double d) - : n_ (s), - d_ (d) - {} + OrientedPlane3 (const Unit3& s, double d) + : n_ (s), + d_ (d) + {} - OrientedPlane3 (Vector vec) - : n_ (vec (0), vec (1), vec (2)), - d_ (vec (3)) - { - } + OrientedPlane3 (Vector vec) + : n_ (vec (0), vec (1), vec (2)), + d_ (vec (3)) + { + } /// Construct from a, b, c, d @@ -75,9 +75,9 @@ public: /// 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); + 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; @@ -104,7 +104,7 @@ public: inline Unit3 normal () const { return n_; } - + /// @} }; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 4a967ca58..14a387102 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -42,30 +42,30 @@ 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 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); + // 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)); - } - - } + 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 @@ -78,9 +78,9 @@ inline static Vector randomVector(const Vector& minLimits, // 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); - } + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } return vector; } @@ -102,7 +102,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { // 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; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index c12d25156..7ec72825b 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -23,7 +23,7 @@ void OrientedPlane3DirectionPrior::print(const string& s) const { //*************************************************************************** bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, - double tol) const { + double tol) const { const This* e = dynamic_cast(&expected); return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); } @@ -33,21 +33,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, 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; - } +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 index ea5fc6d3e..39dcc1f7d 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -16,85 +16,80 @@ namespace gtsam { -/** + /** * Factor to measure a planar landmark from a given pose */ - class OrientedPlane3Factor: public NoiseModelFactor2 { +class OrientedPlane3Factor: public NoiseModelFactor2 { - protected: - Symbol poseSymbol_; - Symbol landmarkSymbol_; - Vector3 measured_coeffs_; - OrientedPlane3 measured_p_; - - typedef NoiseModelFactor2 Base; +protected: + Symbol poseSymbol_; + Symbol landmarkSymbol_; + Vector3 measured_coeffs_; + OrientedPlane3 measured_p_; - 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)); - } + typedef NoiseModelFactor2 Base; - /// 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); - }; +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 { +// 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: - protected: - OrientedPlane3 measured_p_; /// measured plane parameters - Key landmarkKey_; + typedef OrientedPlane3DirectionPrior This; + /// Constructor + OrientedPlane3DirectionPrior () + {} - typedef NoiseModelFactor1 Base; + /// 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; - public: + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - 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; - }; + 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 index b4e1171f1..fa16a0f91 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -141,7 +141,7 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { 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 + // 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);