diff --git a/.cproject b/.cproject index 7257e427d..02f7b71c6 100644 --- a/.cproject +++ b/.cproject @@ -19,7 +19,7 @@ - + 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) 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)); } /* ************************************************************************* */ 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/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/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 5cdaf6ddd..717f49167 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$. */ @@ -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, 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/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/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index cbf71a111..6d4f20059 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())) - { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } - } + // 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; - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); + // 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) + { + 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, and in the case of ISAM2Clique also store the remaining factor myData.bayesTreeNode->setEliminationResult(eliminationResult); - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } + // 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 +} 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/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/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/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 7302b0880..f73b2b531 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(); @@ -385,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/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-inl.h b/gtsam/linear/JacobianFactor-inl.h index fb66be882..d703cde9f 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -21,7 +21,6 @@ #include #include #include -#include #include namespace gtsam { @@ -40,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."); @@ -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 b1cc49181..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()); @@ -354,7 +354,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; 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/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 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)); 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; 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) : 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());