12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1 .Changed wrongly named getmultipleNormal() to normal 

2. Added more limnearization points in unit test.
release/4.3a0
Natesh Srinivasan 2014-01-30 13:05:55 -05:00
parent 08b8e56579
commit 89b582d148
3 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/OrientedPlane3.h
View File

@ -101,7 +101,7 @@ public:
/// Returns the plane coefficients (a, b, c, d)
Vector planeCoefficients () const;
Sphere2 getPlanenormals () const {
Sphere2 normal () const {
return n_;
}

8
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -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;
}

33
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -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<OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T);
OrientedPlane3 T1(theta1);
OrientedPlane3 T2(theta2);
OrientedPlane3 T3(theta3);
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1);
Matrix expectedH2 = numericalDerivative11<OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2);
Matrix expectedH3 = numericalDerivative11<OrientedPlane3>(
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));
}
/* ************************************************************************* */