diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 65cb1ca04..03f199355 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -382,10 +382,9 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_ pair GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, - const Ordering& ordering, + const Ordering& frontal, const Ordering& separator, const Dimensions& dimensions) { bool verbose = false; - Symbol key = ordering.front(); // Use in-place QR on dense Ab appropriate to NoiseModel if (verbose) model->print("Before QR"); @@ -394,15 +393,15 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, // get dimensions of the eliminated variable // TODO: this is another map find that should be avoided ! - size_t n1 = dimensions.at(key), n = Ab.size2() - 1; + size_t n1 = dimensions.at(frontal.front()), n = Ab.size2() - 1; // if mdim(); if (maxRank d(Ab,n); // create base conditional Gaussian - GaussianConditional::shared_ptr conditional(new GaussianConditional(key, + GaussianConditional::shared_ptr conditional(new GaussianConditional(frontal.front(), sub(d, 0, n1), // form d vector sub(Ab, 0, n1, 0, n1), // form R matrix sub(noiseModel->sigmas(),0,n1))); // get standard deviations @@ -418,8 +417,8 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, // extract the block matrices for parents in both CG and LF GaussianFactor::shared_ptr factor(new GaussianFactor); size_t j = n1; - BOOST_FOREACH(const Symbol& cur_key, ordering) - if (cur_key!=key) { + BOOST_FOREACH(const Symbol& cur_key, separator) + if (cur_key!=frontal.front()) { size_t dim = dimensions.at(cur_key); // TODO avoid find ! conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim)); factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly @@ -454,7 +453,8 @@ GaussianFactor::eliminate(const Symbol& key) const } // create an internal ordering that eliminates key first - Ordering ordering; + Ordering frontal, ordering; + frontal += key; ordering += key; BOOST_FOREACH(const Symbol& k, keys()) if (k != key) ordering += k; @@ -463,7 +463,8 @@ GaussianFactor::eliminate(const Symbol& key) const Matrix Ab = matrix_augmented(ordering,false); // TODO: this is where to split - return eliminateMatrix(Ab, model_, ordering, dimensions()); + ordering.pop_front(); + return eliminateMatrix(Ab, model_, frontal, ordering, dimensions()); } /* ************************************************************************* */ diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index cd6d4ffac..6142e10c2 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -248,8 +248,8 @@ public: */ static std::pair, shared_ptr> eliminateMatrix(Matrix& Ab, SharedDiagonal model, - const Ordering& ordering, - const Dimensions& dimensions); + const Ordering& frontal, const Ordering& separator, + const Dimensions& dimensions); /** * Take the factor f, and append to current matrices. Not very general. diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index ab1382de8..847d32bb6 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -223,7 +223,7 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { } // add the keys to the rendering - Ordering render; render += key; + Ordering frontal, render; frontal += key; render += key; BOOST_FOREACH(const Symbol& k, separator) if (k != key) render += k; @@ -235,8 +235,9 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { // eliminate that joint factor GaussianFactor::shared_ptr factor; GaussianConditional::shared_ptr conditional; + render.pop_front(); boost::tie(conditional, factor) = - GaussianFactor::eliminateMatrix(Ab, model, render, dimensions); + GaussianFactor::eliminateMatrix(Ab, model, frontal, render, dimensions); // add new factor on separator back into the graph if (!factor->empty()) push_back(factor); diff --git a/cpp/testJunctionTree.cpp b/cpp/testJunctionTree.cpp index 5c67af9d0..c8d148b17 100644 --- a/cpp/testJunctionTree.cpp +++ b/cpp/testJunctionTree.cpp @@ -62,7 +62,7 @@ TEST( GaussianFactorGraph, constructor ) } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( GaussianFactorGraph, optimizeMultiFrontal ) { // create a graph @@ -72,13 +72,13 @@ TEST( GaussianFactorGraph, optimizeMultiFrontal ) Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; // optimize the graph - LinearJunctionTree junctionTree(fg, ordering); - VectorConfig actual = junctionTree.optimize(); - - // verify - VectorConfig expected = createCorrectDelta(); - - CHECK(assert_equal(expected,actual)); +// GaussianJunctionTree junctionTree(fg, ordering); +// VectorConfig actual = junctionTree.optimize(); +// +// // verify +// VectorConfig expected = createCorrectDelta(); +// +// CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */