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

Fixed Jacobians

release/4.3a0
dellaert 2014-01-25 21:20:18 -05:00
parent f924c21c19
commit 16071f5360
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/navigation/GPSFactor.h
View File

@ -90,7 +90,8 @@ public:
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
if (H) { if (H) {
H->resize(3, 6); H->resize(3, 6);
*H << zeros(3, 3) << eye(3); // TODO make static H->block < 3, 3 > (0, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix();
} }
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return nT_.localCoordinates(p.translation()); return nT_.localCoordinates(p.translation());

8
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -54,15 +54,15 @@ TEST( GPSFactor, Constructors ) {
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), pose); boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH;
factor.evaluateError(pose, actualH1); factor.evaluateError(pose, actualH);
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9)); CHECK(assert_equal(expectedH, actualH, 1e-8));
} }
// ************************************************************************* // *************************************************************************