From 63ca74e49215bd96a425f91cac6a082109256a86 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 23 Sep 2011 02:50:46 +0000 Subject: [PATCH] Fixed bug in multifrontal marginals caused by backwards permutations with LDL (seems to be an inconsistency in Eigen?). Added GaussianConditional constructor from any number of frontal and parent variables. Added several new unit tests on marginals. Fixed small bug in GaussianConditional non-const get_d_ and get_R_ functions that didn't account for multiple frontal variables. --- gtsam/base/tests/testCholesky.cpp | 72 +++++++++ gtsam/linear/GaussianConditional.cpp | 30 +++- gtsam/linear/GaussianConditional.h | 27 +++- gtsam/linear/JacobianFactor.cpp | 2 +- .../linear/tests/testGaussianConditional.cpp | 78 +++++++++ .../linear/tests/testGaussianJunctionTree.cpp | 148 ++++++++++++++++++ tests/testGaussianJunctionTree.cpp | 45 ++++++ 7 files changed, 388 insertions(+), 14 deletions(-) diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index cbc45aff6..fe951767e 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -89,6 +89,78 @@ TEST(cholesky, ldlPartial) { EXPECT(assert_equal(expected, actual, 1e-9)); } +/* ************************************************************************* */ +TEST(cholesky, ldlPartial2) { + // choleskyPartial should only use the upper triangle, so this represents a + // symmetric matrix. + Matrix ABC = Matrix_(7,7, + 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, + 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, + 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992, + 0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347, + 0., 0., 0., 0., 3.0788, 2.6283, 2.3791, + 0., 0., 0., 0., 0., 2.9227, 2.4056, + 0., 0., 0., 0., 0., 0., 2.5776); + + // Do partial Cholesky on 3 frontal scalar variables + Matrix RSL(ABC); + Eigen::LDLT::TranspositionType permutation = ldlPartial(RSL, 6); + + // Compute permuted R (note transposed permutation - seems to be an inconsistency in Eigen!) + Matrix Rp = RSL.topLeftCorner(6,6) * permutation.transpose(); + + Matrix expected = Matrix(ABC.selfadjointView()).topLeftCorner(6,6); + Matrix actual = Rp.transpose() * Rp; + EXPECT(assert_equal(expected, actual)); + + // Directly call LDLT and reconstruct using left and right applications of permutations, + // the permutations need to be transposed between the two cases, which seems to be an + // inconsistency in Eigen! + Matrix A = ABC.topLeftCorner(6,6); + A = A.selfadjointView(); + Eigen::LDLT ldlt; + ldlt.compute(A); + Matrix R = ldlt.vectorD().cwiseSqrt().asDiagonal() * Matrix(ldlt.matrixU()); + Rp = R * ldlt.transpositionsP(); + Matrix actual1 = Matrix::Identity(6,6); + actual1 = Matrix(actual1 * ldlt.transpositionsP()); + actual1 *= ldlt.matrixL(); + actual1 *= ldlt.vectorD().asDiagonal(); + actual1 *= ldlt.matrixU(); + actual1 = Matrix(actual1 * ldlt.transpositionsP().transpose()); + Matrix actual2 = Matrix::Identity(6,6); + actual2 = Matrix(ldlt.transpositionsP() * actual2); + actual2 = ldlt.matrixU() * actual2; + actual2 = ldlt.vectorD().asDiagonal() * actual2; + actual2 = ldlt.matrixL() * actual2; + actual2 = Matrix(ldlt.transpositionsP().transpose() * actual2); + Matrix actual3 = ldlt.reconstructedMatrix(); + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual3)); + + // Again checking the difference between left and right permutation application + EXPECT(assert_equal(Matrix(eye(6) * ldlt.transpositionsP().transpose()), Matrix(ldlt.transpositionsP() * eye(6)))); + + // Same but with a manually-constructed permutation + Matrix I = eye(3); + Eigen::Transpositions p(3); + p.indices()[0] = 1; + p.indices()[1] = 1; + p.indices()[2] = 0; + Matrix IexpectedR = (Matrix(3,3) << + 0, 1, 0, + 0, 0, 1, + 1, 0, 0).finished(); + Matrix IexpectedL = (Matrix(3,3) << + 0, 0, 1, + 1, 0, 0, + 0, 1, 0).finished(); + EXPECT(assert_equal(IexpectedR, I*p)); + EXPECT(assert_equal(IexpectedL, p*I)); + EXPECT(assert_equal(IexpectedR, p.transpose()*I)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 959f2d751..91de58b50 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -87,7 +87,29 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri rsd_(j).noalias() = parent->second; ++ j; } - get_d_().noalias() = d; + get_d_() = d; +} + +/* ************************************************************************* */ +GaussianConditional::GaussianConditional(const std::list >& terms, + const size_t nrFrontals, const Vector& d, const Vector& sigmas) : + IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals), + rsd_(matrix_), sigmas_(sigmas) { + size_t dims[terms.size()+1]; + size_t j=0; + typedef pair Index_Matrix; + BOOST_FOREACH(const Index_Matrix& term, terms) { + dims[j] = term.second.cols(); + ++ j; + } + dims[j] = 1; + rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size())); + j=0; + BOOST_FOREACH(const Index_Matrix& term, terms) { + rsd_(j) = term.second; + ++ j; + } + get_d_() = d; } /* ************************************************************************* */ @@ -104,6 +126,7 @@ void GaussianConditional::print(const string &s) const } gtsam::print(Vector(get_d()),"d"); gtsam::print(sigmas_,"sigmas"); + cout << "Permutation: " << permutation_.indices() << endl; } /* ************************************************************************* */ @@ -139,11 +162,6 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const return true; } -/* ************************************************************************* */ -GaussianConditional::rsd_type::constBlock GaussianConditional::get_R() const { - return rsd_.range(0, nrFrontals()); -} - /* ************************************************************************* */ JacobianFactor::shared_ptr GaussianConditional::toFactor() const { return JacobianFactor::shared_ptr(new JacobianFactor(*this)); diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 3f4551863..9abb66909 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -31,6 +31,9 @@ class eliminate2JacobianFactorTest; class constructorGaussianConditionalTest; class eliminationGaussianFactorGraphTest; +class complicatedMarginalGaussianJunctionTreeTest; +class computeInformationGaussianConditionalTest; +class isGaussianFactorGaussianConditionalTest; namespace gtsam { @@ -105,12 +108,20 @@ public: Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas); /** - * constructor with number of arbitrary parents + * constructor with number of arbitrary parents (only used in unit tests, + * std::list is not efficient) * |Rx+sum(Ai*xi)-d| */ GaussianConditional(Index key, const Vector& d, const Matrix& R, const std::list >& parents, const Vector& sigmas); + /** + * Constructor with arbitrary number of frontals and parents (only used in unit tests, + * std::list is not efficient) + */ + GaussianConditional(const std::list >& terms, + size_t nrFrontals, const Vector& d, const Vector& sigmas); + /** * Constructor when matrices are already stored in a combined matrix, allows * for multiple frontal variables. @@ -131,13 +142,12 @@ public: /** Compute the information matrix */ Matrix computeInformation() const { - Matrix R = get_R(); - R = R*permutation_; - return R.transpose()*R; + Matrix R = get_R() * permutation_.transpose(); + return R.transpose() * R; } /** return stuff contained in GaussianConditional */ - rsd_type::constBlock get_R() const; + rsd_type::constBlock get_R() const { return rsd_.range(0, nrFrontals()); } /** access the d vector */ const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); } @@ -216,8 +226,8 @@ public: void scaleFrontalsBySigma(VectorValues& gy) const; protected: - rsd_type::Column get_d_() { return rsd_.column(1+nrParents(), 0); } - rsd_type::Block get_R_() { return rsd_(0); } + rsd_type::Column get_d_() { return rsd_.column(nrFrontals()+nrParents(), 0); } + rsd_type::Block get_R_() { return rsd_.range(0, nrFrontals()); } rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); } private: @@ -227,6 +237,9 @@ private: friend class ::eliminate2JacobianFactorTest; friend class ::constructorGaussianConditionalTest; friend class ::eliminationGaussianFactorGraphTest; + friend class ::complicatedMarginalGaussianJunctionTreeTest; + friend class ::computeInformationGaussianConditionalTest; + friend class ::isGaussianFactorGaussianConditionalTest; /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 77ba0f217..5c315c43d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -154,7 +154,7 @@ namespace gtsam { JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { Ab_.assignNoalias(cg.rsd_); // TODO: permutation for all frontal blocks - Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_; + Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_.transpose(); // todo SL: make firstNonzeroCols triangular? firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions assertInvariants(); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index a04562653..d211b2e06 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -382,6 +382,84 @@ TEST( GaussianConditional, solveTranspose ) { CHECK(assert_equal(y,actual)); } +/* ************************************************************************* */ +TEST( GaussianConditional, computeInformation ) { + + // Create R matrix + Matrix R = (Matrix(4,4) << + 1, 2, 3, 4, + 0, 5, 6, 7, + 0, 0, 8, 9, + 0, 0, 0, 10).finished(); + + // Shuffle columns + Eigen::Transpositions p(4); + p.indices()[0] = 1; + p.indices()[1] = 1; + p.indices()[2] = 2; + p.indices()[3] = 0; + + // The expected result of permuting R + Matrix RpExpected = (Matrix(4,4) << + 2, 4, 3, 1, + 5, 7, 6, 0, + 0, 9, 8, 0, + 0, 10,0, 0).finished(); + + // Check that the permutation does what we expect + Matrix RpActual = R * p.transpose(); + EXPECT(assert_equal(RpExpected, RpActual)); + + // Create conditional with the permutation + GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); + conditional.permutation_ = p; + + // Expected information matrix (using permuted R) + Matrix IExpected = RpExpected.transpose() * RpExpected; + + // Actual information matrix (conditional should permute R) + Matrix IActual = conditional.computeInformation(); + EXPECT(assert_equal(IExpected, IActual)); +} + +/* ************************************************************************* */ +TEST( GaussianConditional, isGaussianFactor ) { + + // Create R matrix + Matrix R = (Matrix(4,4) << + 1, 2, 3, 4, + 0, 5, 6, 7, + 0, 0, 8, 9, + 0, 0, 0, 10).finished(); + + // Shuffle columns + Eigen::Transpositions p(4); + p.indices()[0] = 1; + p.indices()[1] = 1; + p.indices()[2] = 2; + p.indices()[3] = 0; + + // The expected result of the permutation + Matrix RpExpected = (Matrix(4,4) << + 4, 1, 3, 2, + 7, 0, 6, 5, + 9, 0, 8, 0, + 10,0, 0, 0).finished(); + + // Create a conditional with this permutation + GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); + conditional.permutation_ = p; + + // Expected information matrix computed by conditional + Matrix IExpected = conditional.computeInformation(); + + // Expected information matrix computed by a factor + JacobianFactor jf = *conditional.toFactor(); + Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin()); + + EXPECT(assert_equal(IExpected, IActual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index de38f5b6d..6c2721bdf 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -25,7 +25,10 @@ using namespace boost::assign; #include +#include #include +#include +#include #include using namespace std; @@ -116,6 +119,151 @@ TEST_UNSAFE( GaussianJunctionTree, optimizeMultiFrontal ) EXPECT(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST_UNSAFE(GaussianJunctionTree, complicatedMarginal) { + + // Create the conditionals to go in the BayesTree + GaussianConditional::shared_ptr R_1_2(new GaussianConditional( + pair_list_of + (1, (Matrix(3,1) << + 0.2630, + 0, + 0).finished()) + (2, (Matrix(3,2) << + 0.7482, 0.2290, + 0.4505, 0.9133, + 0, 0.1524).finished()) + (5, (Matrix(3,1) << + 0.8258, + 0.5383, + 0.9961).finished()), + 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3))); + GaussianConditional::shared_ptr R_3_4(new GaussianConditional( + pair_list_of + (3, (Matrix(3,1) << + 0.0540, + 0, + 0).finished()) + (4, (Matrix(3,2) << + 0.9340, 0.4694, + 0.1299, 0.0119, + 0, 0.3371).finished()) + (6, (Matrix(3,2) << + 0.1622, 0.5285, + 0.7943, 0.1656, + 0.3112, 0.6020).finished()), + 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3))); + GaussianConditional::shared_ptr R_5_6(new GaussianConditional( + pair_list_of + (5, (Matrix(3,1) << + 0.2435, + 0, + 0).finished()) + (6, (Matrix(3,2) << + 0.1966, 0.4733, + 0.2511, 0.3517, + 0, 0.8308).finished()) + (7, (Matrix(3,1) << + 0.5853, + 0.5497, + 0.9172).finished()) + (8, (Matrix(3,2) << + 0.2858, 0.3804, + 0.7572, 0.5678, + 0.7537, 0.0759).finished()), + 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3))); + R_5_6->permutation_ = Eigen::Transpositions(2); + R_5_6->permutation_.indices()(0) = 0; + R_5_6->permutation_.indices()(1) = 2; + GaussianConditional::shared_ptr R_7_8(new GaussianConditional( + pair_list_of + (7, (Matrix(3,1) << + 0.2551, + 0, + 0).finished()) + (8, (Matrix(3,2) << + 0.8909, 0.1386, + 0.9593, 0.1493, + 0, 0.2575).finished()) + (11, (Matrix(3,1) << + 0.8407, + 0.2543, + 0.8143).finished()), + 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3))); + GaussianConditional::shared_ptr R_9_10(new GaussianConditional( + pair_list_of + (9, (Matrix(3,1) << + 0.7952, + 0, + 0).finished()) + (10, (Matrix(3,2) << + 0.4456, 0.7547, + 0.6463, 0.2760, + 0, 0.6797).finished()) + (11, (Matrix(3,1) << + 0.6551, + 0.1626, + 0.1190).finished()) + (12, (Matrix(3,2) << + 0.4984, 0.5853, + 0.9597, 0.2238, + 0.3404, 0.7513).finished()), + 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3))); + GaussianConditional::shared_ptr R_11_12(new GaussianConditional( + pair_list_of + (11, (Matrix(3,1) << + 0.0971, + 0, + 0).finished()) + (12, (Matrix(3,2) << + 0.3171, 0.4387, + 0.9502, 0.3816, + 0, 0.7655).finished()), + 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3))); + + // Gaussian Bayes Tree + typedef BayesTree GaussianBayesTree; + typedef GaussianBayesTree::Clique Clique; + typedef GaussianBayesTree::sharedClique sharedClique; + + // Create Bayes Tree + GaussianBayesTree bt; + bt.insert(sharedClique(new Clique(R_11_12))); + bt.insert(sharedClique(new Clique(R_9_10))); + bt.insert(sharedClique(new Clique(R_7_8))); + bt.insert(sharedClique(new Clique(R_5_6))); + bt.insert(sharedClique(new Clique(R_3_4))); + bt.insert(sharedClique(new Clique(R_1_2))); + + // Marginal on 5 + Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); + JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast( + 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()); + Matrix actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); + + // Marginal on 6 +// expectedCov = (Matrix(2,2) << +// 8471.2, 2886.2, +// 2886.2, 1015.8).finished(); + expectedCov = (Matrix(2,2) << + 1015.8, 2886.2, + 2886.2, 8471.2).finished(); + actualJacobian = boost::dynamic_pointer_cast( + 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()); + actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e1)); + +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 7f8470078..8c62dee7c 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -30,11 +30,16 @@ using namespace boost::assign; // Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h #define GTSAM_MAGIC_KEY +#include +#include +#include #include #include #include +#include #include #include +#include using namespace std; using namespace gtsam; @@ -183,7 +188,47 @@ TEST(GaussianJunctionTree, slamlike) { planarSLAM::Values expected = init.expmap(delta, ordering); EXPECT(assert_equal(expected, actual)); +} +/* ************************************************************************* */ +TEST(GaussianJunctionTree, simpleMarginal) { + + typedef BayesTree GaussianBayesTree; + + // Create a simple graph + pose2SLAM::Graph fg; + fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0)); + fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); + + pose2SLAM::Values init; + init.insert(0, Pose2()); + init.insert(1, Pose2(1.0, 0.0, 0.0)); + + Ordering ordering; + ordering += "x1", "x0"; + + GaussianFactorGraph gfg = *fg.linearize(init, ordering); + + // Compute marginals with both sequential and multifrontal + Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); + + Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); + + // Compute marginal directly from marginal factor + GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); + JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); + Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); + + // Compute marginal directly from BayesTree + GaussianBayesTree gbt; + gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateLDL)); + marginalFactor = gbt.marginalFactor(1, EliminateLDL); + marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); + Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); + + EXPECT(assert_equal(expected, actual1)); + EXPECT(assert_equal(expected, actual2)); + EXPECT(assert_equal(expected, actual3)); } /* ************************************************************************* */