diff --git a/.cproject b/.cproject index 2698ef3ac..5633bb6f1 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -322,6 +324,7 @@ make + testSimpleCamera.run true true @@ -337,7 +340,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +347,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +355,6 @@ make - testGaussianConditional.run true true @@ -360,6 +362,7 @@ make + testPose2.run true true @@ -367,6 +370,7 @@ make + testRot3.run true true @@ -374,7 +378,6 @@ make - testNonlinearOptimizer.run true true @@ -382,6 +385,7 @@ make + testGaussianFactor.run true true @@ -389,6 +393,7 @@ make + testGaussianFactorGraph.run true true @@ -396,7 +401,6 @@ make - testNonlinearFactorGraph.run true true @@ -404,6 +408,7 @@ make + testPose3.run true true @@ -411,7 +416,6 @@ make - testVectorConfig.run true true @@ -419,7 +423,6 @@ make - testPoint2.run true true @@ -427,6 +430,7 @@ make + testNonlinearFactor.run true true @@ -434,6 +438,7 @@ make + timeGaussianFactor.run true true @@ -441,6 +446,7 @@ make + timeGaussianFactorGraph.run true true @@ -448,6 +454,7 @@ make + testGaussianBayesNet.run true true @@ -455,7 +462,6 @@ make - testBayesTree.run true false @@ -463,6 +469,7 @@ make + testSymbolicBayesNet.run true false @@ -470,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -478,6 +484,7 @@ make + testVector.run true true @@ -485,22 +492,21 @@ make + testMatrix.run true true true - + make - -testInference.run +testIncremental.run true true true make - testVSLAMGraph.run true true @@ -508,7 +514,6 @@ make - testNonlinearEquality.run true true @@ -516,6 +521,7 @@ make + testSQP.run true true @@ -523,7 +529,6 @@ make - testNonlinearConstraint.run true true @@ -531,7 +536,6 @@ make - install true true @@ -539,7 +543,6 @@ make - clean true true @@ -547,7 +550,6 @@ make - check true true diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 7b4adc69e..5c9a57293 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -187,17 +187,16 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::removeClique(sharedClique clique) { - if (!clique->isRoot()) + + if (clique->isRoot()) + root_.reset(); + else // detach clique from parent clique->parent_->children_.remove(clique); - else { - // we remove the root clique: have to make another clique the root - if (clique->children_.empty()) - root_.reset(); - else - root_ = *(clique->children_.begin()); - } - BOOST_FOREACH(sharedClique child, clique->children_) + + // orphan my children + BOOST_FOREACH(sharedClique child, clique->children_) child->parent_.reset(); + BOOST_FOREACH(std::string key, clique->ordering()) nodes_.erase(key); } diff --git a/cpp/testIncremental.cpp b/cpp/testIncremental.cpp index 0b90e7c67..d72ff9eb7 100644 --- a/cpp/testIncremental.cpp +++ b/cpp/testIncremental.cpp @@ -18,6 +18,7 @@ using namespace boost::assign; #include "BayesTree-inl.h" #include "smallExample.h" +using namespace std; using namespace gtsam; typedef BayesTree SymbolicBayesTree; @@ -32,7 +33,7 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial, SymbolicFactorGraph factorGraph; // the list of orphaned subtrees - std::list orphans; + list orphans; // process each key of the new factor BOOST_FOREACH(string key, newFactor->keys()) { @@ -73,27 +74,26 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial, // add orphans to the bottom of the new tree BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) { - std::list::const_iterator it = orphan->separator_.begin(); - for (; it != orphan->separator_.end(); it++) { - + BOOST_FOREACH(string key1, orphan->separator_) { // get clique from new tree to attach to - SymbolicBayesTree::sharedClique clique = newTree[*it]; + SymbolicBayesTree::sharedClique candidateParent = newTree[key1]; // check if all conditionals in there, only add once bool is_subset = true; - std::list::const_iterator it2 = orphan->separator_.begin(); - for (; it2 != orphan->separator_.end(); it2++) { - // if any one is not included, then we have to stop and search for another clique - std::list keys = clique->keys(); - if (find(keys.begin(), keys.end(), *it2) == keys.end()) { + BOOST_FOREACH(string key2, orphan->separator_) { + // if any one not included, then we have to stop and search for another clique + list keys = candidateParent->keys(); + if (find(keys.begin(), keys.end(), key2) == keys.end()) { is_subset = false; break; } } - // this clique contains all the keys of the orphan, so we can add the orphan as a child todo: what about the tree below the orphan? + // this clique contains all the keys of the orphan, + // so we can add the orphan as a child + // todo: what about the tree below the orphan? if (is_subset) { - clique->children_ += orphan; + candidateParent->children_ += orphan; break; } }