diff --git a/.cproject b/.cproject index 33cca43b3..ccef8a099 100644 --- a/.cproject +++ b/.cproject @@ -302,6 +302,7 @@ make + all true true @@ -309,6 +310,7 @@ make + clean true true @@ -316,6 +318,7 @@ make + check true true @@ -323,6 +326,7 @@ make + testGaussianConditional.run true true @@ -330,6 +334,7 @@ make + testGaussianFactor.run true true @@ -337,6 +342,7 @@ make + timeGaussianFactor.run true true @@ -344,6 +350,7 @@ make + timeVectorConfig.run true true @@ -351,6 +358,7 @@ make + testVectorBTree.run true true @@ -358,6 +366,7 @@ make + testVectorMap.run true true @@ -365,6 +374,7 @@ make + testNoiseModel.run true true @@ -372,6 +382,7 @@ make + testBayesNetPreconditioner.run true true @@ -379,6 +390,7 @@ make + testErrors.run true false @@ -386,7 +398,6 @@ make - check true true @@ -394,14 +405,22 @@ make + check true true true + + make + + tests/testGaussianJunctionTree.run + true + true + true + make - check true true @@ -409,7 +428,6 @@ make - clean true true @@ -417,7 +435,6 @@ make - testBTree.run true true @@ -425,7 +442,6 @@ make - testDSF.run true true @@ -433,7 +449,6 @@ make - testDSFVector.run true true @@ -441,7 +456,6 @@ make - testMatrix.run true true @@ -449,7 +463,6 @@ make - testSPQRUtil.run true true @@ -457,7 +470,6 @@ make - testVector.run true true @@ -465,7 +477,6 @@ make - timeMatrix.run true true @@ -473,7 +484,6 @@ make - all true true @@ -481,6 +491,7 @@ make + all true true @@ -488,6 +499,7 @@ make + clean true true @@ -503,6 +515,7 @@ make + testBayesTree.run true false @@ -510,6 +523,7 @@ make + testBinaryBayesNet.run true false @@ -517,6 +531,7 @@ make + testFactorGraph.run true true @@ -524,6 +539,7 @@ make + testISAM.run true true @@ -531,6 +547,7 @@ make + testJunctionTree.run true true @@ -538,6 +555,7 @@ make + testKey.run true true @@ -545,6 +563,7 @@ make + testOrdering.run true true @@ -552,6 +571,7 @@ make + testSymbolicBayesNet.run true false @@ -559,6 +579,7 @@ make + testSymbolicFactor.run true false @@ -566,6 +587,7 @@ make + testSymbolicFactorGraph.run true false @@ -573,6 +595,7 @@ make + timeSymbolMaps.run true true @@ -580,6 +603,7 @@ make + check true true @@ -587,6 +611,7 @@ make + testClusterTree.run true true @@ -594,6 +619,7 @@ make + testJunctionTree.run true true @@ -601,6 +627,7 @@ make + testEliminationTree.run true true @@ -608,7 +635,6 @@ make - check true true @@ -616,7 +642,6 @@ make - testGaussianFactorGraph.run true true @@ -624,7 +649,6 @@ make - testGaussianISAM.run true true @@ -632,7 +656,6 @@ make - testGaussianISAM2.run true true @@ -640,7 +663,6 @@ make - testGraph.run true false @@ -648,7 +670,6 @@ make - testIterative.run true true @@ -656,7 +677,6 @@ make - testNonlinearEquality.run true true @@ -664,7 +684,6 @@ make - testNonlinearFactor.run true true @@ -672,7 +691,6 @@ make - testNonlinearFactorGraph.run true true @@ -680,7 +698,6 @@ make - testNonlinearOptimizer.run true true @@ -688,7 +705,6 @@ make - testSQP.run true true @@ -696,7 +712,6 @@ make - testSubgraphPreconditioner.run true true @@ -704,7 +719,6 @@ make - testTupleConfig.run true true @@ -712,7 +726,6 @@ make - timeGaussianFactorGraph.run true true @@ -720,7 +733,6 @@ make - testBayesNetPreconditioner.run true true @@ -728,7 +740,6 @@ make - testConstraintOptimizer.run true true @@ -736,7 +747,6 @@ make - testInference.run true false @@ -744,7 +754,6 @@ make - testGaussianBayesNet.run true false @@ -752,7 +761,6 @@ make - testGaussianFactor.run true false @@ -760,7 +768,6 @@ make - testJunctionTree.run true false @@ -768,7 +775,6 @@ make - testSymbolicBayesNet.run true false @@ -776,7 +782,6 @@ make - testSymbolicFactorGraph.run true false @@ -784,6 +789,7 @@ make + clean true true @@ -791,6 +797,7 @@ make + all true true @@ -798,6 +805,7 @@ make + testNonlinearConstraint.run true true @@ -805,6 +813,7 @@ make + testLieConfig.run true true @@ -812,6 +821,7 @@ make + testConstraintOptimizer.run true true @@ -819,6 +829,7 @@ make + install true true @@ -826,6 +837,7 @@ make + clean true true @@ -833,7 +845,6 @@ make - all true true @@ -841,7 +852,6 @@ make - clean true true @@ -849,7 +859,6 @@ make - all true true @@ -857,7 +866,6 @@ make - clean true true @@ -865,7 +873,6 @@ make - all true true @@ -873,7 +880,6 @@ make - clean true true @@ -881,6 +887,7 @@ make + all true true @@ -888,6 +895,7 @@ make + clean true true @@ -895,7 +903,6 @@ make - clean all true true @@ -903,7 +910,6 @@ make - all true true @@ -911,7 +917,6 @@ make - check true true @@ -919,7 +924,6 @@ make - clean true true @@ -927,7 +931,6 @@ make - testPlanarSLAM.run true true @@ -935,7 +938,6 @@ make - testPose2Config.run true true @@ -943,7 +945,6 @@ make - testPose2Factor.run true true @@ -951,7 +952,6 @@ make - testPose2Prior.run true true @@ -959,7 +959,6 @@ make - testPose2SLAM.run true true @@ -967,7 +966,6 @@ make - testPose3Config.run true true @@ -975,7 +973,6 @@ make - testPose3SLAM.run true true @@ -983,7 +980,6 @@ make - testSimulated2DOriented.run true false @@ -991,7 +987,6 @@ make - testVSLAMConfig.run true true @@ -999,7 +994,6 @@ make - testVSLAMFactor.run true true @@ -1007,7 +1001,6 @@ make - testVSLAMGraph.run true true @@ -1015,7 +1008,6 @@ make - testPose3Factor.run true true @@ -1023,7 +1015,6 @@ make - testSimulated2D.run true false @@ -1031,7 +1022,6 @@ make - testSimulated3D.run true false @@ -1039,7 +1029,6 @@ make - check true true @@ -1047,15 +1036,6 @@ make - - check - true - true - true - - - make - check true true @@ -1079,11 +1059,103 @@ make + clean true true true + + make + check + true + true + true + + + make + testRot3.run + true + true + true + + + make + testRot2.run + true + true + true + + + make + testPose3.run + true + true + true + + + make + timeRot3.run + true + true + true + + + make + testPose2.run + true + true + true + + + make + testCal3_S2.run + true + true + true + + + make + testSimpleCamera.run + true + true + true + + + make + testHomography2.run + true + true + true + + + make + testCalibratedCamera.run + true + true + true + + + make + check + true + true + true + + + make + clean + true + true + true + + + make + testPoint2.run + true + true + true + make -j2 @@ -1110,7 +1182,6 @@ make - all true true @@ -1118,117 +1189,38 @@ make - dist true true true - - make - - testRot3.run - true - true - true - - - make - - testRot2.run - true - true - true - - - make - - testPose3.run - true - true - true - - - make - - timeRot3.run - true - true - true - - - make - - testPose2.run - true - true - true - - - make - - testCal3_S2.run - true - true - true - - - make - - testSimpleCamera.run - true - true - true - - - make - - testHomography2.run - true - true - true - - - make - - testCalibratedCamera.run - true - true - true - - - make - - check - true - true - true - - - make - - clean - true - true - true - - - make - - testPoint2.run - true - true - true - make + check true true true + + make + + testGaussianJunctionTree.run + true + true + true + + + make + + testGaussianFactorGraph.run + true + true + true + make + check true true @@ -1236,6 +1228,7 @@ make + clean true true @@ -1243,6 +1236,7 @@ make + install true true @@ -1250,6 +1244,7 @@ make + all true true diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index 2fca9d05d..6dd367731 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -15,7 +15,7 @@ namespace gtsam { /** - * An elimination tree (see Gilbert01bit) associated with a factorg raph and an ordering + * An elimination tree (see Gilbert01bit) associated with a factor graph and an ordering * is a cluster-tree where there is one node j for each variable, and the parent of each node * corresponds to the first variable up the ordering in the Cholesky factor that j is connected to. */ diff --git a/inference/JunctionTree.h b/inference/JunctionTree.h index 00cc59ad7..ce2433fbd 100644 --- a/inference/JunctionTree.h +++ b/inference/JunctionTree.h @@ -36,7 +36,7 @@ namespace gtsam { typedef BayesTree SymbolicBayesTree; private: - // distribute the factors along the Bayes tree + // distribute the factors along the cluster tree sharedClique distributeFactors(FG& fg, const SymbolicBayesTree::sharedClique clique); diff --git a/inference/tests/testJunctionTree.cpp b/inference/tests/testJunctionTree.cpp index 4f06d9af1..3ed077eff 100644 --- a/inference/tests/testJunctionTree.cpp +++ b/inference/tests/testJunctionTree.cpp @@ -41,20 +41,18 @@ TEST( JunctionTree, constructor ) Ordering ordering; ordering += "x2","x1","x3","x4"; SymbolicJunctionTree actual(fg, ordering); +// CHECK(assert_equal(expected, actual)); - /* - CHECK(assert_equal(expected, actual)); Ordering frontal1; frontal1 += "x3", "x4"; Ordering frontal2; frontal2 += "x2", "x1"; Unordered sep1; Unordered sep2; sep2 += "x3"; - CHECK(assert_equal(frontal1, actual.root()->frontal())); - CHECK(assert_equal(sep1, actual.root()->separator())); + CHECK(assert_equal(frontal1, actual.root()->frontal_)); + CHECK(assert_equal(sep1, actual.root()->separator_)); LONGS_EQUAL(1, actual.root()->size()); - CHECK(assert_equal(frontal2, actual.root()->children()[0]->frontal())); - CHECK(assert_equal(sep2, actual.root()->children()[0]->separator())); - LONGS_EQUAL(2, actual.root()->children()[0]->size()); - */ + CHECK(assert_equal(frontal2, actual.root()->children_[0]->frontal_)); + CHECK(assert_equal(sep2, actual.root()->children_[0]->separator_)); + LONGS_EQUAL(2, actual.root()->children_[0]->size()); } /* ************************************************************************* */ diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index 427336af7..4d6657f63 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -389,7 +389,6 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, if (verbose) model->print("Before QR"); SharedDiagonal noiseModel = model->QR(Ab); if (verbose) model->print("After QR"); -// gtsam::print(Ab, "Ab after QR"); // get dimensions of the eliminated variable // TODO: this is another map find that should be avoided ! @@ -398,34 +397,6 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, // Get alias to augmented RHS d ublas::matrix_column d(Ab,n); -// // create base conditional Gaussian -// GaussianConditional::shared_ptr conditional(new GaussianConditional(frontals.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 -// -// // 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, separators) { -// 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 -// j+=dim; -// } -// -// // Set sigmas -// // set the right model here -// if (noiseModel->isConstrained()) -// factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank)); -// else -// factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank)); -// -// // extract ds vector for the new b -// factor->set_b(sub(d, n1, maxRank)); -// -// return make_pair(conditional, factor); - // extract the conditionals GaussianBayesNet bn; size_t n0 = 0; diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index b7d00edda..986851ff9 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -16,7 +16,7 @@ #include "FactorGraph-inl.h" #include "inference-inl.h" #include "iterative.h" -//#include "GaussianJunctionTree.h" +#include "GaussianJunctionTree.h" using namespace std; using namespace gtsam; @@ -296,12 +296,14 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old) } /* ************************************************************************* */ -//VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering) -//{ -// GaussianJunctionTree junctionTree(*this, ordering); -// -// return junctionTree.optimize(); -//} +VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering) +{ + GaussianJunctionTree junctionTree(*this, ordering); + + // calculate new configuration (using backsubstitution) + VectorConfig delta = junctionTree.optimize(); + return delta; +} /* ************************************************************************* */ boost::shared_ptr diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 57d1cf617..ac599e61a 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -155,7 +155,8 @@ namespace gtsam { VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true); /** - * optimize a linear factor graph with multi-frontals + * optimize a linear factor graph using a multi-frontal solver + * @param ordering fg in order */ VectorConfig optimizeMultiFrontals(const Ordering& ordering); diff --git a/linear/GaussianJunctionTree.cpp b/linear/GaussianJunctionTree.cpp index c4b3ac52b..086c96581 100644 --- a/linear/GaussianJunctionTree.cpp +++ b/linear/GaussianJunctionTree.cpp @@ -45,7 +45,7 @@ namespace gtsam { // eliminate from leaves to the root typedef JunctionTree Base; BayesTree bayesTree; - this->eliminate(); + bayesTree = this->eliminate(); // back-substitution VectorConfig result; diff --git a/linear/tests/testGaussianJunctionTree.cpp b/linear/tests/testGaussianJunctionTree.cpp index 9cbf6932e..d570e2f0f 100644 --- a/linear/tests/testGaussianJunctionTree.cpp +++ b/linear/tests/testGaussianJunctionTree.cpp @@ -22,6 +22,24 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +GaussianFactorGraph createChain() { + + typedef GaussianFactorGraph::sharedFactor Factor; + SharedDiagonal model(Vector_(1, 0.5)); + Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model)); + Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model)); + Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.), model)); + Factor factor4(new GaussianFactor("x4", Matrix_(1,1,1.), Vector_(1,1.), model)); + + GaussianFactorGraph fg; + fg.push_back(factor1); + fg.push_back(factor2); + fg.push_back(factor3); + fg.push_back(factor4); + + return fg; +} + /* ************************************************************************* */ /** * x1 - x2 - x3 - x4 @@ -38,19 +56,7 @@ using namespace gtsam; */ TEST( GaussianJunctionTree, eliminate ) { - typedef GaussianFactorGraph::sharedFactor Factor; - SharedDiagonal model(Vector_(1, 0.5)); - Factor factor1(new GaussianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor2(new GaussianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor3(new GaussianFactor("x3", Matrix_(1,1,1.), "x4", Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor4(new GaussianFactor("x4", Matrix_(1,1,1.), Vector_(1,1.), model)); - - GaussianFactorGraph fg; - fg.push_back(factor1); - fg.push_back(factor2); - fg.push_back(factor3); - fg.push_back(factor4); - + GaussianFactorGraph fg = createChain(); Ordering ordering; ordering += "x2","x1","x3","x4"; GaussianJunctionTree junctionTree(fg, ordering); BayesTree bayesTree = junctionTree.eliminate(); @@ -66,6 +72,23 @@ TEST( GaussianJunctionTree, eliminate ) CHECK(assert_equal(bayesTree_expected, bayesTree)); } + +/* ************************************************************************* */ +TEST( GaussianJunctionTree, optimizeMultiFrontal ) +{ + GaussianFactorGraph fg = createChain(); + Ordering ordering; ordering += "x2","x1","x3","x4"; + GaussianJunctionTree tree(fg, ordering); + + VectorConfig actual = tree.optimize(); + VectorConfig expected; + expected.insert("x1", Vector_(1, 0.)); + expected.insert("x2", Vector_(1, 1.)); + expected.insert("x3", Vector_(1, 0.)); + expected.insert("x4", Vector_(1, 1.)); + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index adad96df7..68446f1cd 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -475,6 +475,24 @@ TEST( GaussianFactorGraph, optimize ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, optimizeMultiFrontlas ) +{ + // create a graph + GaussianFactorGraph fg = createGaussianFactorGraph(); + + // create an ordering + Ordering ord; ord += "x2","l1","x1"; + + // optimize the graph + VectorConfig actual = fg.optimizeMultiFrontals(ord); + + // verify + VectorConfig expected = createCorrectDelta(); + + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 9655102f5..53f381c25 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -38,12 +38,8 @@ TEST( GaussianJunctionTree, constructor2 ) // create an ordering Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; - - GaussianJunctionTree expected; GaussianJunctionTree actual(fg, ordering); -// CHECK(assert_equal(expected, actual)); - /* Ordering frontal1; frontal1 += "x5", "x6", "x4"; Ordering frontal2; frontal2 += "x3", "x2"; Ordering frontal3; frontal3 += "x1"; @@ -52,22 +48,21 @@ TEST( GaussianJunctionTree, constructor2 ) Unordered sep2; sep2 += "x4"; Unordered sep3; sep3 += "x2"; Unordered sep4; sep4 += "x6"; - CHECK(assert_equal(frontal1, actual.root()->frontal())); - CHECK(assert_equal(sep1, actual.root()->separator())); + CHECK(assert_equal(frontal1, actual.root()->frontal_)); + CHECK(assert_equal(sep1, actual.root()->separator_)); LONGS_EQUAL(5, actual.root()->size()); - CHECK(assert_equal(frontal2, actual.root()->children()[0]->frontal())); - CHECK(assert_equal(sep2, actual.root()->children()[0]->separator())); - LONGS_EQUAL(4, actual.root()->children()[0]->size()); - CHECK(assert_equal(frontal3, actual.root()->children()[0]->children()[0]->frontal())); - CHECK(assert_equal(sep3, actual.root()->children()[0]->children()[0]->separator())); - LONGS_EQUAL(2, actual.root()->children()[0]->children()[0]->size()); - CHECK(assert_equal(frontal4, actual.root()->children()[1]->frontal())); - CHECK(assert_equal(sep4, actual.root()->children()[1]->separator())); - LONGS_EQUAL(2, actual.root()->children()[1]->size()); - */ + CHECK(assert_equal(frontal2, actual.root()->children_[0]->frontal_)); + CHECK(assert_equal(sep2, actual.root()->children_[0]->separator_)); + LONGS_EQUAL(4, actual.root()->children_[0]->size()); + CHECK(assert_equal(frontal3, actual.root()->children_[0]->children_[0]->frontal_)); + CHECK(assert_equal(sep3, actual.root()->children_[0]->children_[0]->separator_)); + LONGS_EQUAL(2, actual.root()->children_[0]->children_[0]->size()); + CHECK(assert_equal(frontal4, actual.root()->children_[1]->frontal_)); + CHECK(assert_equal(sep4, actual.root()->children_[1]->separator_)); + LONGS_EQUAL(2, actual.root()->children_[1]->size()); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( GaussianJunctionTree, optimizeMultiFrontal ) { // create a graph @@ -77,13 +72,33 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; // optimize the graph - GaussianJunctionTree actual(fg, ordering); - VectorConfig actual = actual.optimize(); + GaussianJunctionTree tree(fg, ordering); + VectorConfig actual = tree.optimize(); // verify -// VectorConfig expected = createCorrectDelta(); -// -// CHECK(assert_equal(expected,actual)); + VectorConfig expected; // expected solution + Vector v = Vector_(2, 0., 0.); + for (int i=1; i<=7; i++) + expected.insert(symbol('x', i), v); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( GaussianJunctionTree, optimizeMultiFrontal2) +{ + // create a graph + Graph nlfg = createNonlinearFactorGraph(); + Config noisy = createNoisyConfig(); + GaussianFactorGraph fg = *nlfg.linearize(noisy); + + // optimize the graph + Ordering ordering; ordering += "x1","x2","l1"; + GaussianJunctionTree tree(fg, ordering); + VectorConfig actual = tree.optimize(); + + // verify + VectorConfig expected = createCorrectDelta(); // expected solution + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */