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)); } /* ************************************************************************* */