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

Bug fix in converting HessianFactor to JacobianFactor - had to zero out lower triangle of eliminated matrix. Additional unit tests to catch this.

release/4.3a0
Richard Roberts 2012-01-18 20:44:27 +00:00
parent 8bf0b3c04c
commit 4b95a2799b
3 changed files with 41 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/JacobianFactor.cpp
View File

@ -164,6 +164,9 @@ namespace gtsam {
keys_ = factor.keys_;
Ab_.assignNoalias(factor.info_);
size_t maxrank = choleskyCareful(matrix_).first;
// Zero out lower triangle
matrix_.topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, matrix_.cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = noiseModel::Unit::Create(maxrank);

26
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -236,12 +236,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
// Marginal on 5
Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
JacobianFactor::shared_ptr actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(5, EliminateLDL));
LONGS_EQUAL(1, actualJacobian->rows());
LONGS_EQUAL(1, actualJacobian->size());
LONGS_EQUAL(5, actualJacobian->keys()[0]);
Matrix actualA = actualJacobian->getA(actualJacobian->begin());
JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(5, EliminateQR));
CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same
LONGS_EQUAL(1, actualJacobianLDL->rows());
LONGS_EQUAL(1, actualJacobianLDL->size());
LONGS_EQUAL(5, actualJacobianLDL->keys()[0]);
Matrix actualA = actualJacobianLDL->getA(actualJacobianLDL->begin());
Matrix actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
@ -252,12 +255,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
expectedCov = (Matrix(2,2) <<
1015.8, 2886.2,
2886.2, 8471.2).finished();
actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(6, EliminateLDL));
LONGS_EQUAL(2, actualJacobian->rows());
LONGS_EQUAL(1, actualJacobian->size());
LONGS_EQUAL(6, actualJacobian->keys()[0]);
actualA = actualJacobian->getA(actualJacobian->begin());
actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(6, EliminateQR));
CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same
LONGS_EQUAL(2, actualJacobianLDL->rows());
LONGS_EQUAL(1, actualJacobianLDL->size());
LONGS_EQUAL(6, actualJacobianLDL->keys()[0]);
actualA = actualJacobianLDL->getA(actualJacobianLDL->begin());
actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e1));

22
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -20,6 +20,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
using namespace std;
@ -70,6 +71,27 @@ TEST(JacobianFactor, constructor2)
EXPECT(assert_equal(b, actualb));
}
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) <<
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);
JacobianFactor expected(0, (Matrix(2,4) <<
1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(),
(Vector(2) << -6.2929, -5.7941).finished(),
sharedUnit(2));
JacobianFactor actual(hessian);
EXPECT(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST(JacobianFactor, error) {
Vector b = Vector_(3, 1., 2., 3.);