diff --git a/base/FastList.h b/base/FastList.h new file mode 100644 index 000000000..ed993e3e8 --- /dev/null +++ b/base/FastList.h @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FastList.h + * @brief A thin wrapper around std::list that uses boost's fast_pool_allocator. + * @author Richard Roberts + * @created Oct 22, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * FastSet is a thin wrapper around std::set that uses the boost + * fast_pool_allocator instead of the default STL allocator. This is just a + * convenience to avoid having lengthy types in the code. Through timing, + * we've seen that the fast_pool_allocator can lead to speedups of several + * percent. + */ +template +class FastList: public std::list > { + +public: + + typedef std::list > Base; + + /** Default constructor */ + FastList() {} + + /** Constructor from a range, passes through to base class */ + template + FastList(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} + + /** Copy constructor from another FastList */ + FastList(const FastList& x) : Base(x) {} + + /** Copy constructor from the base map class */ + FastList(const Base& x) : Base(x) {} + +}; + +} diff --git a/base/FastSet.h b/base/FastSet.h index 58d69ce9c..a8f8d367e 100644 --- a/base/FastSet.h +++ b/base/FastSet.h @@ -37,11 +37,14 @@ public: typedef std::set, boost::fast_pool_allocator > Base; + /** Default constructor */ + FastSet() {} + /** Constructor from a range, passes through to base class */ template FastSet(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} - /** Copy constructor from another FastMap */ + /** Copy constructor from another FastSet */ FastSet(const FastSet& x) : Base(x) {} /** Copy constructor from the base map class */ diff --git a/base/Makefile.am b/base/Makefile.am index f0d337ae1..923a63518 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -27,7 +27,7 @@ sources += LieVector.cpp check_PROGRAMS += tests/testLieVector tests/testLieScalar # Data structures -headers += BTree.h DSF.h FastMap.h +headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h sources += DSFVector.cpp check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index bd265f15b..e698509c3 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -19,6 +19,11 @@ #pragma once +#include +#include +#include +#include + #include #include @@ -32,11 +37,6 @@ using namespace boost::assign; namespace lam = boost::lambda; -#include -#include -#include -#include - namespace gtsam { using namespace std; diff --git a/inference/Conditional.h b/inference/ConditionalBase.h similarity index 99% rename from inference/Conditional.h rename to inference/ConditionalBase.h index 5341ebf8b..2a2c462b8 100644 --- a/inference/Conditional.h +++ b/inference/ConditionalBase.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include namespace gtsam { diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index 70d73d5bb..19b32c4c7 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -47,7 +47,7 @@ EliminationTree::eliminate_(Conditionals& conditionals) const { /* ************************************************************************* */ template -vector EliminationTree::ComputeParents(const VariableIndex<>& structure) { +vector EliminationTree::ComputeParents(const VariableIndex& structure) { // Number of factors and variables const size_t m = structure.nFactors(); @@ -62,8 +62,7 @@ vector EliminationTree::ComputeParents(const VariableIndex<>& str // for column j \in 1 to n do for (Index j = 0; j < n; j++) { // for row i \in Struct[A*j] do - BOOST_FOREACH(typename VariableIndex<>::mapped_factor_type i_pos, structure[j]) { - const size_t i = i_pos.factorIndex; + BOOST_FOREACH(const size_t i, structure[j]) { if (prevCol[i] != none) { Index k = prevCol[i]; // find root r of the current tree that contains k @@ -83,7 +82,7 @@ vector EliminationTree::ComputeParents(const VariableIndex<>& str template template typename EliminationTree::shared_ptr -EliminationTree::Create(const FactorGraph& factorGraph, const VariableIndex<>& structure) { +EliminationTree::Create(const FactorGraph& factorGraph, const VariableIndex& structure) { // Compute the tree structure vector parents(ComputeParents(structure)); @@ -125,7 +124,7 @@ template template typename EliminationTree::shared_ptr EliminationTree::Create(const FactorGraph& factorGraph) { - return Create(factorGraph, VariableIndex<>(factorGraph)); + return Create(factorGraph, VariableIndex(factorGraph)); } /* ************************************************************************* */ diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index 53f779ada..c93b3d6e6 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -55,7 +55,7 @@ private: * Static internal function to build a vector of parent pointers using the * algorithm of Gilbert et al., 2001, BIT. */ - static std::vector ComputeParents(const VariableIndex<>& structure); + static std::vector ComputeParents(const VariableIndex& structure); /** * Recursive routine that eliminates the factors arranged in an elimination tree @@ -72,7 +72,7 @@ public: * pre-computed column structure. */ template - static shared_ptr Create(const FactorGraph& factorGraph, const VariableIndex<>& structure); + static shared_ptr Create(const FactorGraph& factorGraph, const VariableIndex& structure); /** Named constructor to build the elimination tree of a factor graph */ template diff --git a/inference/Factor-inl.h b/inference/FactorBase-inl.h similarity index 99% rename from inference/Factor-inl.h rename to inference/FactorBase-inl.h index 53ec15115..7dad0c180 100644 --- a/inference/Factor-inl.h +++ b/inference/FactorBase-inl.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include #include diff --git a/inference/Factor.h b/inference/FactorBase.h similarity index 88% rename from inference/Factor.h rename to inference/FactorBase.h index 803c50e44..1c848e2cb 100644 --- a/inference/Factor.h +++ b/inference/FactorBase.h @@ -99,15 +99,6 @@ public: FactorBase(Key key1, Key key2, Key key3, Key key4) : keys_(4) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } - /** Named constructor for combining a set of factors with pre-computed set of - * variables. (Old style - will be removed when scalar elimination is - * removed in favor of the EliminationTree). */ - template - static typename DERIVED::shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph, - const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions) { - return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); } - /** Create a combined joint factor (new style for EliminationTree). */ template static typename DERIVED::shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); diff --git a/inference/GenericMultifrontalSolver-inl.h b/inference/GenericMultifrontalSolver-inl.h index 18899a76c..c80297682 100644 --- a/inference/GenericMultifrontalSolver-inl.h +++ b/inference/GenericMultifrontalSolver-inl.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GenericMultifrontalSolver-inl.h * @brief @@ -8,7 +19,7 @@ #pragma once #include -#include +#include #include #include #include @@ -18,19 +29,22 @@ namespace gtsam { /* ************************************************************************* */ -template -GenericMultifrontalSolver::GenericMultifrontalSolver(const FactorGraph& factorGraph) : - junctionTree_(new JunctionTree >(factorGraph)) {} +template +GenericMultifrontalSolver::GenericMultifrontalSolver(const FactorGraph& factorGraph) : + junctionTree_(factorGraph) {} /* ************************************************************************* */ -template -typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate() const { - return junctionTree_->eliminate(); +template +typename JUNCTIONTREE::BayesTree::shared_ptr +GenericMultifrontalSolver::eliminate() const { + typename JUNCTIONTREE::BayesTree::shared_ptr bayesTree(new typename JUNCTIONTREE::BayesTree); + bayesTree->insert(junctionTree_.eliminate()); + return bayesTree; } /* ************************************************************************* */ -template -typename FACTOR::shared_ptr GenericMultifrontalSolver::marginal(Index j) const { +template +typename FACTOR::shared_ptr GenericMultifrontalSolver::marginal(Index j) const { return eliminate()->marginal(j); } diff --git a/inference/GenericMultifrontalSolver.h b/inference/GenericMultifrontalSolver.h index a36693dfb..9432e5495 100644 --- a/inference/GenericMultifrontalSolver.h +++ b/inference/GenericMultifrontalSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GenericMultifrontalSolver.h * @brief @@ -15,13 +26,13 @@ namespace gtsam { -template +template class GenericMultifrontalSolver { protected: - // Elimination tree that performs elimination. - typename JunctionTree >::shared_ptr junctionTree_; + // Junction tree that performs elimination. + JUNCTIONTREE junctionTree_; public: @@ -35,7 +46,7 @@ public: * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename BayesTree::shared_ptr eliminate() const; + typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const; /** * Compute the marginal Gaussian density over a variable, by integrating out diff --git a/inference/GenericSequentialSolver-inl.h b/inference/GenericSequentialSolver-inl.h index 0c7c2437a..c31ec480a 100644 --- a/inference/GenericSequentialSolver-inl.h +++ b/inference/GenericSequentialSolver-inl.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GenericSequentialSolver.cpp * @brief @@ -8,7 +19,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/inference/GenericSequentialSolver.h b/inference/GenericSequentialSolver.h index 05227cc60..a0d406217 100644 --- a/inference/GenericSequentialSolver.h +++ b/inference/GenericSequentialSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GenericSequentialSolver.h * @brief @@ -24,7 +35,7 @@ protected: FactorGraph factors_; // Column structure of the factor graph - VariableIndex<> structure_; + VariableIndex structure_; // Elimination tree that performs elimination. typename EliminationTree::shared_ptr eliminationTree_; diff --git a/inference/ISAM-inl.h b/inference/ISAM-inl.h index 53c71f912..270998878 100644 --- a/inference/ISAM-inl.h +++ b/inference/ISAM-inl.h @@ -21,7 +21,7 @@ #include // for operator += using namespace boost::assign; -#include +#include #include #include #include diff --git a/inference/IndexConditional.h b/inference/IndexConditional.h index 9501eb23c..2955efa64 100644 --- a/inference/IndexConditional.h +++ b/inference/IndexConditional.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include namespace gtsam { diff --git a/inference/IndexFactor.cpp b/inference/IndexFactor.cpp index 8ce8a2fbc..89f5a4fdf 100644 --- a/inference/IndexFactor.cpp +++ b/inference/IndexFactor.cpp @@ -16,7 +16,7 @@ * @created Oct 17, 2010 */ -#include +#include #include namespace gtsam { diff --git a/inference/IndexFactor.h b/inference/IndexFactor.h index 19b7f33f8..836466bb4 100644 --- a/inference/IndexFactor.h +++ b/inference/IndexFactor.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -63,12 +63,6 @@ public: static shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); - template - static shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph, - const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions) { - return Base::Combine(factorGraph, variableIndex, factors, variables, variablePositions); } - /** * eliminate the first variable involved in this factor * @return a conditional on the eliminated variable diff --git a/inference/Makefile.am b/inference/Makefile.am index f11f654ac..99ae80a74 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -15,19 +15,19 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # GTSAM core -headers += Factor.h Factor-inl.h Conditional.h +headers += FactorBase.h FactorBase-inl.h ConditionalBase.h # Symbolic Inference -sources += SymbolicFactorGraph.cpp SymbolicSequentialSolver.cpp +sources += SymbolicFactorGraph.cpp SymbolicMultifrontalSolver.cpp SymbolicSequentialSolver.cpp check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots # Inference +headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-inl.h headers += inference-inl.h VariableSlots-inl.h -sources += inference.cpp VariableSlots.cpp Permutation.cpp +sources += inference.cpp VariableSlots.cpp Permutation.cpp VariableIndex.cpp sources += IndexFactor.cpp IndexConditional.cpp headers += graph.h graph-inl.h -headers += VariableIndex.h headers += FactorGraph.h FactorGraph-inl.h headers += ClusterTree.h ClusterTree-inl.h headers += JunctionTree.h JunctionTree-inl.h diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 308341e89..5cab6885c 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -36,7 +36,6 @@ typedef EliminationTree SymbolicEliminationTree; class SymbolicFactorGraph: public FactorGraph { public: typedef SymbolicBayesNet bayesnet_type; - typedef VariableIndex<> variableindex_type; /** Construct empty factor graph */ SymbolicFactorGraph() {} diff --git a/inference/SymbolicMultifrontalSolver.cpp b/inference/SymbolicMultifrontalSolver.cpp new file mode 100644 index 000000000..b849046f1 --- /dev/null +++ b/inference/SymbolicMultifrontalSolver.cpp @@ -0,0 +1,26 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicMultifrontalSolver.cpp + * @brief + * @author Richard Roberts + * @created Oct 22, 2010 + */ + +#include +#include + +namespace gtsam { + +template class GenericMultifrontalSolver > >; + +} diff --git a/inference/SymbolicMultifrontalSolver.h b/inference/SymbolicMultifrontalSolver.h new file mode 100644 index 000000000..8c8c8abc8 --- /dev/null +++ b/inference/SymbolicMultifrontalSolver.h @@ -0,0 +1,30 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicMultifrontalSolver.h + * @brief + * @author Richard Roberts + * @created Oct 22, 2010 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +// The base class provides all of the needed functionality +typedef GenericMultifrontalSolver > > SymbolicMultifrontalSolver; + +} diff --git a/inference/SymbolicSequentialSolver.cpp b/inference/SymbolicSequentialSolver.cpp index bf40088d2..dbe98c233 100644 --- a/inference/SymbolicSequentialSolver.cpp +++ b/inference/SymbolicSequentialSolver.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file SymbolicSequentialSolver.cpp * @brief @@ -10,19 +21,21 @@ namespace gtsam { +// An explicit instantiation to be compiled into the library +template class GenericSequentialSolver; -/* ************************************************************************* */ -SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph& factorGraph) : - Base(factorGraph) {} - -/* ************************************************************************* */ -BayesNet::shared_ptr SymbolicSequentialSolver::eliminate() const { - return Base::eliminate(); -} - -/* ************************************************************************* */ -SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector& js) const { - return SymbolicFactorGraph::shared_ptr(new SymbolicFactorGraph(*Base::joint(js))); -} +///* ************************************************************************* */ +//SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph& factorGraph) : +// Base(factorGraph) {} +// +///* ************************************************************************* */ +//BayesNet::shared_ptr SymbolicSequentialSolver::eliminate() const { +// return Base::eliminate(); +//} +// +///* ************************************************************************* */ +//SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector& js) const { +// return Base::joint(js); +//} } diff --git a/inference/SymbolicSequentialSolver.h b/inference/SymbolicSequentialSolver.h index afcc13fa1..42ac592b1 100644 --- a/inference/SymbolicSequentialSolver.h +++ b/inference/SymbolicSequentialSolver.h @@ -1,10 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file SymbolicSequentialSolver.h * @brief * @author Richard Roberts * @created Oct 21, 2010 */ - #pragma once #include @@ -12,37 +22,40 @@ namespace gtsam { -class SymbolicSequentialSolver : GenericSequentialSolver { +// The base class provides all of the needed functionality +typedef GenericSequentialSolver SymbolicSequentialSolver; -protected: - - typedef GenericSequentialSolver Base; - -public: - - SymbolicSequentialSolver(const FactorGraph& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - BayesNet::shared_ptr eliminate() const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - IndexFactor::shared_ptr marginal(Index j) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as an upper- - * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with - * R*x = d. To get a mean and covariance matrix, use jointStandard(...) - */ - SymbolicFactorGraph::shared_ptr joint(const std::vector& js) const; - -}; +//class SymbolicSequentialSolver : GenericSequentialSolver { +// +//protected: +// +// typedef GenericSequentialSolver Base; +// +//public: +// +// SymbolicSequentialSolver(const FactorGraph& factorGraph); +// +// /** +// * Eliminate the factor graph sequentially. Uses a column elimination tree +// * to recursively eliminate. +// */ +// BayesNet::shared_ptr eliminate() const; +// +// /** +// * Compute the marginal Gaussian density over a variable, by integrating out +// * all of the other variables. This function returns the result as a factor. +// */ +// IndexFactor::shared_ptr marginal(Index j) const; +// +// /** +// * Compute the marginal joint over a set of variables, by integrating out +// * all of the other variables. This function returns the result as an upper- +// * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with +// * R*x = d. To get a mean and covariance matrix, use jointStandard(...) +// */ +// SymbolicFactorGraph::shared_ptr joint(const std::vector& js) const; +// +//}; } diff --git a/inference/VariableIndex.cpp b/inference/VariableIndex.cpp new file mode 100644 index 000000000..9541693ad --- /dev/null +++ b/inference/VariableIndex.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex.cpp + * @brief + * @author Richard Roberts + * @created Oct 22, 2010 + */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void VariableIndex::permute(const Permutation& permutation) { +#ifndef NDEBUG + // Assert that the permutation does not leave behind any non-empty variables, + // otherwise the nFactors and nEntries counts would be incorrect. + for(Index j=0; jindex_.size(); ++j) + if(find(permutation.begin(), permutation.end(), j) == permutation.end()) + assert(this->operator[](j).empty()); +#endif + index_.permute(permutation); +} + +/* ************************************************************************* */ +void VariableIndex::rebaseFactors(ptrdiff_t baseIndexChange) { + BOOST_FOREACH(Factors& factors, index_.container()) { + BOOST_FOREACH(size_t& factor, factors) { + factor += baseIndexChange; + } + } +} + +/* ************************************************************************* */ +bool VariableIndex::equals(const VariableIndex& other, double tol) const { + if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { + for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var) + if(var >= this->index_.size() || var >= other.index_.size()) { + if(!((var >= this->index_.size() && other.index_[var].empty()) || + (var >= other.index_.size() && this->index_[var].empty()))) + return false; + } else if(this->index_[var] != other.index_[var]) + return false; + } else + return false; + return true; +} + +/* ************************************************************************* */ +void VariableIndex::print(const std::string& str) const { + std::cout << str; + Index var = 0; + BOOST_FOREACH(const Factors& variable, index_.container()) { + Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var); + assert(rvar != index_.permutation().end()); + std::cout << "var " << (rvar-index_.permutation().begin()) << ":"; + BOOST_FOREACH(const size_t factor, variable) { + std::cout << " " << factor; + } + std::cout << "\n"; + ++ var; + } + std::cout << std::flush; +} + +} diff --git a/inference/VariableIndex.h b/inference/VariableIndex.h index 0a8637166..250f8e986 100644 --- a/inference/VariableIndex.h +++ b/inference/VariableIndex.h @@ -19,10 +19,10 @@ #pragma once #include +#include #include #include -#include #include #include #include @@ -31,43 +31,23 @@ namespace gtsam { class Inference; -/** - * The VariableIndex can internally use either a vector or a deque to store - * variables. For one-shot elimination, using a vector will give the best - * performance, and this is the default. For incremental updates, such as in - * ISAM and ISAM2, deque storage is used to prevent frequent reallocation. - */ -struct VariableIndexStorage_vector { template struct type_factory { typedef std::vector type; }; }; -struct VariableIndexStorage_deque { template struct type_factory { typedef std::deque type; }; }; - /** * The VariableIndex class stores the information necessary to perform * elimination, including an index of factors involving each variable and * the structure of the intermediate joint factors that develop during - * elimination. This maps variables to deque's of pair, - * which is pairs the factor index with the position of the variable within - * the factor. + * elimination. This maps variables to collections of factor indices. */ -struct _mapped_factor_type { - size_t factorIndex; - size_t variablePosition; - _mapped_factor_type(size_t _factorIndex, size_t _variablePosition) : factorIndex(_factorIndex), variablePosition(_variablePosition) {} - bool operator==(const _mapped_factor_type& o) const { return factorIndex == o.factorIndex && variablePosition == o.variablePosition; } -}; -template class VariableIndex { public: - typedef _mapped_factor_type mapped_factor_type; typedef boost::shared_ptr shared_ptr; - typedef std::deque mapped_type; - typedef typename mapped_type::iterator factor_iterator; - typedef typename mapped_type::const_iterator const_factor_iterator; + typedef FastList Factors; + typedef typename Factors::iterator Factor_iterator; + typedef typename Factors::const_iterator Factor_const_iterator; protected: - typedef typename VARIABLEINDEXSTORAGE::template type_factory::type storage_type; - storage_type indexUnpermuted_; - Permuted index_; + std::vector indexUnpermuted_; + Permuted, Factors&> index_; size_t nFactors_; size_t nEntries_; // Sum of involved variable counts of each factor @@ -79,13 +59,13 @@ public: Index size() const { return index_.size(); } size_t nFactors() const { return nFactors_; } size_t nEntries() const { return nEntries_; } - const mapped_type& operator[](Index variable) const { checkVar(variable); return index_[variable]; } - mapped_type& operator[](Index variable) { checkVar(variable); return index_[variable]; } + const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; } + Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; } void permute(const Permutation& permutation); template void augment(const FactorGraph& factorGraph); void rebaseFactors(ptrdiff_t baseIndexChange); - template bool equals(const Derived& other, double tol=0.0) const; + bool equals(const VariableIndex& other, double tol=0.0) const; void print(const std::string& str = "VariableIndex: ") const; protected: @@ -94,56 +74,32 @@ protected: template void fill(const FactorGraph& factorGraph); - factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } - const_factor_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } - factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } - const_factor_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } - - friend class Inference; + Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } + Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } + Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } + Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } }; /* ************************************************************************* */ -template -void VariableIndex::permute(const Permutation& permutation) { -#ifndef NDEBUG - // Assert that the permutation does not leave behind any non-empty variables, - // otherwise the nFactors and nEntries counts would be incorrect. - for(Index j=0; jindex_.size(); ++j) - if(find(permutation.begin(), permutation.end(), j) == permutation.end()) - assert(this->operator[](j).empty()); -#endif - index_.permute(permutation); -// storage_type original(this->index_.size()); -// this->index_.swap(original); -// for(Index j=0; jindex_[j].swap(original[permutation[j]]); -} - -/* ************************************************************************* */ -template template -void VariableIndex::fill(const FactorGraph& factorGraph) { +void VariableIndex::fill(const FactorGraph& factorGraph) { // Build index mapping from variable id to factor index for(size_t fi=0; fikeys()) { if(key < index_.size()) { - index_[key].push_back(mapped_factor_type(fi, fvari)); - ++ fvari; + index_[key].push_back(fi); ++ nEntries_; } } ++ nFactors_; } - } /* ************************************************************************* */ -template template -VariableIndex::VariableIndex(const FactorGraph& factorGraph) : +VariableIndex::VariableIndex(const FactorGraph& factorGraph) : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { // If the factor graph is empty, return an empty index because inside this @@ -166,21 +122,18 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : fill(factorGraph); } - } /* ************************************************************************* */ -template template -VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : +VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { fill(factorGraph); } /* ************************************************************************* */ -template template -void VariableIndex::augment(const FactorGraph& factorGraph) { +void VariableIndex::augment(const FactorGraph& factorGraph) { // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. if(factorGraph.size() > 0) { @@ -206,10 +159,8 @@ void VariableIndex::augment(const FactorGraph& factorGraph) { size_t orignFactors = nFactors_; for(size_t fi=0; fikeys()) { - index_[key].push_back(mapped_factor_type(orignFactors + fi, fvari)); - ++ fvari; + index_[key].push_back(orignFactors + fi); ++ nEntries_; } ++ nFactors_; @@ -217,49 +168,4 @@ void VariableIndex::augment(const FactorGraph& factorGraph) { } } -/* ************************************************************************* */ -template -void VariableIndex::rebaseFactors(ptrdiff_t baseIndexChange) { - BOOST_FOREACH(mapped_type& factors, index_.container()) { - BOOST_FOREACH(mapped_factor_type& factor, factors) { - factor.factorIndex += baseIndexChange; - } - } -} - -/* ************************************************************************* */ -template -template -bool VariableIndex::equals(const Derived& other, double tol) const { - if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { - for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var) - if(var >= this->index_.size() || var >= other.index_.size()) { - if(!((var >= this->index_.size() && other.index_[var].empty()) || - (var >= other.index_.size() && this->index_[var].empty()))) - return false; - } else if(this->index_[var] != other.index_[var]) - return false; - } else - return false; - return true; -} - -/* ************************************************************************* */ -template -void VariableIndex::print(const std::string& str) const { - std::cout << str; - Index var = 0; - BOOST_FOREACH(const mapped_type& variable, index_.container()) { - Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var); - assert(rvar != index_.permutation().end()); - std::cout << "var " << (rvar-index_.permutation().begin()) << ":"; - BOOST_FOREACH(const mapped_factor_type& factor, variable) { - std::cout << " " << factor.factorIndex << "-" << factor.variablePosition; - } - std::cout << "\n"; - ++ var; - } - std::cout << std::flush; -} - } diff --git a/inference/inference-inl.h b/inference/inference-inl.h index 7f1845e66..727325e6d 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -33,7 +33,7 @@ #include #include #include -#include +#include using namespace std; @@ -54,13 +54,13 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& va p[0] = 0; int count = 0; for(Index var = 0; var < variableIndex.size(); ++var) { - const typename VARIABLEINDEXTYPE::mapped_type& column(variableIndex[var]); + const typename VARIABLEINDEXTYPE::Factors& column(variableIndex[var]); size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(const typename VARIABLEINDEXTYPE::mapped_factor_type& factor_pos, column) { + BOOST_FOREACH(const size_t& factorIndex, column) { if(lastFactorId != numeric_limits::max()) - assert(factor_pos.factorIndex > lastFactorId); - A[count++] = factor_pos.factorIndex; // copy sparse column - if(debug) cout << "A[" << count-1 << "] = " << factor_pos.factorIndex << endl; + assert(factorIndex > lastFactorId); + A[count++] = factorIndex; // copy sparse column + if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl; } p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 cmember[var] = 0; diff --git a/inference/tests/testVariableIndex.cpp b/inference/tests/testVariableIndex.cpp index 388f3b825..58ea05a63 100644 --- a/inference/tests/testVariableIndex.cpp +++ b/inference/tests/testVariableIndex.cpp @@ -39,8 +39,8 @@ TEST(VariableIndex, augment) { SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); - VariableIndex<> expected(fgCombined); - VariableIndex<> actual(fg1); + VariableIndex expected(fgCombined); + VariableIndex actual(fg1); actual.augment(fg2); CHECK(assert_equal(expected, actual)); diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index 33b1dfddc..4e3cc4f90 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -556,196 +556,196 @@ struct _RowSource { bool operator<(const _RowSource& o) const { return firstNonzeroVar < o.firstNonzeroVar; } }; -/* Explicit instantiations for storage types */ -template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); -template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); - -// Utility function to determine row count and check if any noise models are constrained -// TODO: would be nicer if this function was split and are factorgraph methods -static std::pair rowCount(const FactorGraph& factorGraph, - const vector& factorIndices) -{ - static const bool debug = false; - tic("Combine: count sizes"); - size_t m = 0; - bool constrained = false; - BOOST_FOREACH(const size_t i, factorIndices) - { - assert(factorGraph[i] != NULL); - assert(!factorGraph[i]->keys().empty()); - m += factorGraph[i]->numberOfRows(); - if (debug) cout << "Combining factor " << i << endl; - if (debug) factorGraph[i]->print(" :"); - if (!constrained && factorGraph[i]->isConstrained()) { - constrained = true; - if (debug) std::cout << "Found a constraint!" << std::endl; - } - } - toc("Combine: count sizes"); - return make_pair(m,constrained); -} - -// Determine row and column counts and check if any noise models are constrained -template -static vector columnDimensions( - const GaussianVariableIndex& variableIndex, - const vector& variables) -{ - tic("Combine: count dims"); - static const bool debug = false; - vector dims(variables.size() + 1); - size_t n = 0; - { - size_t j = 0; - BOOST_FOREACH(const Index& var, variables) - { - if (debug) cout << "Have variable " << var << endl; - dims[j] = variableIndex.dim(var); - n += dims[j]; - ++j; - } - dims[j] = 1; - } - toc("Combine: count dims"); - return dims; -} - -// To do this, we merge-sort the rows so that the column indices of the first structural -// non-zero in each row increase monotonically. -vector<_RowSource> computeRowPermutation(size_t m, const vector& factorIndices, - const FactorGraph& factorGraph) { - vector<_RowSource> rowSources; - rowSources.reserve(m); - size_t i1 = 0; - BOOST_FOREACH(const size_t i2, factorIndices) { - const GaussianFactor& factor(*factorGraph[i2]); - size_t factorRowI = 0; - assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows()); - BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) { - Index firstNonzeroVar; - firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos]; - rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI)); - ++ factorRowI; - } - assert(factorRowI == factor.numberOfRows()); - ++ i1; - } - assert(rowSources.size() == m); - assert(i1 == factorIndices.size()); - sort(rowSources.begin(), rowSources.end()); - return rowSources; -} - -void copyMatrices(boost::shared_ptr combinedFactor, size_t row, - const GaussianFactor& factor, const std::vector >& variablePositions, - const size_t factorRow, const size_t factorI, const vector& variables) { - const static bool debug = false; - const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow]; - std::vector::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos; - std::vector::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos; - combinedFactor->set_firstNonzeroBlocks(row, *varposIt); - if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl; - std::vector::const_iterator keyitend = factor.keys().end(); - while(keyit != keyitend) { - const size_t varpos = *varposIt; - assert(variables[varpos] == *keyit); - GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos)); - const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit)); - ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow); - ++ keyit; - ++ varposIt; - } -} - -template -GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const vector& factorIndices, - const vector& variables, const std::vector >& variablePositions) { - - // Debugging flags - static const bool verbose = false; - static const bool debug = false; - if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl; - assert(factorIndices.size() == variablePositions.size()); - - // Determine row count and check if any noise models are constrained - size_t m; bool constrained; - boost::tie(m,constrained) = rowCount(factorGraph,factorIndices); - - // Determine column dimensions - vector dims = columnDimensions(variableIndex,variables); - - // Allocate return value, the combined factor, the augmented Ab matrix and other arrays - tic("Combine: set up empty"); - shared_ptr combinedFactor(boost::make_shared()); - combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims.begin(), dims.end(), m)); - ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); - combinedFactor->firstNonzeroBlocks_.resize(m); - Vector sigmas(m); - - // Copy keys - combinedFactor->keys_.reserve(variables.size()); - combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end()); - toc("Combine: set up empty"); - - // Compute a row permutation that maintains a staircase pattern in the new combined factor. - tic("Combine: sort rows"); - vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph); - toc("Combine: sort rows"); - - // Fill in the rows of the new factor in sorted order. Fill in the array of - // the left-most nonzero for each row and the first structural zero in each column. - // todo SL: smarter ignoring of zero factor variables (store first possible like above) - - if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: "); - - tic("Combine: copy rows"); -#ifndef NDEBUG - size_t lastRowFirstVarpos = 0; -#endif - for(size_t row=0; rowgetb()(row) = factor.getb()(factorRow); - sigmas(row) = factor.get_sigmas()(factorRow); - - // Copy the row of A variable by variable, starting at the first nonzero variable. - copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables); - -#ifndef NDEBUG - // Debug check, make sure the first column of nonzeros increases monotonically - if(row != 0) - assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos); - lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row]; -#endif - } - toc("Combine: copy rows"); - - if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl; - - if (constrained) { - combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas); - if (verbose) combinedFactor->model_->print("Just created Constraint ^"); - } else { - combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas); - if (verbose) combinedFactor->model_->print("Just created Diagonal"); - } - - if(debug) combinedFactor->print("Combined factor: "); - - combinedFactor->assertInvariants(); - - return combinedFactor; -} +///* Explicit instantiations for storage types */ +//template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +//template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +// +//// Utility function to determine row count and check if any noise models are constrained +//// TODO: would be nicer if this function was split and are factorgraph methods +//static std::pair rowCount(const FactorGraph& factorGraph, +// const vector& factorIndices) +//{ +// static const bool debug = false; +// tic("Combine: count sizes"); +// size_t m = 0; +// bool constrained = false; +// BOOST_FOREACH(const size_t i, factorIndices) +// { +// assert(factorGraph[i] != NULL); +// assert(!factorGraph[i]->keys().empty()); +// m += factorGraph[i]->numberOfRows(); +// if (debug) cout << "Combining factor " << i << endl; +// if (debug) factorGraph[i]->print(" :"); +// if (!constrained && factorGraph[i]->isConstrained()) { +// constrained = true; +// if (debug) std::cout << "Found a constraint!" << std::endl; +// } +// } +// toc("Combine: count sizes"); +// return make_pair(m,constrained); +//} +// +//// Determine row and column counts and check if any noise models are constrained +//template +//static vector columnDimensions( +// const GaussianVariableIndex& variableIndex, +// const vector& variables) +//{ +// tic("Combine: count dims"); +// static const bool debug = false; +// vector dims(variables.size() + 1); +// size_t n = 0; +// { +// size_t j = 0; +// BOOST_FOREACH(const Index& var, variables) +// { +// if (debug) cout << "Have variable " << var << endl; +// dims[j] = variableIndex.dim(var); +// n += dims[j]; +// ++j; +// } +// dims[j] = 1; +// } +// toc("Combine: count dims"); +// return dims; +//} +// +//// To do this, we merge-sort the rows so that the column indices of the first structural +//// non-zero in each row increase monotonically. +//vector<_RowSource> computeRowPermutation(size_t m, const vector& factorIndices, +// const FactorGraph& factorGraph) { +// vector<_RowSource> rowSources; +// rowSources.reserve(m); +// size_t i1 = 0; +// BOOST_FOREACH(const size_t i2, factorIndices) { +// const GaussianFactor& factor(*factorGraph[i2]); +// size_t factorRowI = 0; +// assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows()); +// BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) { +// Index firstNonzeroVar; +// firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos]; +// rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI)); +// ++ factorRowI; +// } +// assert(factorRowI == factor.numberOfRows()); +// ++ i1; +// } +// assert(rowSources.size() == m); +// assert(i1 == factorIndices.size()); +// sort(rowSources.begin(), rowSources.end()); +// return rowSources; +//} +// +//void copyMatrices(boost::shared_ptr combinedFactor, size_t row, +// const GaussianFactor& factor, const std::vector >& variablePositions, +// const size_t factorRow, const size_t factorI, const vector& variables) { +// const static bool debug = false; +// const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow]; +// std::vector::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos; +// std::vector::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos; +// combinedFactor->set_firstNonzeroBlocks(row, *varposIt); +// if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl; +// std::vector::const_iterator keyitend = factor.keys().end(); +// while(keyit != keyitend) { +// const size_t varpos = *varposIt; +// assert(variables[varpos] == *keyit); +// GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos)); +// const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit)); +// ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow); +// ++ keyit; +// ++ varposIt; +// } +//} +// +//template +//GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, +// const GaussianVariableIndex& variableIndex, const vector& factorIndices, +// const vector& variables, const std::vector >& variablePositions) { +// +// // Debugging flags +// static const bool verbose = false; +// static const bool debug = false; +// if (verbose) std::cout << "GaussianFactor::GaussianFactor (factors)" << std::endl; +// assert(factorIndices.size() == variablePositions.size()); +// +// // Determine row count and check if any noise models are constrained +// size_t m; bool constrained; +// boost::tie(m,constrained) = rowCount(factorGraph,factorIndices); +// +// // Determine column dimensions +// vector dims = columnDimensions(variableIndex,variables); +// +// // Allocate return value, the combined factor, the augmented Ab matrix and other arrays +// tic("Combine: set up empty"); +// shared_ptr combinedFactor(boost::make_shared()); +// combinedFactor->Ab_.copyStructureFrom(ab_type(combinedFactor->matrix_, dims.begin(), dims.end(), m)); +// ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); +// combinedFactor->firstNonzeroBlocks_.resize(m); +// Vector sigmas(m); +// +// // Copy keys +// combinedFactor->keys_.reserve(variables.size()); +// combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end()); +// toc("Combine: set up empty"); +// +// // Compute a row permutation that maintains a staircase pattern in the new combined factor. +// tic("Combine: sort rows"); +// vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph); +// toc("Combine: sort rows"); +// +// // Fill in the rows of the new factor in sorted order. Fill in the array of +// // the left-most nonzero for each row and the first structural zero in each column. +// // todo SL: smarter ignoring of zero factor variables (store first possible like above) +// +// if(debug) gtsam::print(combinedFactor->matrix_, "matrix_ before copying rows: "); +// +// tic("Combine: copy rows"); +//#ifndef NDEBUG +// size_t lastRowFirstVarpos = 0; +//#endif +// for(size_t row=0; rowgetb()(row) = factor.getb()(factorRow); +// sigmas(row) = factor.get_sigmas()(factorRow); +// +// // Copy the row of A variable by variable, starting at the first nonzero variable. +// copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables); +// +//#ifndef NDEBUG +// // Debug check, make sure the first column of nonzeros increases monotonically +// if(row != 0) +// assert(combinedFactor->firstNonzeroBlocks_[row] >= lastRowFirstVarpos); +// lastRowFirstVarpos = combinedFactor->firstNonzeroBlocks_[row]; +//#endif +// } +// toc("Combine: copy rows"); +// +// if (verbose) std::cout << "GaussianFactor::GaussianFactor done" << std::endl; +// +// if (constrained) { +// combinedFactor->model_ = noiseModel::Constrained::MixedSigmas(sigmas); +// if (verbose) combinedFactor->model_->print("Just created Constraint ^"); +// } else { +// combinedFactor->model_ = noiseModel::Diagonal::Sigmas(sigmas); +// if (verbose) combinedFactor->model_->print("Just created Diagonal"); +// } +// +// if(debug) combinedFactor->print("Combined factor: "); +// +// combinedFactor->assertInvariants(); +// +// return combinedFactor; +//} /* ************************************************************************* */ // Helper functions for Combine diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index e1d8bfa22..55cacacac 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -48,7 +48,6 @@ namespace gtsam { class GaussianFactorGraph; -template class GaussianVariableIndex; /** A map from key to dimension, useful in various contexts */ typedef std::map Dimensions; @@ -146,13 +145,13 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); - /** - * Named constructor for combining a set of factors with pre-computed set of variables. - */ - template - static shared_ptr Combine(const FactorGraph& factorGraph, - const GaussianVariableIndex& variableIndex, const std::vector& factorIndices, - const std::vector& variables, const std::vector >& variablePositions); +// /** +// * Named constructor for combining a set of factors with pre-computed set of variables. +// */ +// template +// static shared_ptr Combine(const FactorGraph& factorGraph, +// const GaussianVariableIndex& variableIndex, const std::vector& factorIndices, +// const std::vector& variables, const std::vector >& variablePositions); /** * Named constructor for combining a set of factors with pre-computed set of variables. diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index 4956ca4e2..b96f936a0 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -146,25 +146,21 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg } /* ************************************************************************* */ -GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const { +GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector& dimensions) const { // start with this factor graph GaussianFactorGraph result = *this; // for each of the variables, add a prior - for(Index var=0; var(*this)) ; -} - } // namespace gtsam diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 3f2a36173..0112c907a 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -39,7 +39,6 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef GaussianBayesNet bayesnet_type; - typedef GaussianVariableIndex<> variableindex_type; /** * Default constructor @@ -152,122 +151,8 @@ namespace gtsam { * Add zero-mean i.i.d. Gaussian prior terms to each variable * @param sigma Standard deviation of Gaussian */ - GaussianFactorGraph add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const; - GaussianFactorGraph add_priors(double sigma) const; - + GaussianFactorGraph add_priors(double sigma, const std::vector& dimensions) const; }; - - /* ************************************************************************* */ - template - class GaussianVariableIndex : public VariableIndex { - public: - typedef VariableIndex Base; - typedef typename VARIABLEINDEXSTORAGE::template type_factory::type storage_type; - - storage_type dims_; - - public: - typedef boost::shared_ptr shared_ptr; - - /** Construct an empty GaussianVariableIndex */ - GaussianVariableIndex() {} - - /** - * Constructor from a GaussianFactorGraph, lets the base class build the - * column-wise index then fills the dims_ array. - */ - GaussianVariableIndex(const GaussianFactorGraph& factorGraph); - - /** - * Constructor to "upgrade" from the base class without recomputing the - * column index, i.e. just fills the dims_ array. - */ - GaussianVariableIndex(const VariableIndex& variableIndex, const GaussianFactorGraph& factorGraph); - - /** - * Another constructor to upgrade from the base class using an existing - * array of variable dimensions. - */ - GaussianVariableIndex(const VariableIndex& variableIndex, const storage_type& dimensions); - - const storage_type& dims() const { return dims_; } - size_t dim(Index variable) const { Base::checkVar(variable); return dims_[variable]; } - - /** Permute */ - void permute(const Permutation& permutation); - - /** Augment this variable index with the contents of another one */ - void augment(const GaussianFactorGraph& factorGraph); - - protected: - GaussianVariableIndex(size_t nVars) : Base(nVars), dims_(nVars) {} - void fillDims(const GaussianFactorGraph& factorGraph); - }; - - - /* ************************************************************************* */ - template - GaussianVariableIndex::GaussianVariableIndex(const GaussianFactorGraph& factorGraph) : - Base(factorGraph), dims_(Base::index_.size()) { - fillDims(factorGraph); } - - /* ************************************************************************* */ - template - GaussianVariableIndex::GaussianVariableIndex( - const VariableIndex& variableIndex, const GaussianFactorGraph& factorGraph) : - Base(variableIndex), dims_(Base::index_.size()) { - fillDims(factorGraph); } - - /* ************************************************************************* */ - template - GaussianVariableIndex::GaussianVariableIndex( - const VariableIndex& variableIndex, const storage_type& dimensions) : - Base(variableIndex), dims_(dimensions) { - assert(Base::index_.size() == dims_.size()); } - - /* ************************************************************************* */ - template - void GaussianVariableIndex::fillDims(const GaussianFactorGraph& factorGraph) { - // Store dimensions of each variable - assert(dims_.size() == Base::index_.size()); - for(Index var=0; var factor(factorGraph[factorIndex]); - dims_[var] = factor->getDim(factor->begin() + variablePosition); - } else - dims_[var] = 0; - } - - /* ************************************************************************* */ - template - void GaussianVariableIndex::permute(const Permutation& permutation) { - VariableIndex::permute(permutation); - storage_type original(this->dims_.size()); - this->dims_.swap(original); - for(Index j=0; jdims_[j] = original[permutation[j]]; - } - - /* ************************************************************************* */ - template - void GaussianVariableIndex::augment(const GaussianFactorGraph& factorGraph) { - Base::augment(factorGraph); - dims_.resize(Base::index_.size(), 0); - BOOST_FOREACH(boost::shared_ptr factor, factorGraph) { - for(GaussianFactor::const_iterator var=factor->begin(); var!=factor->end(); ++var) { -#ifndef NDEBUG - if(dims_[*var] != 0) - assert(dims_[*var] == factor->getDim(var)); -#endif - if(dims_[*var] == 0) - dims_[*var] = factor->getDim(var); - } - } - } - - } // namespace gtsam diff --git a/linear/GaussianMultifrontalSolver.cpp b/linear/GaussianMultifrontalSolver.cpp index 06ea42b2e..6027cf27f 100644 --- a/linear/GaussianMultifrontalSolver.cpp +++ b/linear/GaussianMultifrontalSolver.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GaussianMultifrontalSolver.cpp * @brief @@ -13,23 +24,21 @@ namespace gtsam { /* ************************************************************************* */ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph& factorGraph) : - junctionTree_(new GaussianJunctionTree(factorGraph)) {} + Base(factorGraph) {} /* ************************************************************************* */ -BayesTree::sharedClique GaussianMultifrontalSolver::eliminate() const { - return junctionTree_->eliminate(); +BayesTree::shared_ptr GaussianMultifrontalSolver::eliminate() const { + return Base::eliminate(); } /* ************************************************************************* */ VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { - return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize())); + return VectorValues::shared_ptr(new VectorValues(junctionTree_.optimize())); } /* ************************************************************************* */ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { - BayesTree bayesTree; - bayesTree.insert(junctionTree_->eliminate()); - return bayesTree.marginal(j); + return Base::marginal(j); } } diff --git a/linear/GaussianMultifrontalSolver.h b/linear/GaussianMultifrontalSolver.h index 1fb8df845..a8ac17179 100644 --- a/linear/GaussianMultifrontalSolver.h +++ b/linear/GaussianMultifrontalSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file GaussianMultifrontalSolver.h * @brief @@ -7,6 +18,7 @@ #pragma once +#include #include #include #include @@ -38,11 +50,11 @@ namespace gtsam { * typedef'ed in linear/GaussianBayesNet, on which this class calls * optimize(...) to perform back-substitution. */ -class GaussianMultifrontalSolver { +class GaussianMultifrontalSolver : GenericMultifrontalSolver { protected: - GaussianJunctionTree::shared_ptr junctionTree_; + typedef GenericMultifrontalSolver Base; public: @@ -56,7 +68,7 @@ public: * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - BayesTree::sharedClique eliminate() const; + BayesTree::shared_ptr eliminate() const; /** * Compute the least-squares solution of the GaussianFactorGraph. This diff --git a/linear/GaussianSequentialSolver.cpp b/linear/GaussianSequentialSolver.cpp index 5f6b308f4..dcfba358a 100644 --- a/linear/GaussianSequentialSolver.cpp +++ b/linear/GaussianSequentialSolver.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file SequentialSolver.cpp * @brief diff --git a/linear/GaussianSequentialSolver.h b/linear/GaussianSequentialSolver.h index de9ab6775..31065aff3 100644 --- a/linear/GaussianSequentialSolver.h +++ b/linear/GaussianSequentialSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file SequentialSolver.h * @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination. diff --git a/linear/SubgraphPreconditioner.cpp b/linear/SubgraphPreconditioner.cpp index d8d556d66..097b0f814 100644 --- a/linear/SubgraphPreconditioner.cpp +++ b/linear/SubgraphPreconditioner.cpp @@ -41,11 +41,11 @@ namespace gtsam { return x; } - SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { - SubgraphPreconditioner result = *this ; - result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; - return result ; - } +// SubgraphPreconditioner SubgraphPreconditioner::add_priors(double sigma) const { +// SubgraphPreconditioner result = *this ; +// result.Ab2_ = sharedFG(new GaussianFactorGraph(Ab2_->add_priors(sigma))) ; +// return result ; +// } /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { diff --git a/linear/SubgraphPreconditioner.h b/linear/SubgraphPreconditioner.h index 49fe760ba..e5c9ff7ff 100644 --- a/linear/SubgraphPreconditioner.h +++ b/linear/SubgraphPreconditioner.h @@ -60,7 +60,7 @@ namespace gtsam { * Add zero-mean i.i.d. Gaussian prior terms to each variable * @param sigma Standard deviation of Gaussian */ - SubgraphPreconditioner add_priors(double sigma) const; +// SubgraphPreconditioner add_priors(double sigma) const; /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; diff --git a/linear/tests/testGaussianFactor.cpp b/linear/tests/testGaussianFactor.cpp index c12855540..b34494c0c 100644 --- a/linear/tests/testGaussianFactor.cpp +++ b/linear/tests/testGaussianFactor.cpp @@ -84,59 +84,59 @@ TEST( GaussianFactor, constructor2) CHECK(assert_equal(b, actualb)); } -/* ************************************************************************* */ -TEST(GaussianFactor, Combine) -{ - Matrix A00 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 0.0, 0.0, 0.0); - Vector s0 = Vector_(3, 0.0, 0.0, 0.0); - - Matrix A10 = Matrix_(3,3, - 0.0, -2.0, -4.0, - 2.0, 0.0, 2.0, - 0.0, 0.0, -10.0); - Matrix A11 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 10.0); - Vector b1 = Vector_(3, 6.0, 2.0, 0.0); - Vector s1 = Vector_(3, 1.0, 1.0, 1.0); - - Matrix A20 = Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0); - Vector b2 = Vector_(3, 0.0, 0.0, 0.0); - Vector s2 = Vector_(3, 100.0, 100.0, 100.0); - - GaussianFactorGraph gfg; - gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - GaussianVariableIndex<> varindex(gfg); - vector factors(3); factors[0]=0; factors[1]=1; factors[2]=2; - vector variables(2); variables[0]=0; variables[1]=1; - vector > variablePositions(3); - variablePositions[0].resize(1); variablePositions[0][0]=0; - variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1; - variablePositions[2].resize(1); variablePositions[2][0]=0; - - GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A00, &A10, &A20); - Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3); - Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); - Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); - - GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - - CHECK(assert_equal(expected, actual)); -} +///* ************************************************************************* */ +//TEST(GaussianFactor, Combine) +//{ +// Matrix A00 = Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 1.0, 0.0, +// 0.0, 0.0, 1.0); +// Vector b0 = Vector_(3, 0.0, 0.0, 0.0); +// Vector s0 = Vector_(3, 0.0, 0.0, 0.0); +// +// Matrix A10 = Matrix_(3,3, +// 0.0, -2.0, -4.0, +// 2.0, 0.0, 2.0, +// 0.0, 0.0, -10.0); +// Matrix A11 = Matrix_(3,3, +// 2.0, 0.0, 0.0, +// 0.0, 2.0, 0.0, +// 0.0, 0.0, 10.0); +// Vector b1 = Vector_(3, 6.0, 2.0, 0.0); +// Vector s1 = Vector_(3, 1.0, 1.0, 1.0); +// +// Matrix A20 = Matrix_(3,3, +// 1.0, 0.0, 0.0, +// 0.0, 1.0, 0.0, +// 0.0, 0.0, 1.0); +// Vector b2 = Vector_(3, 0.0, 0.0, 0.0); +// Vector s2 = Vector_(3, 100.0, 100.0, 100.0); +// +// GaussianFactorGraph gfg; +// gfg.add(0, A00, b0, noiseModel::Diagonal::Sigmas(s0, true)); +// gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); +// gfg.add(0, A20, b2, noiseModel::Diagonal::Sigmas(s2, true)); +// +// GaussianVariableIndex varindex(gfg); +// vector factors(3); factors[0]=0; factors[1]=1; factors[2]=2; +// vector variables(2); variables[0]=0; variables[1]=1; +// vector > variablePositions(3); +// variablePositions[0].resize(1); variablePositions[0][0]=0; +// variablePositions[1].resize(2); variablePositions[1][0]=0; variablePositions[1][1]=1; +// variablePositions[2].resize(1); variablePositions[2][0]=0; +// +// GaussianFactor actual = *GaussianFactor::Combine(gfg, varindex, factors, variables, variablePositions); +// +// Matrix zero3x3 = zeros(3,3); +// Matrix A0 = gtsam::stack(3, &A00, &A10, &A20); +// Matrix A1 = gtsam::stack(3, &zero3x3, &A11, &zero3x3); +// Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); +// Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); +// +// GaussianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); +// +// CHECK(assert_equal(expected, actual)); +//} /* ************************************************************************* */ TEST(GaussianFactor, Combine2) @@ -712,23 +712,23 @@ TEST(GaussianFactor, permuteWithInverse) GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0)); GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual))); - GaussianVariableIndex<> actualIndex(actualFG); + VariableIndex actualIndex(actualFG); actual.permuteWithInverse(inversePermutation); // actualIndex.permute(*inversePermutation.inverse()); GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected))); -// GaussianVariableIndex<> expectedIndex(expectedFG); +// GaussianVariableIndex expectedIndex(expectedFG); CHECK(assert_equal(expected, actual)); // // todo: fix this!!! VariableIndex should not hold slots // for(Index j=0; j::mapped_factor_type& factor_pos, actualIndex[j]) { +// BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, actualIndex[j]) { // factor_pos.variablePosition = numeric_limits::max(); } // } // for(Index j=0; j::mapped_factor_type& factor_pos, expectedIndex[j]) { +// BOOST_FOREACH( GaussianVariableIndex::mapped_factor_type& factor_pos, expectedIndex[j]) { // factor_pos.variablePosition = numeric_limits::max(); } // } // CHECK(assert_equal(expectedIndex, actualIndex)); diff --git a/nonlinear/NonlinearFactorGraph-inl.h b/nonlinear/NonlinearFactorGraph-inl.h index 316875ae6..a0f262fbd 100644 --- a/nonlinear/NonlinearFactorGraph-inl.h +++ b/nonlinear/NonlinearFactorGraph-inl.h @@ -68,7 +68,7 @@ void NonlinearFactorGraph::print(const std::string& str) const { boost::tie(symbolic,ordering) = this->symbolic(config); // Compute the VariableIndex (column-wise index) - VariableIndex<> variableIndex(*symbolic); + VariableIndex variableIndex(*symbolic); // Compute a fill-reducing ordering with COLAMD Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex)); diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index a9663b03b..4abb99e72 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -75,14 +75,14 @@ namespace gtsam { template NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, double lambda) : - graph_(graph), config_(config), ordering_(ordering), lambda_(lambda) { + graph_(graph), config_(config), error_(graph->error(*config)), + ordering_(ordering), lambda_(lambda), dimensions_(new vector(config->dims(*ordering))) { if (!graph) throw std::invalid_argument( "NonlinearOptimizer constructor: graph = NULL"); if (!config) throw std::invalid_argument( "NonlinearOptimizer constructor: config = NULL"); if (!ordering) throw std::invalid_argument( "NonlinearOptimizer constructor: ordering = NULL"); - error_ = graph->error(*config); } /* ************************************************************************* */ @@ -91,7 +91,7 @@ namespace gtsam { template VectorValues NonlinearOptimizer::linearizeAndOptimizeForDelta() const { boost::shared_ptr linearized = graph_->linearize(*config_, *ordering_); - NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); +// NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); return *S(*linearized).optimize(); } @@ -115,7 +115,7 @@ namespace gtsam { if (verbosity >= Parameters::CONFIG) newValues->print("newValues"); - NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, ordering_, lambda_); + NonlinearOptimizer newOptimizer = newValues_(newValues); if (verbosity >= Parameters::ERROR) cout << "error: " << newOptimizer.error_ << endl; @@ -172,7 +172,7 @@ namespace gtsam { cout << "trying lambda = " << lambda_ << endl; // add prior-factors - L damped = linear.add_priors(1.0/sqrt(lambda_)); + L damped = linear.add_priors(1.0/sqrt(lambda_), *dimensions_); if (verbosity >= Parameters::DAMPED) damped.print("damped"); @@ -187,7 +187,7 @@ namespace gtsam { // newValues->print("config"); // create new optimization state with more adventurous lambda - NonlinearOptimizer next(graph_, newValues, ordering_, lambda_ / factor); + NonlinearOptimizer next(newValuesNewLambda_(newValues, lambda_ / factor)); if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << next.error_ << endl; if(lambdaMode >= Parameters::CAUTIOUS) { @@ -199,7 +199,7 @@ namespace gtsam { // If we're cautious, see if the current lambda is better // todo: include stopping criterion here? if(lambdaMode == Parameters::CAUTIOUS) { - NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); + NonlinearOptimizer sameLambda(newValues_(newValues)); if(sameLambda.error_ <= next.error_) return sameLambda; } @@ -212,7 +212,7 @@ namespace gtsam { // A more adventerous lambda was worse. If we're cautious, try the same lambda. if(lambdaMode == Parameters::CAUTIOUS) { - NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); + NonlinearOptimizer sameLambda(newValues_(newValues)); if(sameLambda.error_ <= error_) return sameLambda; } @@ -223,9 +223,9 @@ namespace gtsam { // TODO: can we avoid copying the config ? if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) { - return NonlinearOptimizer(graph_, newValues, ordering_, lambda_);; + return NonlinearOptimizer(newValues_(newValues)); } else { - NonlinearOptimizer cautious(graph_, config_, ordering_, lambda_ * factor); + NonlinearOptimizer cautious(newLambda_(lambda_ * factor)); return cautious.try_lambda(linear, verbosity, factor, lambdaMode); } @@ -249,13 +249,12 @@ namespace gtsam { // linearize all factors once boost::shared_ptr linear = graph_->linearize(*config_, *ordering_); - NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); if (verbosity >= Parameters::LINEAR) linear->print("linear"); // try lambda steps with successively larger lambda until we achieve descent if (verbosity >= Parameters::LAMBDA) cout << "Trying Lambda for the first time" << endl; - return prepared.try_lambda(*linear, verbosity, lambdaFactor, lambdaMode); + return try_lambda(*linear, verbosity, lambdaFactor, lambdaMode); } /* ************************************************************************* */ diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index bf2c40457..b30e6d5d1 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -70,13 +70,14 @@ namespace gtsam { // For performance reasons in recursion, we store configs in a shared_ptr typedef boost::shared_ptr shared_values; typedef boost::shared_ptr shared_graph; - typedef boost::shared_ptr shared_ordering; - //typedef boost::shared_ptr shared_solver; - //typedef const GS solver; + typedef boost::shared_ptr shared_ordering; typedef NonlinearOptimizationParameters Parameters; private: + typedef NonlinearOptimizer This; + typedef boost::shared_ptr > shared_dimensions; + // keep a reference to const version of the graph // These normally do not change const shared_graph graph_; @@ -84,7 +85,7 @@ namespace gtsam { // keep a values structure and its error // These typically change once per iteration (in a functional way) const shared_values config_; - double error_; // TODO FD: no more const because in constructor I need to set it after checking :-( + const double error_; const shared_ordering ordering_; // the linear system solver @@ -94,10 +95,30 @@ namespace gtsam { // TODO: red flag, should we have an LM class ? const double lambda_; + // The dimensions of each linearized variable + const shared_dimensions dimensions_; + // Recursively try to do tempered Gauss-Newton steps until we succeed NonlinearOptimizer try_lambda(const L& linear, Parameters::verbosityLevel verbosity, double factor, Parameters::LambdaMode lambdaMode) const; + /** + * Constructor that does not do any computation + */ + NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, + const double error, const double lambda, shared_dimensions dimensions): graph_(graph), config_(config), + error_(error), ordering_(ordering), lambda_(lambda), dimensions_(dimensions) {} + + /** Create a new NonlinearOptimizer with a different lambda */ + This newLambda_(double newLambda) const { + return NonlinearOptimizer(graph_, config_, ordering_, error_, newLambda, dimensions_); } + + This newValues_(shared_values newValues) const { + return NonlinearOptimizer(graph_, newValues, ordering_, graph_->error(*newValues), lambda_, dimensions_); } + + This newValuesNewLambda_(shared_values newValues, double newLambda) const { + return NonlinearOptimizer(graph_, newValues, ordering_, graph_->error(*newValues), newLambda, dimensions_); } + public: /** @@ -106,19 +127,12 @@ namespace gtsam { NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, const double lambda = 1e-5); - /** - * Constructor that does not do any computation - */ - NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, - const double error, const double lambda): graph_(graph), config_(config), - error_(error), ordering_(ordering), lambda_(lambda) {} - /** * Copy constructor */ NonlinearOptimizer(const NonlinearOptimizer &optimizer) : - graph_(optimizer.graph_), config_(optimizer.config_), - error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_) {} + graph_(optimizer.graph_), config_(optimizer.config_), error_(optimizer.error_), + ordering_(optimizer.ordering_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {} /** * Return current error diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index 5f3c66e09..66d7c1d3e 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -344,7 +344,7 @@ TEST( GaussianFactorGraph, add_priors ) { Ordering ordering; ordering += "l1","x1","x2"; GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianFactorGraph actual = fg.add_priors(3, GaussianVariableIndex<>(fg)); + GaussianFactorGraph actual = fg.add_priors(3, vector(3,2)); GaussianFactorGraph expected = createGaussianFactorGraph(ordering); Matrix A = eye(2); Vector b = zero(2); @@ -460,7 +460,7 @@ TEST( GaussianFactorGraph, getOrdering) { Ordering original; original += "l1","x1","x2"; FactorGraph symbolic(createGaussianFactorGraph(original)); - Permutation perm(*Inference::PermutationCOLAMD(VariableIndex<>(symbolic))); + Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic))); Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering expected; expected += "l1","x2","x1"; CHECK(assert_equal(expected,actual)); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 6e059921b..d0a747488 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -266,7 +266,7 @@ TEST( BayesTree, balanced_smoother_shortcuts ) // push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2)); // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]]; // GaussianFactorGraph marginal = C3->marginal(R); -// GaussianVariableIndex<> varIndex(marginal); +// GaussianVariableIndex varIndex(marginal); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); // Permutation toFrontInverse(*toFront.inverse()); // varIndex.permute(toFront); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index f8b618ffd..57aecdf46 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -94,7 +94,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) VectorValues actual = tree.optimize(); // verify - VectorValues expected(GaussianVariableIndex<>(fg).dims()); // expected solution + VectorValues expected(vector(7,2)); // expected solution Vector v = Vector_(2, 0., 0.); for (int i=1; i<=7; i++) expected[ordering[Symbol('x',i)]] = v;