diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index c50fd1ff2..b2b4ecc43 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -24,33 +24,39 @@ using namespace boost::assign; using namespace gtsam; using namespace std; +using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* -TEST (OrientedPlane3, transform) -{ +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)); + 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, + none, 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); + expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose); - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); - EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1, + none); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); } { - expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); + expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane); - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); - EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none, + actualH2); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } } @@ -66,23 +72,23 @@ 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; } //******************************************************************************* 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; + 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); @@ -92,7 +98,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { Vector v12 = randomVector(minXiLimit, maxXiLimit); // Magnitude of the rotation can be at most pi - if (v12.segment<3>(0).norm () > M_PI) + if (v12.segment<3>(0).norm() > M_PI) v12.segment<3>(0) = v12.segment<3>(0) / M_PI; OrientedPlane3 p2 = p1.retract(v12); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 47ed6780d..06c1a19f6 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -5,7 +5,6 @@ * Author: Natesh Srinivasan */ - #include "OrientedPlane3Factor.h" using namespace std; @@ -30,9 +29,10 @@ void OrientedPlane3DirectionPrior::print(const string& s, //*************************************************************************** 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); + return e != NULL && Base::equals(*e, tol) + && this->measured_p_.equals(e->measured_p_, tol); } //*************************************************************************** @@ -40,21 +40,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 67601fe03..b485c3165 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -26,24 +26,20 @@ protected: Vector measured_coeffs_; OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; public: /// Constructor - OrientedPlane3Factor () - {} + 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)); + 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 @@ -52,14 +48,15 @@ public: /// evaluateError 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); + 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_); + err << predicted_plane.error(measured_p_); return (err); - }; + } + ; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior @@ -67,21 +64,19 @@ class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { protected: OrientedPlane3 measured_p_; /// measured plane parameters Key landmarkKey_; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; public: typedef OrientedPlane3DirectionPrior This; /// Constructor - OrientedPlane3DirectionPrior () - {} + 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)); + 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 @@ -92,7 +87,7 @@ public: virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual Vector evaluateError(const OrientedPlane3& plane, - boost::optional H = boost::none) const; + boost::optional H = boost::none) const; }; } // gtsam diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 8832315bc..699829261 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -34,89 +34,99 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) // ************************************************************************* -TEST (OrientedPlane3Factor, lm_translation_error) -{ +TEST (OrientedPlane3Factor, 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::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::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); - + 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); + 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::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); - + 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); + gtsam::ISAM2Result result = isam2.update(new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate(); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at< + gtsam::OrientedPlane3>(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)); + gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } // ************************************************************************* -TEST (OrientedPlane3Factor, lm_rotation_error) -{ +TEST (OrientedPlane3Factor, 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::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); - + 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); + 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); + 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); - + 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); + gtsam::ISAM2Result result = isam2.update(new_graph, new_values); + gtsam::Values result_values = isam2.calculateEstimate(); + gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at< + gtsam::OrientedPlane3>(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)); + 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)); } // ************************************************************************* @@ -130,29 +140,31 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // Factor Key key(1); - SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); + 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); - + 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 expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T2); - Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T3); // Use the factor to calculate the derivative Matrix actualH1, actualH2, actualH3;