diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 393c58dfb..dbf2757b8 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -447,11 +447,8 @@ namespace gtsam { size_t nj = Ab_(pos).cols(); Vector dj(nj); for (size_t k = 0; k < nj; ++k) { - Vector column_k; - if(model_) - column_k = model_->whiten(Ab_(pos).col(k)); - else - column_k = Ab_(pos).col(k); + Vector column_k = Ab_(pos).col(k); + if (model_) column_k = model_->whiten(column_k); dj(k) = dot(column_k,column_k); } d.insert(j,dj); @@ -465,7 +462,8 @@ namespace gtsam { for(size_t pos=0; posWhiten(Ab_(pos)); + Matrix Aj = Ab_(pos); + if (model_) Aj = model_->Whiten(Aj); blocks.insert(make_pair(j,Aj.transpose()*Aj)); } return blocks; diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index aacf38199..d280a5827 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -204,9 +204,54 @@ TEST(JacobianFactor, error) DOUBLES_EQUAL(expected_error, actual_error, 1e-10); } +/* ************************************************************************* */ +TEST(JacobianFactor, matrices_NULL) +{ + // Make sure everything works with NULL noise model + JacobianFactor factor(simple::terms, simple::b); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * augmentedJacobianExpected; + + // Hessian + EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information())); + EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation())); + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); + + // hessianDiagonal + VectorValues expectDiagonal; + expectDiagonal.insert(5, ones(3)); + expectDiagonal.insert(10, 4*ones(3)); + expectDiagonal.insert(15, 9*ones(3)); + EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(1*eye(3),actualBD[5])); + EXPECT(assert_equal(4*eye(3),actualBD[10])); + EXPECT(assert_equal(9*eye(3),actualBD[15])); +} + /* ************************************************************************* */ TEST(JacobianFactor, matrices) { + // And now witgh a non-unit noise model JacobianFactor factor(simple::terms, simple::b, simple::noise); Matrix jacobianExpected(3, 9);