From 89b582d1480ca116a4c3c138f8015677aceb7d7a Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Thu, 30 Jan 2014 13:05:55 -0500 Subject: [PATCH] 1 .Changed wrongly named getmultipleNormal() to normal 2. Added more limnearization points in unit test. --- gtsam/geometry/OrientedPlane3.h | 2 +- gtsam/slam/OrientedPlane3Factor.cpp | 8 ++--- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 33 ++++++++++++++----- 3 files changed, 29 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index f40b11088..5cdaeba93 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -101,7 +101,7 @@ public: /// Returns the plane coefficients (a, b, c, d) Vector planeCoefficients () const; - Sphere2 getPlanenormals () const { + Sphere2 normal () const { return n_; } diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index f4cf3ed04..25b6d5369 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -35,16 +35,16 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, if(H) { Matrix H_p; - Sphere2 n_hat_p = measured_p_.getPlanenormals(); - Sphere2 n_hat_q = plane.getPlanenormals(); + Sphere2 n_hat_p = measured_p_.normal(); + Sphere2 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 { - Sphere2 n_hat_p = measured_p_.getPlanenormals(); - Sphere2 n_hat_q = plane.getPlanenormals(); + Sphere2 n_hat_p = measured_p_.normal(); + Sphere2 n_hat_q = plane.normal(); Vector e = n_hat_p.error(n_hat_q); return e; } diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 0c0cc3bce..dd3ff7ce4 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -130,21 +130,36 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { OrientedPlane3DirectionPrior factor(lm_sym, planeOrientation, model); // Create a linearization point at the zero-error point - Vector theta = Vector_(4,0.0, 0.0, -20.0, 10.0); - OrientedPlane3 T(theta); - EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + Vector theta1 = Vector_(4,0.0, 0.02, -1.2, 10.0); + Vector theta2 = Vector_(4,0.0, 0.1, - 0.8, 10.0); + Vector theta3 = Vector_(4,0.0, 0.2, -0.9, 10.0); -// // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T); + 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 actualH; - factor.evaluateError(T, actualH); + 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(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); + EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); } /* ************************************************************************* */