From 2b1a53347797436a02b06c190583cdbbe5367ab9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Aug 2013 13:29:35 +0000 Subject: [PATCH 01/15] Fixing issues with boost::assign --- gtsam/inference/BayesTreeCliqueBase-inst.h | 3 ++- gtsam/inference/ISAM-inst.h | 3 ++- gtsam/linear/HessianFactor.cpp | 17 +++++++++++++++-- gtsam/linear/JacobianFactor-inl.h | 14 ++++++++++++-- gtsam/linear/JacobianFactor.cpp | 3 ++- gtsam/linear/KalmanFilter.cpp | 6 ++++-- 6 files changed, 37 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 90ca98863..104d7f26f 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -160,7 +160,8 @@ namespace gtsam { gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); // now add the parent conditional - p_Cp += parent->conditional_; // P(Fp|Sp) +// p_Cp += parent->conditional_; // P(Fp|Sp) // FIXME: assign error? + p_Cp.push_back(parent->conditional_); // The variables we want to keepSet are exactly the ones in S std::vector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index d73673755..43f82fb1b 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -41,7 +41,8 @@ namespace gtsam { // Add the orphaned subtrees BOOST_FOREACH(const sharedClique& orphan, orphans) - factors += boost::make_shared >(orphan); + factors.push_back(boost::make_shared >(orphan)); +// factors += boost::make_shared >(orphan); // eliminate into a Bayes net const VariableIndex varIndex(factors); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7302b0880..fc3061be3 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -168,13 +168,26 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), } /* ************************************************************************* */ -namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } +namespace { +DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +} /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js) //, info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) { + +// // FIXME: Issue with boost::range::join here - replacing with simpler version + info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), std::vector(1,1))); +// info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))); // Original + // Alternate implementation +// std::vector dims(gs.size() + 1, 1); +// size_t i=0; +// BOOST_FOREACH(const Vector& m, gs) +// dims[i++] = m.size(); +// info_ = SymmetricBlockMatrix(dims); + // Get the number of variables size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index fb66be882..94b3583dc 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -82,9 +82,19 @@ namespace gtsam { // a single '1' to add a dimension for the b vector. { using boost::adaptors::transformed; - using boost::assign::cref_list_of; +// using boost::assign::cref_list_of; namespace br = boost::range; - Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); + // FIXME: compile error with type mismatch in boost::join +// Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); // Original + Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), std::vector(1,1)), b.size()); + +// // Simple implementation +// std::vector dims(terms.size() + 1, 1); +// size_t i=0; +// typedef std::pair KeyMatrixPair; +// BOOST_FOREACH(const KeyMatrixPair& p, terms) +// dims[i++] = p.second.cols(); +// Ab_ = VerticalBlockMatrix(dims, b.size()); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b1cc49181..e94d860fc 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -265,7 +265,8 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix + Ab_ = VerticalBlockMatrix(boost::join(varDims, std::vector(1,1)), m); // Allocate augmented matrix +// Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix - original Base::keys_.resize(orderedSlots.size()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c37df5ad2..aea379857 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -57,12 +57,14 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor, + KalmanFilter::State fuse(KalmanFilter::State p, GaussianFactor::shared_ptr newFactor, const GaussianFactorGraph::Eliminate& function) { // Create a factor graph GaussianFactorGraph factorGraph; - factorGraph += p, newFactor; + factorGraph.push_back(p); + factorGraph.push_back(newFactor); +// factorGraph += p, newFactor; // FIXME: bad insert at some point here - fills in a template argument with a shared_ptr where a non-pointer should go // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) return solve(factorGraph, function); From 410d8773eb427ecde9df0e40948bf0bd24c92ab5 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Aug 2013 18:58:36 +0000 Subject: [PATCH 02/15] Added comments with instructions for choosing a different version of boost --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 349474dfc..876363f9d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,6 +100,10 @@ endif() ############################################################################### # Find boost +# To change the path for boost, you will need to set: +# BOOST_ROOT: path to install prefix for boost +# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT + # If using Boost shared libs, set up auto linking for shared libs if(MSVC AND NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_DYN_LINK) From 4d357fd6398429f27df051be6e371035efc85599 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Aug 2013 19:58:58 +0000 Subject: [PATCH 03/15] Fixed small issue with ambiguous function --- gtsam/base/tests/testVector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 8fd151dbf..a0de5cf54 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -199,7 +199,7 @@ TEST( TestVector, weightedPseudoinverse_constraint ) // verify EXPECT(assert_equal(expected,actual)); - EXPECT(isinf(precision)); + EXPECT(std::isinf(precision)); } /* ************************************************************************* */ From 6556fb5b1eac7aa6d0e7e9c49d0c72153bf61b16 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 7 Aug 2013 21:57:25 +0000 Subject: [PATCH 04/15] Fixed a couple errors and warnings --- gtsam/inference/EliminationTree-inst.h | 2 +- gtsam/linear/IterativeSolver.h | 1 + gtsam/linear/JacobianFactor.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index d8791d1ba..617fb6e0e 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -251,7 +251,7 @@ namespace gtsam { if(*it1 && *it2) { if(!(*it1)->equals(**it2, tol)) return false; - } else if(*it1 && !*it2 || *it2 && !*it1) { + } else if((*it1 && !*it2) || (*it2 && !*it1)) { return false; } } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index c019aaa69..988293a4f 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -14,6 +14,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index e94d860fc..a7f1dbcb1 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -355,7 +355,7 @@ namespace gtsam { return false; // Check noise model - if(model_ && !f.model_ || !model_ && f.model_) + if((model_ && !f.model_) || (!model_ && f.model_)) return false; if(model_ && f.model_ && !model_->equals(*f.model_, tol)) return false; From 2388f5df45b7445f8a53947a71f6eccca7e2ff2b Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:29:48 +0000 Subject: [PATCH 05/15] Revert "Fixing issues with boost::assign" This reverts commit d820732adc34a6622aae5d2bc0eb4a88e133beaf. --- gtsam/inference/BayesTreeCliqueBase-inst.h | 3 +-- gtsam/inference/ISAM-inst.h | 3 +-- gtsam/linear/HessianFactor.cpp | 17 ++--------------- gtsam/linear/JacobianFactor-inl.h | 14 ++------------ gtsam/linear/JacobianFactor.cpp | 3 +-- gtsam/linear/KalmanFilter.cpp | 6 ++---- 6 files changed, 9 insertions(+), 37 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 104d7f26f..90ca98863 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -160,8 +160,7 @@ namespace gtsam { gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); // now add the parent conditional -// p_Cp += parent->conditional_; // P(Fp|Sp) // FIXME: assign error? - p_Cp.push_back(parent->conditional_); + p_Cp += parent->conditional_; // P(Fp|Sp) // The variables we want to keepSet are exactly the ones in S std::vector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 43f82fb1b..d73673755 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -41,8 +41,7 @@ namespace gtsam { // Add the orphaned subtrees BOOST_FOREACH(const sharedClique& orphan, orphans) - factors.push_back(boost::make_shared >(orphan)); -// factors += boost::make_shared >(orphan); + factors += boost::make_shared >(orphan); // eliminate into a Bayes net const VariableIndex varIndex(factors); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index fc3061be3..7302b0880 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -168,26 +168,13 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), } /* ************************************************************************* */ -namespace { -DenseIndex _getSizeHF(const Vector& m) { return m.size(); } -} +namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js) //, info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) { - -// // FIXME: Issue with boost::range::join here - replacing with simpler version - info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), std::vector(1,1))); -// info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))); // Original - // Alternate implementation -// std::vector dims(gs.size() + 1, 1); -// size_t i=0; -// BOOST_FOREACH(const Vector& m, gs) -// dims[i++] = m.size(); -// info_ = SymmetricBlockMatrix(dims); - // Get the number of variables size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 94b3583dc..fb66be882 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -82,19 +82,9 @@ namespace gtsam { // a single '1' to add a dimension for the b vector. { using boost::adaptors::transformed; -// using boost::assign::cref_list_of; + using boost::assign::cref_list_of; namespace br = boost::range; - // FIXME: compile error with type mismatch in boost::join -// Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); // Original - Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), std::vector(1,1)), b.size()); - -// // Simple implementation -// std::vector dims(terms.size() + 1, 1); -// size_t i=0; -// typedef std::pair KeyMatrixPair; -// BOOST_FOREACH(const KeyMatrixPair& p, terms) -// dims[i++] = p.second.cols(); -// Ab_ = VerticalBlockMatrix(dims, b.size()); + Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a7f1dbcb1..2b154958b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -265,8 +265,7 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, std::vector(1,1)), m); // Allocate augmented matrix -// Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix - original + Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix Base::keys_.resize(orderedSlots.size()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index aea379857..c37df5ad2 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -57,14 +57,12 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter::State fuse(KalmanFilter::State p, GaussianFactor::shared_ptr newFactor, + KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor, const GaussianFactorGraph::Eliminate& function) { // Create a factor graph GaussianFactorGraph factorGraph; - factorGraph.push_back(p); - factorGraph.push_back(newFactor); -// factorGraph += p, newFactor; // FIXME: bad insert at some point here - fills in a template argument with a shared_ptr where a non-pointer should go + factorGraph += p, newFactor; // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) return solve(factorGraph, function); From 35281737817c2712a814f579287ba84d40ca5f4e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:29:55 +0000 Subject: [PATCH 06/15] Fixed compile problem on linux using boost::join with boost::cref_list_of --- .cproject | 2 +- gtsam/base/types.h | 26 ++++++++++++++++++++++++++ gtsam/inference/BayesTree-inst.h | 1 + gtsam/linear/GaussianConditional-inl.h | 2 +- gtsam/linear/HessianFactor.cpp | 6 ++++-- gtsam/linear/JacobianFactor-inl.h | 6 ++---- gtsam/linear/JacobianFactor.cpp | 2 +- 7 files changed, 36 insertions(+), 9 deletions(-) diff --git a/.cproject b/.cproject index 7257e427d..02f7b71c6 100644 --- a/.cproject +++ b/.cproject @@ -19,7 +19,7 @@ - + diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 6ca9b9ea1..5a9ee28a3 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { @@ -103,6 +104,31 @@ namespace gtsam { operator T() const { return value; } }; + /** A helper class that behaves as a container with one element, and works with + * boost::range */ + template + class ListOfOneContainer { + T element_; + public: + typedef T value_type; + typedef const T* const_iterator; + typedef T* iterator; + ListOfOneContainer(const T& element) : element_(element) {} + const T* begin() const { return &element_; } + const T* end() const { return &element_ + 1; } + T* begin() { return &element_; } + T* end() { return &element_ + 1; } + size_t size() const { return 1; } + }; + + BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept >)); + + /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ + template + ListOfOneContainer ListOfOne(const T& element) { + return ListOfOneContainer(element); + } + /** An assertion that throws an exception if NDEBUG is not defined and * evaluates to an empty statement otherwise. */ #ifdef NDEBUG diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 3b31906b2..6a303e081 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -146,6 +146,7 @@ namespace gtsam { } /* ************************************************************************* */ + // TODO: Clean up namespace { template int _pushClique(FactorGraph& fg, const boost::shared_ptr& clique) { diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index f9a925d23..6fe7bd642 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -29,7 +29,7 @@ namespace gtsam { GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas, const typename PARENTS::value_type*) : BaseFactor(boost::join( - boost::assign::cref_list_of<1,typename PARENTS::value_type>(std::make_pair(key, R)), + ListOfOne(std::make_pair(key, R)), parents), d, sigmas), BaseConditional(1) {} diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7302b0880..d865bf589 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -168,12 +168,14 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), } /* ************************************************************************* */ -namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } +namespace { + DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +} /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))) +GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) { // Get the number of variables size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index fb66be882..fb9582f24 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -21,7 +21,6 @@ #include #include #include -#include #include namespace gtsam { @@ -71,7 +70,7 @@ namespace gtsam { void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) { // Check noise model dimension - if(noiseModel && noiseModel->dim() != b.size()) + if(noiseModel && (DenseIndex)noiseModel->dim() != b.size()) throw InvalidNoiseModel(b.size(), noiseModel->dim()); // Resize base class key vector @@ -82,9 +81,8 @@ namespace gtsam { // a single '1' to add a dimension for the b vector. { using boost::adaptors::transformed; - using boost::assign::cref_list_of; namespace br = boost::range; - Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); + Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), ListOfOne((DenseIndex)1)), b.size()); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2b154958b..be2636a16 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -265,7 +265,7 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix + Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix Base::keys_.resize(orderedSlots.size()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); From 0726a5ffbc898a46c769e874fb9cc8efd17969cd Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:30:02 +0000 Subject: [PATCH 07/15] Instantiating template base classes in derived class .cpp files --- gtsam/inference/FactorGraph.h | 4 ++-- gtsam/linear/GaussianBayesNet.cpp | 3 +++ gtsam/linear/GaussianBayesTree.cpp | 4 ++++ gtsam/linear/GaussianEliminationTree.cpp | 3 +++ gtsam/linear/GaussianFactorGraph.cpp | 4 ++++ gtsam/linear/GaussianISAM.cpp | 3 +++ gtsam/linear/GaussianJunctionTree.cpp | 3 +++ gtsam/symbolic/SymbolicBayesNet.cpp | 3 +++ gtsam/symbolic/SymbolicBayesTree.cpp | 4 ++++ gtsam/symbolic/SymbolicEliminationTree.cpp | 3 +++ gtsam/symbolic/SymbolicFactorGraph.cpp | 4 ++++ gtsam/symbolic/SymbolicISAM.cpp | 3 +++ gtsam/symbolic/SymbolicJunctionTree.cpp | 3 +++ 13 files changed, 42 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 004303245..672fbdaef 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -335,10 +335,10 @@ namespace gtsam { void replace(size_t index, sharedFactor factor) { at(index) = factor; } /** Erase factor and rearrange other factors to take up the empty space */ - void erase(const_iterator item) { factors_.erase(item); } + void erase(iterator item) { factors_.erase(item); } /** Erase factors and rearrange other factors to take up the empty space */ - void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); } + void erase(iterator first, iterator last) { factors_.erase(first, last); } /// @} /// @name Advanced Interface diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 7e66d4ce3..8b3dc0084 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -30,6 +30,9 @@ using namespace gtsam; namespace gtsam { + // Instantiate base class + template class FactorGraph; + /* ************************************************************************* */ bool GaussianBayesNet::equals(const This& bn, double tol) const { diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 688334918..63dcefeb7 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -26,6 +26,10 @@ namespace gtsam { + // Instantiate base class + template class BayesTreeCliqueBase; + template class BayesTree; + /* ************************************************************************* */ namespace internal { diff --git a/gtsam/linear/GaussianEliminationTree.cpp b/gtsam/linear/GaussianEliminationTree.cpp index 3979b7e6e..5933643bf 100644 --- a/gtsam/linear/GaussianEliminationTree.cpp +++ b/gtsam/linear/GaussianEliminationTree.cpp @@ -21,6 +21,9 @@ namespace gtsam { + // Instantiate base class + template class EliminationTree; + /* ************************************************************************* */ GaussianEliminationTree::GaussianEliminationTree( const GaussianFactorGraph& factorGraph, const VariableIndex& structure, diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 2d0e47a47..3275409c2 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -35,6 +35,10 @@ using namespace gtsam; namespace gtsam { + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + /* ************************************************************************* */ bool GaussianFactorGraph::equals(const This& fg, double tol) const { diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index dffc532c7..ddbc5c1b3 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -21,6 +21,9 @@ namespace gtsam { + // Instantiate base class + template class ISAM; + /* ************************************************************************* */ GaussianISAM::GaussianISAM() {} diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 10cf8e11f..ae5f116c2 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -22,6 +22,9 @@ namespace gtsam { + // Instantiate base class + template class JunctionTree; + /* ************************************************************************* */ GaussianJunctionTree::GaussianJunctionTree( const GaussianEliminationTree& eliminationTree) : diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp index 179e99c0c..c5a04eb0d 100644 --- a/gtsam/symbolic/SymbolicBayesNet.cpp +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -24,6 +24,9 @@ namespace gtsam { + // Instantiate base class + template class FactorGraph; + /* ************************************************************************* */ bool SymbolicBayesNet::equals(const This& bn, double tol) const { diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp index 1779964ad..f6665c39b 100644 --- a/gtsam/symbolic/SymbolicBayesTree.cpp +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -27,6 +27,10 @@ namespace gtsam { + // Instantiate base classes + template class BayesTreeCliqueBase; + template class BayesTree; + /* ************************************************************************* */ SymbolicBayesTree::SymbolicBayesTree(const SymbolicBayesTree& other) : Base(other) {} diff --git a/gtsam/symbolic/SymbolicEliminationTree.cpp b/gtsam/symbolic/SymbolicEliminationTree.cpp index e01d04bd6..588983a79 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.cpp +++ b/gtsam/symbolic/SymbolicEliminationTree.cpp @@ -21,6 +21,9 @@ namespace gtsam { + // Instantiate base class + template class EliminationTree; + /* ************************************************************************* */ SymbolicEliminationTree::SymbolicEliminationTree( const SymbolicFactorGraph& factorGraph, const VariableIndex& structure, diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp index ff34b27e0..270930472 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.cpp +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -27,6 +27,10 @@ namespace gtsam { + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + using namespace std; /* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicISAM.cpp b/gtsam/symbolic/SymbolicISAM.cpp index f53b96754..d7af82e5c 100644 --- a/gtsam/symbolic/SymbolicISAM.cpp +++ b/gtsam/symbolic/SymbolicISAM.cpp @@ -21,6 +21,9 @@ namespace gtsam { + // Instantiate base class + template class ISAM; + /* ************************************************************************* */ SymbolicISAM::SymbolicISAM() {} diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index ca4f82deb..4b3aadf53 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -22,6 +22,9 @@ namespace gtsam { + // Instantiate base class + template class JunctionTree; + /* ************************************************************************* */ SymbolicJunctionTree::SymbolicJunctionTree( const SymbolicEliminationTree& eliminationTree) : From 2925995d250f05a2baa15faadeddd9854883009f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:30:05 +0000 Subject: [PATCH 08/15] Fixed signed/unsigned warnings --- gtsam/linear/HessianFactor-inl.h | 4 ++-- gtsam/linear/HessianFactor.cpp | 4 ++-- gtsam/linear/JacobianFactor-inl.h | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/HessianFactor-inl.h b/gtsam/linear/HessianFactor-inl.h index 31a327bb8..4a21ab0ff 100644 --- a/gtsam/linear/HessianFactor-inl.h +++ b/gtsam/linear/HessianFactor-inl.h @@ -26,7 +26,7 @@ namespace gtsam { GaussianFactor(keys), info_(augmentedInformation) { // Check number of variables - if(Base::keys_.size() != augmentedInformation.nBlocks() - 1) + if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1) throw std::invalid_argument( "Error in HessianFactor constructor input. Number of provided keys plus\n" "one for the information vector must equal the number of provided matrix blocks."); @@ -38,4 +38,4 @@ namespace gtsam { "must be the information vector, but the last provided block had more than one column."); } -} \ No newline at end of file +} diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d865bf589..f73b2b531 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -387,9 +387,9 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // Apply updates to the upper triangle gttic(update); for(DenseIndex j2=0; j2info_.nBlocks()-1 : slots[j2]; + DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j2]; for(DenseIndex j1=0; j1<=j2; ++j1) { - DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j1]; if(slot2 > slot1) info_(slot1, slot2).noalias() += update.info_(j1, j2); else if(slot1 > slot2) diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index fb9582f24..d703cde9f 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -39,11 +39,11 @@ namespace gtsam { Base(keys), Ab_(augmentedMatrix) { // Check noise model dimension - if(model && model->dim() != augmentedMatrix.rows()) + if(model && (DenseIndex)model->dim() != augmentedMatrix.rows()) throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); // Check number of variables - if(Base::keys_.size() != augmentedMatrix.nBlocks() - 1) + if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) throw std::invalid_argument( "Error in JacobianFactor constructor input. Number of provided keys plus\n" "one for the RHS vector must equal the number of provided matrix blocks."); From b572ad8131289a3a8f82019cf47620f694cd7e1a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:30:10 +0000 Subject: [PATCH 09/15] Use functor structs instead of pointer-to-template-function --- gtsam/base/treeTraversal-inst.h | 22 +++++---- gtsam/inference/JunctionTree-inst.h | 77 +++++++++++++++-------------- gtsam/inference/inference-inst.h | 26 ++++++---- 3 files changed, 69 insertions(+), 56 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index b4aaca072..a90121738 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -312,15 +312,18 @@ namespace gtsam { /* ************************************************************************* */ /** Traversal function for PrintForest */ namespace { - template - std::string - PrintForestVisitorPre(const boost::shared_ptr& node, const std::string& parentString, const KeyFormatter& formatter) + struct PrintForestVisitorPre { - // Print the current node - node->print(parentString + "-", formatter); - // Increment the indentation - return parentString + "| "; - } + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} + template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) + { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } + }; } /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. @@ -328,7 +331,8 @@ namespace gtsam { template void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { typedef typename FOREST::Node Node; - DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre, _1, _2, keyFormatter)); + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); } } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index e4e3e6630..7a2895c99 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -161,47 +161,49 @@ namespace gtsam { /* ************************************************************************* */ // Elimination post-order visitor - combine the child factors with our own factors, add the - // resulting conditional to the BayesTree, and add the remaining factor to the parent. The - // extra argument 'eliminationFunction' is passed from JunctionTree::eliminate using - // boost::bind. + // resulting conditional to the BayesTree, and add the remaining factor to the parent. template - void eliminationPostOrderVisitor(const typename JUNCTIONTREE::sharedNode& node, EliminationData& myData, - const typename JUNCTIONTREE::Eliminate& eliminationFunction) + struct EliminationPostOrderVisitor { - // Typedefs - typedef typename JUNCTIONTREE::sharedFactor sharedFactor; - typedef typename JUNCTIONTREE::FactorType FactorType; - typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType; - typedef typename JUNCTIONTREE::ConditionalType ConditionalType; - typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode; - - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; - - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) + const typename JUNCTIONTREE::Eliminate& eliminationFunction; + EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction) : eliminationFunction(eliminationFunction) {} + void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData& myData) { - if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) + // Typedefs + typedef typename JUNCTIONTREE::sharedFactor sharedFactor; + typedef typename JUNCTIONTREE::FactorType FactorType; + typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType; + typedef typename JUNCTIONTREE::ConditionalType ConditionalType; + typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode; + + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; + + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; + if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) + { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } } + + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, Ordering(node->keys)); + + // Store conditional in BayesTree clique + myData.bayesTreeNode->conditional_ = eliminationResult.first; + + // Store remaining factor in parent's gathered factors + if(!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; } - - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); - - // Store conditional in BayesTree clique - myData.bayesTreeNode->conditional_ = eliminationResult.first; - - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } + }; } /* ************************************************************************* */ @@ -278,9 +280,10 @@ namespace gtsam { // that contains all of the roots as its children. rootsContainer also stores the remaining // uneliminated factors passed up from the roots. EliminationData rootsContainer(0, roots_.size()); + EliminationPostOrderVisitor visitorPost(function); //tbb::task_scheduler_init init(1); - treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer, eliminationPreOrderVisitor, - boost::bind(eliminationPostOrderVisitor, _1, _2, function)/*, 10*/); + treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer, + eliminationPreOrderVisitor, visitorPost/*, 10*/); // Create BayesTree from roots stored in the dummy BayesTree node. boost::shared_ptr result = boost::make_shared(); diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index eb6ef9de7..1d4e7c0dc 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -50,14 +50,20 @@ namespace gtsam { /* ************************************************************************* */ template - void eliminationPostOrderVisitor(const typename TREE::sharedNode& node, EliminationData& myData, - RESULT& result, const typename TREE::Eliminate& eliminationFunction) + struct EliminationPostOrderVisitor { - // Call eliminate on the node and add the result to the parent's gathered factors - typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); - if(!childFactor->empty()) - myData.parentData->childFactors.push_back(childFactor); - } + RESULT& result; + const typename TREE::Eliminate& eliminationFunction; + EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) : + result(result), eliminationFunction(eliminationFunction) {} + void operator()(const typename TREE::sharedNode& node, EliminationData& myData) + { + // Call eliminate on the node and add the result to the parent's gathered factors + typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); + if(!childFactor->empty()) + myData.parentData->childFactors.push_back(childFactor); + } + }; } /* ************************************************************************* */ @@ -79,12 +85,12 @@ namespace gtsam { // elimination (using the gathered child factors) and store the result in the parent's // gathered factors. EliminationData rootData(0, tree.roots().size()); - treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor, - boost::bind(eliminationPostOrderVisitor, _1, _2, result, function)); + EliminationPostOrderVisitor visitorPost(result, function); + treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor, visitorPost); // Return remaining factors return rootData.childFactors; } } -} \ No newline at end of file +} From 92459bc33b2c065be1f7ebe9f6b046fedfb332e1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 16:30:14 +0000 Subject: [PATCH 10/15] Fixed gcc unit test compile errors --- .../tests/testGaussianBayesTreeUnordered.cpp | 2 +- gtsam/symbolic/tests/testSymbolicBayesTree.cpp | 2 +- .../tests/testSymbolicEliminationTree.cpp | 3 --- gtsam/symbolic/tests/testSymbolicFactorGraph.cpp | 1 - .../symbolic/tests/testSymbolicJunctionTree.cpp | 16 +++++++--------- 5 files changed, 9 insertions(+), 15 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp index 5cbbd195c..dbd9a3641 100644 --- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -57,7 +57,7 @@ namespace { GaussianBayesTreeClique::shared_ptr clique = boost::make_shared( boost::make_shared(conditional)); - clique->children = children; + clique->children.assign(children.begin(), children.end()); BOOST_FOREACH(const GaussianBayesTreeClique::shared_ptr& child, children) child->parent_ = clique; return clique; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 793d2d5dc..43598cb16 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -58,7 +58,7 @@ namespace { boost::make_shared( boost::make_shared( SymbolicConditional::FromKeys(keys, nrFrontals))); - clique->children = children; + clique->children.assign(children.begin(), children.end()); BOOST_FOREACH(const SymbolicBayesTreeClique::shared_ptr& child, children) child->parent_ = clique; return clique; diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 095e84a18..c6d4c8937 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -19,9 +19,6 @@ #include #include -#include -#include -using namespace boost::assign; #include #include diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 668d4e9d0..879ab6601 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -22,7 +22,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index e21ba7bcf..07c0c58e7 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -16,11 +16,6 @@ * @author Frank Dellaert */ -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; - #include #include @@ -28,6 +23,9 @@ using namespace boost::assign; #include #include +#include +using namespace boost::assign; + #include "symbolicExampleGraphs.h" using namespace gtsam; @@ -44,10 +42,10 @@ TEST( JunctionTree, constructor ) SymbolicJunctionTree actual(SymbolicEliminationTree(simpleChain, order)); - vector frontal1; frontal1 += 2, 3; - vector frontal2; frontal2 += 0, 1; - vector sep1; - vector sep2; sep2 += 2; + vector frontal1 = list_of(2)(3); + vector frontal2 = list_of(0)(1); + vector sep1; + vector sep2 = list_of(2); EXPECT(assert_equal(frontal1, actual.roots().front()->keys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); From 2d7abe071076234a34599a7b89107f7252227a79 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 16:51:04 +0000 Subject: [PATCH 11/15] Added explicit templated base class instantiation within NonlinearFactorGraph --- gtsam/nonlinear/NonlinearFactorGraph.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 6441c2eba..555e7a22d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -32,6 +32,9 @@ using namespace std; namespace gtsam { +// Instantiate base classes +template class FactorGraph; + /* ************************************************************************* */ double NonlinearFactorGraph::probPrime(const Values& c) const { return exp(-0.5 * error(c)); From db47948a2047984ca8aa3c88917c409215390347 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 16:58:58 +0000 Subject: [PATCH 12/15] Disabled testLinearContainerFactor and testWhiteNoiseFactor in cmake --- gtsam/nonlinear/CMakeLists.txt | 4 +++- gtsam/nonlinear/LinearContainerFactor.h | 2 -- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 4dcb5e8d8..47f14c8d9 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -15,7 +15,9 @@ set(nonlinear_local_libs # Files to exclude from compilation of tests and timing scripts set(nonlinear_excluded_files # "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test - "" # Add to this list, with full path, to exclude + #"" # Add to this list, with full path, to exclude + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testLinearContainerFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tests/testWhiteNoiseFactor.cpp" ) # Build tests diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 304201ff8..f78edec1b 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -11,7 +11,6 @@ #include -#if 0 namespace gtsam { @@ -160,4 +159,3 @@ private: } // \namespace gtsam -#endif From b05748683c8b9652926e657bb90efd86bd34826d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 17:14:59 +0000 Subject: [PATCH 13/15] Fixed failing retract unit test in Values --- gtsam/nonlinear/Values.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b96d58a14..47ad0ca6f 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -82,10 +82,16 @@ namespace gtsam { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - const Vector& singleDelta = delta[key_value->key]; // Delta for this value + VectorValues::const_iterator vector_item = delta.find(key_value->key); Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument - Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract - result.values_.insert(key, retractedValue); // Add retracted result directly to result values + if(vector_item != delta.end()) { +// const Vector& singleDelta = delta[key_value->key]; // Delta for this value + const Vector& singleDelta = vector_item->second; + Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract + result.values_.insert(key, retractedValue); // Add retracted result directly to result values + } else { + result.values_.insert(key, key_value->value.clone_()); // Add original version to result values + } } return result; From 7175d588e1e05601d808cb9a17caebc6afe6dd33 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 8 Aug 2013 17:26:45 +0000 Subject: [PATCH 14/15] Fixed comment wording --- gtsam/inference/EliminateableFactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 5cdaf6ddd..bd1d71857 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -144,7 +144,7 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - /** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net + /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ * B = X\backslash A \f$. */ From 41a1b3c30eb9c63c45a5e8c85908e053048201c3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 17:35:33 +0000 Subject: [PATCH 15/15] Fixed comment wording --- gtsam/inference/EliminateableFactorGraph.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index bd1d71857..717f49167 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -164,20 +164,20 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to - * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) - * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the - * factor graph, and \f$ B = X\backslash A \f$. */ + /** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes + * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) + * \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and + * \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - /** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes - * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) - * \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and - * \f$ B = X\backslash A \f$. */ + /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to + * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) + * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the + * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal( const std::vector& variables,