diff --git a/gtsam/inference/BayesNetOrdered-inl.h b/gtsam/inference/BayesNetOrdered-inl.h deleted file mode 100644 index bdf07942b..000000000 --- a/gtsam/inference/BayesNetOrdered-inl.h +++ /dev/null @@ -1,192 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesNet-inl.h - * @brief Bayes net template definitions - * @author Frank Dellaert - */ - -#pragma once - -#include - -#include -#include -#include - -#include // for += -using boost::assign::operator+=; - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void BayesNetOrdered::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s; - BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) - conditional->print("Conditional", formatter); - } - - /* ************************************************************************* */ - template - bool BayesNetOrdered::equals(const BayesNetOrdered& cbn, double tol) const { - if (size() != cbn.size()) - return false; - return std::equal(conditionals_.begin(), conditionals_.end(), - cbn.conditionals_.begin(), equals_star(tol)); - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::printStats(const std::string& s) const { - - const size_t n = conditionals_.size(); - size_t max_size = 0; - size_t total = 0; - BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) { - max_size = std::max(max_size, conditional->size()); - total += conditional->size(); - } - std::cout << s - << "maximum conditional size = " << max_size << std::endl - << "average conditional size = " << total / n << std::endl - << "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl; - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::saveGraph(const std::string &s, - const IndexFormatter& indexFormatter) const { - std::ofstream of(s.c_str()); - of << "digraph G{\n"; - - BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Index me = frontals.front(); - typename CONDITIONAL::Parents parents = conditional->parents(); - BOOST_FOREACH(Index p, parents) - of << p << "->" << me << std::endl; - } - - of << "}"; - of.close(); - } - - /* ************************************************************************* */ - template - typename BayesNetOrdered::const_iterator BayesNetOrdered::find( - Index key) const { - for (const_iterator it = begin(); it != end(); ++it) - if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) - != (*it)->endFrontals()) - return it; - return end(); - } - - /* ************************************************************************* */ - template - typename BayesNetOrdered::iterator BayesNetOrdered::find( - Index key) { - for (iterator it = begin(); it != end(); ++it) - if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key) - != (*it)->endFrontals()) - return it; - return end(); - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(sharedConditional conditional, conditionals_) { - conditional->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::push_back(const BayesNetOrdered& bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_back(conditional); - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::push_front(const BayesNetOrdered& bn) { - BOOST_FOREACH(sharedConditional conditional,bn.conditionals_) - push_front(conditional); - } - - /* ************************************************************************* */ - template - void BayesNetOrdered::popLeaf(iterator conditional) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) { - BOOST_FOREACH(Index key, (*conditional)->frontals()) { - if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents()) - throw std::invalid_argument( - "Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf."); - } - } -#endif - conditionals_.erase(conditional); - } - - /* ************************************************************************* */ - template - FastList BayesNetOrdered::ordering() const { - FastList ord; - BOOST_FOREACH(sharedConditional conditional,conditionals_) - ord.insert(ord.begin(), conditional->beginFrontals(), - conditional->endFrontals()); - return ord; - } - -// /* ************************************************************************* */ -// template -// void BayesNet::saveGraph(const std::string &s) const { -// ofstream of(s.c_str()); -// of<< "digraph G{\n"; -// BOOST_FOREACH(const_sharedConditional conditional,conditionals_) { -// Index child = conditional->key(); -// BOOST_FOREACH(Index parent, conditional->parents()) { -// of << parent << "->" << child << endl; -// } -// } -// of<<"}"; -// of.close(); -// } -// - /* ************************************************************************* */ - - template - typename BayesNetOrdered::sharedConditional BayesNetOrdered::operator[]( - Index key) const { - BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) { - typename CONDITIONAL::const_iterator it = std::find( - conditional->beginFrontals(), conditional->endFrontals(), key); - if (it != conditional->endFrontals()) { - return conditional; - } - } - throw(std::invalid_argument( - (boost::format("BayesNet::operator['%1%']: not found") % key).str())); - } - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/inference/BayesNetOrdered.h b/gtsam/inference/BayesNetOrdered.h deleted file mode 100644 index 073e2e88b..000000000 --- a/gtsam/inference/BayesNetOrdered.h +++ /dev/null @@ -1,244 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesNet.h - * @brief Bayes network - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace gtsam { - -/** - * A BayesNet is a list of conditionals, stored in elimination order, i.e. - * leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are - * defined as typedefs of this class, using GaussianConditional and - * IndexConditional as the CONDITIONAL template argument. - * - * todo: Symbolic using Index is a misnomer. - * todo: how to handle Bayes nets with an optimize function? Currently using global functions. - * \nosubgrouping - */ -template -class BayesNetOrdered { - -public: - - typedef typename boost::shared_ptr > shared_ptr; - - /** We store shared pointers to Conditional densities */ - typedef typename CONDITIONAL::KeyType KeyType; - typedef typename boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr const_sharedConditional; - typedef typename std::list Conditionals; - - typedef typename Conditionals::iterator iterator; - typedef typename Conditionals::reverse_iterator reverse_iterator; - typedef typename Conditionals::const_iterator const_iterator; - typedef typename Conditionals::const_reverse_iterator const_reverse_iterator; - -protected: - - /** - * Conditional densities are stored in reverse topological sort order (i.e., leaves first, - * parents last), which corresponds to the elimination ordering if so obtained, - * and is consistent with the column (block) ordering of an upper triangular matrix. - */ - Conditionals conditionals_; - -public: - - /// @name Standard Constructors - /// @{ - - /** Default constructor as an empty BayesNet */ - BayesNetOrdered() {}; - - /** convert from a derived type */ - template - BayesNetOrdered(const BayesNetOrdered& bn) { - conditionals_.assign(bn.begin(), bn.end()); - } - - /** BayesNet with 1 conditional */ - explicit BayesNetOrdered(const sharedConditional& conditional) { push_back(conditional); } - - /// @} - /// @name Testable - /// @{ - - /** print */ - void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** check equality */ - bool equals(const BayesNetOrdered& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** size is the number of nodes */ - size_t size() const { - return conditionals_.size(); - } - - /** @return true if there are no conditionals/nodes */ - bool empty() const { - return conditionals_.empty(); - } - - /** print statistics */ - void printStats(const std::string& s = "") const; - - /** save dot graph */ - void saveGraph(const std::string &s, const IndexFormatter& indexFormatter = - DefaultIndexFormatter) const; - - /** return keys in reverse topological sort order, i.e., elimination order */ - FastList ordering() const; - - /** SLOW O(n) random access to Conditional by key */ - sharedConditional operator[](Index key) const; - - /** return last node in ordering */ - boost::shared_ptr front() const { return conditionals_.front(); } - - /** return last node in ordering */ - boost::shared_ptr back() const { return conditionals_.back(); } - - /** return iterators. FD: breaks encapsulation? */ - const_iterator begin() const {return conditionals_.begin();} ///& bn); - - /// push_front an entire Bayes net - void push_front(const BayesNetOrdered& bn); - - /** += syntax for push_back, e.g. bayesNet += c1, c2, c3 - * @param conditional The conditional to add to the back of the BayesNet - */ - boost::assign::list_inserter >, sharedConditional> - operator+=(const sharedConditional& conditional) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back >(*this))(conditional); } - - /** - * pop_front: remove node at the bottom, used in marginalization - * For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C) - */ - void pop_front() {conditionals_.pop_front();} - - /** Permute the variables in the BayesNet */ - void permuteWithInverse(const Permutation& inversePermutation); - - iterator begin() {return conditionals_.begin();} /// - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditionals_); - } - - /// @} -}; // BayesNet - -} // namespace gtsam - -#include diff --git a/gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h b/gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h deleted file mode 100644 index 1d2ccb57b..000000000 --- a/gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h +++ /dev/null @@ -1,301 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTreeCliqueBase-inl.h - * @brief Base class for cliques of a BayesTree - * @author Richard Roberts and Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void BayesTreeCliqueBaseOrdered::assertInvariants() const { - } - - /* ************************************************************************* */ - template - std::vector BayesTreeCliqueBaseOrdered::separator_setminus_B( - derived_ptr B) const { - sharedConditional p_F_S = this->conditional(); - std::vector &indicesB = B->conditional()->keys(); - std::vector S_setminus_B; - std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), // - indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); - return S_setminus_B; - } - - /* ************************************************************************* */ - template - std::vector BayesTreeCliqueBaseOrdered::shortcut_indices( - derived_ptr B, const FactorGraphOrdered& p_Cp_B) const - { - gttic(shortcut_indices); - std::set allKeys = p_Cp_B.keys(); - std::vector &indicesB = B->conditional()->keys(); - std::vector S_setminus_B = separator_setminus_B(B); // TODO, get as argument? - std::vector keep; - // keep = S\B intersect allKeys - std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // - allKeys.begin(), allKeys.end(), back_inserter(keep)); - // keep += B intersect allKeys - std::set_intersection(indicesB.begin(), indicesB.end(), // - allKeys.begin(), allKeys.end(), back_inserter(keep)); - // BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl; - return keep; - } - - /* ************************************************************************* */ - template - BayesTreeCliqueBaseOrdered::BayesTreeCliqueBaseOrdered( - const sharedConditional& conditional) : - conditional_(conditional) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - BayesTreeCliqueBaseOrdered::BayesTreeCliqueBaseOrdered( - const std::pair >& result) : - conditional_(result.first) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBaseOrdered::print(const std::string& s, - const IndexFormatter& indexFormatter) const { - conditional_->print(s, indexFormatter); - } - - /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseOrdered::treeSize() const { - size_t size = 1; - BOOST_FOREACH(const derived_ptr& child, children_) - size += child->treeSize(); - return size; - } - - /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseOrdered::numCachedSeparatorMarginals() const { - if (!cachedSeparatorMarginal_) - return 0; - - size_t subtree_count = 1; - BOOST_FOREACH(const derived_ptr& child, children_) - subtree_count += child->numCachedSeparatorMarginals(); - - return subtree_count; - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBaseOrdered::printTree( - const std::string& indent, const IndexFormatter& indexFormatter) const { - asDerived(this)->print(indent, indexFormatter); - BOOST_FOREACH(const derived_ptr& child, children_) - child->printTree(indent + " ", indexFormatter); - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBaseOrdered::permuteWithInverse( - const Permutation& inversePermutation) { - conditional_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const derived_ptr& child, children_) { - child->permuteWithInverse(inversePermutation); - } - assertInvariants(); - } - - /* ************************************************************************* */ - template - bool BayesTreeCliqueBaseOrdered::reduceSeparatorWithInverse( - const internal::Reduction& inverseReduction) - { - bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - if(!changed) { - BOOST_FOREACH(const derived_ptr& child, children_) { - assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } - } -#endif - if(changed) { - BOOST_FOREACH(const derived_ptr& child, children_) { - (void) child->reduceSeparatorWithInverse(inverseReduction); } - } - assertInvariants(); - return changed; - } - - /* ************************************************************************* */ - // The shortcut density is a conditional P(S|R) of the separator of this - // clique on the root. We can compute it recursively from the parent shortcut - // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p - /* ************************************************************************* */ - template - BayesNetOrdered BayesTreeCliqueBaseOrdered::shortcut( - derived_ptr B, Eliminate function) const - { - gttic(BayesTreeCliqueBase_shortcut); - - // We only calculate the shortcut when this clique is not B - // and when the S\B is not empty - std::vector S_setminus_B = separator_setminus_B(B); - if (B.get() != this && !S_setminus_B.empty()) { - - // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBase_shortcut); - FactorGraphOrdered p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) - gttic(BayesTreeCliqueBase_shortcut); - p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - - // Determine the variables we want to keepSet, S union B - std::vector keep = shortcut_indices(B, p_Cp_B); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { - if(factor) factor->reduceWithInverse(inverseReduction); } - inverseReduction.applyInverse(keep); - gttoc(Reduce); - - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp_B); - - // Finally, we only want to have S\B variables in the Bayes net, so - size_t nrFrontals = S_setminus_B.size(); - BayesNetOrdered result = *solver.conditionalBayesNet(keep, nrFrontals, function); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { - if (factor) factor->permuteWithInverse(reduction); } - result.permuteWithInverse(reduction); - gttoc(Undo_Reduce); - - assertInvariants(); - - return result; - } else { - return BayesNetOrdered(); - } - } - - /* ************************************************************************* */ - // separator marginal, uses separator marginal of parent recursively - // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template - FactorGraphOrdered::FactorType> BayesTreeCliqueBaseOrdered< - DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBase_separatorMarginal); - // Check if the Separator marginal was already calculated - if (!cachedSeparatorMarginal_) { - gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); - // If this is the root, there is no separator - if (R.get() == this) { - // we are root, return empty - FactorGraphOrdered empty; - cachedSeparatorMarginal_ = empty; - } else { - // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) - // initialize P(Cp) with the parent separator marginal - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline - gttoc(BayesTreeCliqueBase_separatorMarginal); - FactorGraphOrdered p_Cp(parent->separatorMarginal(R, function)); // P(Sp) - gttic(BayesTreeCliqueBase_separatorMarginal); - gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); - // now add the parent conditional - p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { - if(factor) factor->reduceWithInverse(inverseReduction); } - - // The variables we want to keepSet are exactly the ones in S - sharedConditional p_F_S = this->conditional(); - std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); - inverseReduction.applyInverse(indicesS); - gttoc(Reduce); - - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp); - - cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { - if (factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { - if (factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - } - } else { - gttic(BayesTreeCliqueBase_separatorMarginal_cachehit); - } - - // return the shortcut P(S||B) - return *cachedSeparatorMarginal_; // return the cached version - } - - /* ************************************************************************* */ - // marginal2, uses separator marginal of parent recursively - // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template - FactorGraphOrdered::FactorType> BayesTreeCliqueBaseOrdered< - DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBase_marginal2); - // initialize with separator marginal P(S) - FactorGraphOrdered p_C(this->separatorMarginal(R, function)); - // add the conditional P(F|S) - p_C.push_back(this->conditional()->toFactor()); - return p_C; - } - - /* ************************************************************************* */ - template - void BayesTreeCliqueBaseOrdered::deleteCachedShortcuts() { - - // When a shortcut is requested, all of the shortcuts between it and the - // root are also generated. So, if this clique's cached shortcut is set, - // recursively call over all child cliques. Otherwise, it is unnecessary. - if (cachedSeparatorMarginal_) { - BOOST_FOREACH(derived_ptr& child, children_) { - child->deleteCachedShortcuts(); - } - - //Delete CachedShortcut for this clique - cachedSeparatorMarginal_ = boost::none; - } - - } - -} diff --git a/gtsam/inference/BayesTreeCliqueBaseOrdered.h b/gtsam/inference/BayesTreeCliqueBaseOrdered.h deleted file mode 100644 index e663c92af..000000000 --- a/gtsam/inference/BayesTreeCliqueBaseOrdered.h +++ /dev/null @@ -1,273 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTreeCliqueBase.h - * @brief Base class for cliques of a BayesTree - * @author Richard Roberts and Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include - -namespace gtsam { - template class BayesTreeOrdered; -} - -namespace gtsam { - - /** - * This is the base class for BayesTree cliques. The default and standard - * derived type is BayesTreeClique, but some algorithms, like iSAM2, use a - * different clique type in order to store extra data along with the clique. - * - * This class is templated on the derived class (i.e. the curiously recursive - * template pattern). The advantage of this over using virtual classes is - * that it avoids the need for casting to get the derived type. This is - * possible because all cliques in a BayesTree are the same type - if they - * were not then we'd need a virtual class. - * - * @tparam DERIVED The derived clique type. - * @tparam CONDITIONAL The conditional type. - * \nosubgrouping - */ - template - struct BayesTreeCliqueBaseOrdered { - - public: - typedef BayesTreeCliqueBaseOrdered This; - typedef DERIVED DerivedType; - typedef CONDITIONAL ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef boost::shared_ptr derived_ptr; - typedef boost::weak_ptr derived_weak_ptr; - typedef typename ConditionalType::FactorType FactorType; - typedef typename FactorGraphOrdered::Eliminate Eliminate; - - protected: - - /// @name Standard Constructors - /// @{ - - /** Default constructor */ - BayesTreeCliqueBaseOrdered() {} - - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBaseOrdered(const sharedConditional& conditional); - - /** Construct from an elimination result, which is a pair */ - BayesTreeCliqueBaseOrdered( - const std::pair >& result); - - /// @} - - /// This stores the Cached separator margnal P(S) - mutable boost::optional > cachedSeparatorMarginal_; - - public: - sharedConditional conditional_; - derived_weak_ptr parent_; - FastList children_; - - /// @name Testable - /// @{ - - /** check equality */ - bool equals(const This& other, double tol = 1e-9) const { - return (!conditional_ && !other.conditional()) - || conditional_->equals(*other.conditional(), tol); - } - - /** print this node */ - void print(const std::string& s = "", const IndexFormatter& indexFormatter = - DefaultIndexFormatter) const; - - /** print this node and entire subtree below it */ - void printTree(const std::string& indent = "", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** Access the conditional */ - const sharedConditional& conditional() const { - return conditional_; - } - - /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { - return parent_.expired(); - } - - /** The size of subtree rooted at this clique, i.e., nr of Cliques */ - size_t treeSize() const; - - /** Collect number of cliques with cached separator marginals */ - size_t numCachedSeparatorMarginals() const; - - /** The arrow operator accesses the conditional */ - const ConditionalType* operator->() const { - return conditional_.get(); - } - - /** return the const reference of children */ - const std::list& children() const { - return children_; - } - - /** return a shared_ptr to the parent clique */ - derived_ptr parent() const { - return parent_.lock(); - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** The arrow operator accesses the conditional */ - ConditionalType* operator->() { - return conditional_.get(); - } - - /** return the reference of children non-const version*/ - FastList& children() { - return children_; - } - - /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ - static derived_ptr Create(const sharedConditional& conditional) { - return boost::make_shared(conditional); - } - - /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class - * the conditional part is kept and the factor part is ignored, but in derived clique - * types, such as ISAM2Clique, the factor part is kept as a cached factor. - * @param result An elimination result, which is a pair - */ - static derived_ptr Create(const std::pair >& result) { - return boost::make_shared(result); - } - - /** Returns a new clique containing a copy of the conditional but without - * the parent and child clique pointers. - */ - derived_ptr clone() const { - return Create(sharedConditional(new ConditionalType(*conditional_))); - } - - /** Permute the variables in the whole subtree rooted at this clique */ - void permuteWithInverse(const Permutation& inversePermutation); - - /** Permute variables when they only appear in the separators. In this - * case the running intersection property will be used to prevent always - * traversing the whole tree. Returns whether any separator variables in - * this subtree were reordered. - */ - bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); - - /** return the conditional P(S|Root) on the separator given the root */ - BayesNetOrdered shortcut(derived_ptr root, Eliminate function) const; - - /** return the marginal P(S) on the separator */ - FactorGraphOrdered separatorMarginal(derived_ptr root, Eliminate function) const; - - /** return the marginal P(C) of the clique, using marginal caching */ - FactorGraphOrdered marginal2(derived_ptr root, Eliminate function) const; - - /** - * This deletes the cached shortcuts of all cliques (subtree) below this clique. - * This is performed when the bayes tree is modified. - */ - void deleteCachedShortcuts(); - - const boost::optional >& cachedSeparatorMarginal() const { - return cachedSeparatorMarginal_; } - - friend class BayesTreeOrdered ; - - protected: - - /// assert invariants that have to hold in a clique - void assertInvariants() const; - - /// Calculate set \f$ S \setminus B \f$ for shortcut calculations - std::vector separator_setminus_B(derived_ptr B) const; - - /// Calculate set \f$ S_p \cap B \f$ for shortcut calculations - std::vector parent_separator_intersection_B(derived_ptr B) const; - - /** - * Determine variable indices to keep in recursive separator shortcut calculation - * The factor graph p_Cp_B has keys from the parent clique Cp and from B. - * But we only keep the variables not in S union B. - */ - std::vector shortcut_indices(derived_ptr B, - const FactorGraphOrdered& p_Cp_B) const; - - /** Non-recursive delete cached shortcuts and marginals - internal only. */ - void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } - - private: - - /** - * Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - BayesTreeCliqueBaseOrdered(const This& other) { - assert(false); - } - - /** Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - This& operator=(const This& other) { - assert(false); - return *this; - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); - ar & BOOST_SERIALIZATION_NVP(children_); - } - - /// @} - - }; - // \struct Clique - - template - const DERIVED* asDerived( - const BayesTreeCliqueBaseOrdered* base) { - return static_cast(base); - } - - template - DERIVED* asDerived(BayesTreeCliqueBaseOrdered* base) { - return static_cast(base); - } - -} diff --git a/gtsam/inference/BayesTreeOrdered-inl.h b/gtsam/inference/BayesTreeOrdered-inl.h deleted file mode 100644 index 4a73b5d17..000000000 --- a/gtsam/inference/BayesTreeOrdered-inl.h +++ /dev/null @@ -1,804 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTree-inl.h - * @brief Bayes Tree is a tree of cliques of a Bayes Chain - * @author Frank Dellaert - * @author Michael Kaess - * @author Viorela Ila - * @author Richard Roberts - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include // for operator += -using boost::assign::operator+=; -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::CliqueData - BayesTreeOrdered::getCliqueData() const { - CliqueData data; - getCliqueData(data, root_); - return data; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::getCliqueData(CliqueData& data, sharedClique clique) const { - data.conditionalSizes.push_back((*clique)->nrFrontals()); - data.separatorSizes.push_back((*clique)->nrParents()); - BOOST_FOREACH(sharedClique c, clique->children_) { - getCliqueData(data, c); - } - } - - /* ************************************************************************* */ - template - size_t BayesTreeOrdered::numCachedSeparatorMarginals() const { - return (root_) ? root_->numCachedSeparatorMarginals() : 0; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { - if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); - std::ofstream of(s.c_str()); - of<< "digraph G{\n"; - saveGraph(of, root_, indexFormatter); - of<<"}"; - of.close(); - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { - static int num = 0; - bool first = true; - std::stringstream out; - out << num; - std::string parent = out.str(); - parent += "[label=\""; - - BOOST_FOREACH(Index index, clique->conditional_->frontals()) { - if(!first) parent += ","; first = false; - parent += indexFormatter(index); - } - - if( clique != root_){ - parent += " : "; - s << parentnum << "->" << num << "\n"; - } - - first = true; - BOOST_FOREACH(Index sep, clique->conditional_->parents()) { - if(!first) parent += ","; first = false; - parent += indexFormatter(sep); - } - parent += "\"];\n"; - s << parent; - parentnum = num; - - BOOST_FOREACH(sharedClique c, clique->children_) { - num++; - saveGraph(s, c, indexFormatter, parentnum); - } - } - - - /* ************************************************************************* */ - template - void BayesTreeOrdered::CliqueStats::print(const std::string& s) const { - std::cout << s - <<"avg Conditional Size: " << avgConditionalSize << std::endl - << "max Conditional Size: " << maxConditionalSize << std::endl - << "avg Separator Size: " << avgSeparatorSize << std::endl - << "max Separator Size: " << maxSeparatorSize << std::endl; - } - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::CliqueStats - BayesTreeOrdered::CliqueData::getStats() const { - CliqueStats stats; - - double sum = 0.0; - size_t max = 0; - BOOST_FOREACH(size_t s, conditionalSizes) { - sum += (double)s; - if(s > max) max = s; - } - stats.avgConditionalSize = sum / (double)conditionalSizes.size(); - stats.maxConditionalSize = max; - - sum = 0.0; - max = 1; - BOOST_FOREACH(size_t s, separatorSizes) { - sum += (double)s; - if(s > max) max = s; - } - stats.avgSeparatorSize = sum / (double)separatorSizes.size(); - stats.maxSeparatorSize = max; - - return stats; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { - std::cout << s << ":\n"; - BOOST_FOREACH(sharedClique clique, *this) - clique->printTree("", indexFormatter); - } - - /* ************************************************************************* */ - template - bool BayesTreeOrdered::Cliques::equals(const Cliques& other, double tol) const { - return other == *this; - } - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::sharedClique - BayesTreeOrdered::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { - sharedClique new_clique(new Clique(conditional)); - addClique(new_clique, parent_clique); - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::addClique(const sharedClique& clique, const sharedClique& parent_clique) { - nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index j, (*clique)->frontals()) - nodes_[j] = clique; - if (parent_clique != NULL) { - clique->parent_ = parent_clique; - parent_clique->children_.push_back(clique); - } else { - assert(!root_); - root_ = clique; - } - clique->assertInvariants(); - } - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::sharedClique BayesTreeOrdered::addClique( - const sharedConditional& conditional, std::list& child_cliques) { - sharedClique new_clique(new Clique(conditional)); - nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index j, conditional->frontals()) - nodes_[j] = new_clique; - new_clique->children_ = child_cliques; - BOOST_FOREACH(sharedClique& child, child_cliques) - child->parent_ = new_clique; - new_clique->assertInvariants(); - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::permuteWithInverse(const Permutation& inversePermutation) { - // recursively permute the cliques and internal conditionals - if (root_) - root_->permuteWithInverse(inversePermutation); - - // need to know what the largest key is to get the right number of cliques - Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end()); - - // Update the nodes structure - typename BayesTreeOrdered::Nodes newNodes(maxIndex+1); -// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation - for(size_t i = 0; i < nodes_.size(); ++i) - newNodes[inversePermutation[i]] = nodes_[i]; - - nodes_ = newNodes; - } - - /* ************************************************************************* */ - template - inline void BayesTreeOrdered::addToCliqueFront(BayesTreeOrdered& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { - static const bool debug = false; -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Debug check to make sure the conditional variable is ordered lower than - // its parents and that all of its parents are present either in this - // clique or its separator. - BOOST_FOREACH(Index parent, conditional->parents()) { - assert(parent > conditional->lastFrontalKey()); - const Clique& cliquer(*clique); - assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); - } -#endif - if(debug) conditional->print("Adding conditional "); - if(debug) clique->print("To clique "); - Index j = conditional->lastFrontalKey(); - bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size())); - bayesTree.nodes_[j] = clique; - FastVector newIndices((*clique)->size() + 1); - newIndices[0] = j; - std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1); - clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1); - if(debug) clique->print("Expanded clique is "); - clique->assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::removeClique(sharedClique clique) { - - if (clique->isRoot()) - root_.reset(); - else { // detach clique from parent - sharedClique parent = clique->parent_.lock(); - typename FastList::iterator child = std::find(parent->children().begin(), parent->children().end(), clique); - assert(child != parent->children().end()); - parent->children().erase(child); - } - - // orphan my children - BOOST_FOREACH(sharedClique child, clique->children_) - child->parent_ = typename Clique::weak_ptr(); - - BOOST_FOREACH(Index j, clique->conditional()->frontals()) { - nodes_[j].reset(); - } - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTreeOrdered::sharedClique& parent) { - - // Helper function to build a non-symbolic tree (e.g. Gaussian) using a - // symbolic tree, used in the BT(BN) constructor. - - // Build the current clique - FastList cliqueConditionals; - BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { - cliqueConditionals.push_back(conditionals[j]); } - typename BayesTreeOrdered::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); - - // Add the new clique with the current parent - this->addClique(thisClique, parent); - - // Build the children, whose parent is the new clique - BOOST_FOREACH(const BayesTreeOrdered::sharedClique& child, symbolic->children()) { - this->recursiveTreeBuild(child, conditionals, thisClique); } - } - - /* ************************************************************************* */ - template - BayesTreeOrdered::BayesTreeOrdered(const BayesNetOrdered& bayesNet) { - // First generate symbolic BT to determine clique structure - BayesTreeOrdered sbt(bayesNet); - - // Build index of variables to conditionals - std::vector > conditionals(sbt.root()->conditional()->frontals().back() + 1); - BOOST_FOREACH(const boost::shared_ptr& c, bayesNet) { - if(c->nrFrontals() != 1) - throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals"); - if(c->firstFrontalKey() >= conditionals.size()) - throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!"); - if(conditionals[c->firstFrontalKey()]) - throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!"); - - conditionals[c->firstFrontalKey()] = c; - } - - // Build the new tree - this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); - } - - /* ************************************************************************* */ - template<> - inline BayesTreeOrdered::BayesTreeOrdered(const BayesNetOrdered& bayesNet) { - BayesNetOrdered::const_reverse_iterator rit; - for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - insert(*this, *rit); - } - - /* ************************************************************************* */ - template - BayesTreeOrdered::BayesTreeOrdered(const BayesNetOrdered& bayesNet, std::list > subtrees) { - if (bayesNet.size() == 0) - throw std::invalid_argument("BayesTree::insert: empty bayes net!"); - - // get the roots of child subtrees and merge their nodes_ - std::list childRoots; - typedef BayesTreeOrdered Tree; - BOOST_FOREACH(const Tree& subtree, subtrees) { - nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); - childRoots.push_back(subtree.root()); - } - - // create a new clique and add all the conditionals to the clique - sharedClique new_clique; - typename BayesNetOrdered::sharedConditional conditional; - BOOST_REVERSE_FOREACH(conditional, bayesNet) { - if (!new_clique.get()) - new_clique = addClique(conditional,childRoots); - else - addToCliqueFront(*this, conditional, new_clique); - } - - root_ = new_clique; - } - - /* ************************************************************************* */ - template - BayesTreeOrdered::BayesTreeOrdered(const This& other) { - *this = other; - } - - /* ************************************************************************* */ - template - BayesTreeOrdered& BayesTreeOrdered::operator=(const This& other) { - this->clear(); - other.cloneTo(*this); - return *this; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::print(const std::string& s, const IndexFormatter& indexFormatter) const { - if (root_.use_count() == 0) { - std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl; - return; - } - std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl; - if (nodes_.empty()) return; - root_->printTree("", indexFormatter); - } - - /* ************************************************************************* */ - // binary predicate to test equality of a pair for use in equals - template - bool check_sharedCliques( - const typename BayesTreeOrdered::sharedClique& v1, - const typename BayesTreeOrdered::sharedClique& v2 - ) { - return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); - } - - /* ************************************************************************* */ - template - bool BayesTreeOrdered::equals(const BayesTreeOrdered& other, - double tol) const { - return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); - } - - /* ************************************************************************* */ - template - template - inline Index BayesTreeOrdered::findParentClique(const CONTAINER& parents) const { - typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); - assert(lowestOrderedParent != parents.end()); - return *lowestOrderedParent; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::insert(BayesTreeOrdered& bayesTree, const sharedConditional& conditional) - { - static const bool debug = false; - - // get indices and parents - const typename CONDITIONAL::Parents& parents = conditional->parents(); - - if(debug) conditional->print("Adding conditional "); - - // if no parents, start a new root clique - if (parents.empty()) { - if(debug) std::cout << "No parents so making root" << std::endl; - bayesTree.root_ = bayesTree.addClique(conditional); - return; - } - - // otherwise, find the parent clique by using the index data structure - // to find the lowest-ordered parent - Index parentRepresentative = bayesTree.findParentClique(parents); - if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; - sharedClique parent_clique = bayesTree[parentRepresentative]; - if(debug) parent_clique->print("Parent clique is "); - - // if the parents and parent clique have the same size, add to parent clique - if ((*parent_clique)->size() == size_t(parents.size())) { - if(debug) std::cout << "Adding to parent clique" << std::endl; - addToCliqueFront(bayesTree, conditional, parent_clique); - } else { - if(debug) std::cout << "Starting new clique" << std::endl; - // otherwise, start a new clique and add it to the tree - bayesTree.addClique(conditional,parent_clique); - } - } - - /* ************************************************************************* */ - //TODO: remove this function after removing TSAM.cpp - template - typename BayesTreeOrdered::sharedClique BayesTreeOrdered::insert( - const sharedConditional& clique, std::list& children, bool isRootClique) { - - if (clique->nrFrontals() == 0) - throw std::invalid_argument("BayesTree::insert: empty clique!"); - - // create a new clique and add all the conditionals to the clique - sharedClique new_clique = addClique(clique, children); - if (isRootClique) root_ = new_clique; - - return new_clique; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::fillNodesIndex(const sharedClique& subtree) { - // Add each frontal variable of this root node - BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } - // Fill index for each child - typedef typename BayesTreeOrdered::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, subtree->children_) { - fillNodesIndex(child); } - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::insert(const sharedClique& subtree) { - if(subtree) { - // Find the parent clique of the new subtree. By the running intersection - // property, those separator variables in the subtree that are ordered - // lower than the highest frontal variable of the subtree root will all - // appear in the separator of the subtree root. - if(subtree->conditional()->parents().empty()) { - assert(!root_); - root_ = subtree; - } else { - Index parentRepresentative = findParentClique(subtree->conditional()->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += subtree; - subtree->parent_ = parent; // set new parent! - } - - // Now fill in the nodes index - if(nodes_.size() == 0 || - *std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) { - nodes_.resize(subtree->conditional()->lastFrontalKey() + 1); - } - fillNodesIndex(subtree); - } - } - - /* ************************************************************************* */ - // First finds clique marginal then marginalizes that - /* ************************************************************************* */ - template - typename CONDITIONAL::FactorType::shared_ptr BayesTreeOrdered::marginalFactor( - Index j, Eliminate function) const - { - gttic(BayesTree_marginalFactor); - - // get clique containing Index j - sharedClique clique = (*this)[j]; - - // calculate or retrieve its marginal P(C) = P(F,S) -#ifdef OLD_SHORTCUT_MARGINALS - FactorGraphOrdered cliqueMarginal = clique->marginal(root_,function); -#else - FactorGraphOrdered cliqueMarginal = clique->marginal2(root_,function); -#endif - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); - - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(cliqueMarginal); - boost::shared_ptr result = solver.marginalFactor(inverseReduction[j], function); - - // Undo the reduction - gttic(Undo_Reduce); - result->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; - } - - /* ************************************************************************* */ - template - typename BayesNetOrdered::shared_ptr BayesTreeOrdered::marginalBayesNet( - Index j, Eliminate function) const - { - gttic(BayesTree_marginalBayesNet); - - // calculate marginal as a factor graph - FactorGraphOrdered fg; - fg.push_back(this->marginalFactor(j,function)); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(fg.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->reduceWithInverse(inverseReduction); } - gttoc(Reduce); - - // eliminate factor graph marginal to a Bayes net - boost::shared_ptr > bn = GenericSequentialSolver(fg).eliminate(function); - - // Undo the reduction - gttic(Undo_Reduce); - bn->permuteWithInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, fg) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return bn; - } - - /* ************************************************************************* */ - // Find two cliques, their joint, then marginalizes - /* ************************************************************************* */ - template - typename FactorGraphOrdered::shared_ptr - BayesTreeOrdered::joint(Index j1, Index j2, Eliminate function) const { - gttic(BayesTree_joint); - - // get clique C1 and C2 - sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; - - gttic(Lowest_common_ancestor); - // Find lowest common ancestor clique - sharedClique B; { - // Build two paths to the root - FastList path1, path2; { - sharedClique p = C1; - while(p) { - path1.push_front(p); - p = p->parent(); - } - } { - sharedClique p = C2; - while(p) { - path2.push_front(p); - p = p->parent(); - } - } - // Find the path intersection - B = this->root(); - typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); - while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { - B = *p1; - ++p1; - ++p2; - } - } - gttoc(Lowest_common_ancestor); - - // Compute marginal on lowest common ancestor clique - gttic(LCA_marginal); - FactorGraphOrdered p_B = B->marginal2(this->root(), function); - gttoc(LCA_marginal); - - // Compute shortcuts of the requested cliques given the lowest common ancestor - gttic(Clique_shortcuts); - BayesNetOrdered p_C1_Bred = C1->shortcut(B, function); - BayesNetOrdered p_C2_Bred = C2->shortcut(B, function); - gttoc(Clique_shortcuts); - - // Factor the shortcuts to be conditioned on the full root - // Get the set of variables to eliminate, which is C1\B. - gttic(Full_root_factoring); - sharedConditional p_C1_B; { - std::vector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C1_minus_B_set.erase(j); } - C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); - } - // Factor into C1\B | B. - FactorGraphOrdered temp_remaining; - boost::tie(p_C1_B, temp_remaining) = FactorGraphOrdered(p_C1_Bred).eliminate(C1_minus_B, function); - } - sharedConditional p_C2_B; { - std::vector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { - C2_minus_B_set.erase(j); } - C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); - } - // Factor into C2\B | B. - FactorGraphOrdered temp_remaining; - boost::tie(p_C2_B, temp_remaining) = FactorGraphOrdered(p_C2_Bred).eliminate(C2_minus_B, function); - } - gttoc(Full_root_factoring); - - gttic(Variable_joint); - // Build joint on all involved variables - FactorGraphOrdered p_BC1C2; - p_BC1C2.push_back(p_B); - p_BC1C2.push_back(p_C1_B->toFactor()); - p_BC1C2.push_back(p_C2_B->toFactor()); - if(C1 != B) - p_BC1C2.push_back(C1->conditional()->toFactor()); - if(C2 != B) - p_BC1C2.push_back(C2->conditional()->toFactor()); - - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->reduceWithInverse(inverseReduction); } - std::vector js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); - gttoc(Reduce); - - // now, marginalize out everything that is not variable j - GenericSequentialSolver solver(p_BC1C2); - boost::shared_ptr > result = solver.jointFactorGraph(js, function); - - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const boost::shared_ptr& factor, *result) { - if(factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { - if(factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - return result; - - } - - /* ************************************************************************* */ - template - typename BayesNetOrdered::shared_ptr BayesTreeOrdered::jointBayesNet( - Index j1, Index j2, Eliminate function) const { - - // eliminate factor graph marginal to a Bayes net - return GenericSequentialSolver ( - *this->joint(j1, j2, function)).eliminate(function); - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::clear() { - // Remove all nodes and clear the root pointer - nodes_.clear(); - root_.reset(); - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::removePath(sharedClique clique, - BayesNetOrdered& bn, typename BayesTreeOrdered::Cliques& orphans) { - - // base case is NULL, if so we do nothing and return empties above - if (clique!=NULL) { - - // remove the clique from orphans in case it has been added earlier - orphans.remove(clique); - - // remove me - this->removeClique(clique); - - // remove path above me - this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); - - // add children to list of orphans (splice also removed them from clique->children_) - orphans.splice(orphans.begin(), clique->children_); - - bn.push_back(clique->conditional()); - - } - } - - /* ************************************************************************* */ - template - template - void BayesTreeOrdered::removeTop(const CONTAINER& keys, - BayesNetOrdered& bn, typename BayesTreeOrdered::Cliques& orphans) { - - // process each key of the new factor - BOOST_FOREACH(const Index& j, keys) { - - // get the clique - if(j < nodes_.size()) { - const sharedClique& clique(nodes_[j]); - if(clique) { - // remove path from clique to root - this->removePath(clique, bn, orphans); - } - } - } - - // Delete cachedShortcuts for each orphan subtree - //TODO: Consider Improving - BOOST_FOREACH(sharedClique& orphan, orphans) - orphan->deleteCachedShortcuts(); - } - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::Cliques BayesTreeOrdered::removeSubtree( - const sharedClique& subtree) - { - // Result clique list - Cliques cliques; - cliques.push_back(subtree); - - // Remove the first clique from its parents - if(!subtree->isRoot()) - subtree->parent()->children().remove(subtree); - else - root_.reset(); - - // Add all subtree cliques and erase the children and parent of each - for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique) - { - // Add children - BOOST_FOREACH(const sharedClique& child, (*clique)->children()) { - cliques.push_back(child); } - - // Delete cached shortcuts - (*clique)->deleteCachedShortcutsNonRecursive(); - - // Remove this node from the nodes index - BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) { - nodes_[j].reset(); } - - // Erase the parent and children pointers - (*clique)->parent_.reset(); - (*clique)->children_.clear(); - } - - return cliques; - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::cloneTo(This& newTree) const { - if(root()) - cloneTo(newTree, root(), sharedClique()); - } - - /* ************************************************************************* */ - template - void BayesTreeOrdered::cloneTo( - This& newTree, const sharedClique& subtree, const sharedClique& parent) const { - sharedClique newClique(subtree->clone()); - newTree.addClique(newClique, parent); - BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { - cloneTo(newTree, childClique, newClique); - } - } - - /* ************************************************************************* */ - -} // \namespace gtsam diff --git a/gtsam/inference/BayesTreeOrdered.h b/gtsam/inference/BayesTreeOrdered.h deleted file mode 100644 index 5d09368ad..000000000 --- a/gtsam/inference/BayesTreeOrdered.h +++ /dev/null @@ -1,418 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 BayesTree.h - * @brief Bayes Tree is a tree of cliques of a Bayes Chain - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - // Forward declaration of BayesTreeClique which is defined below BayesTree in this file - template struct BayesTreeClique; - - /** - * Bayes tree - * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, - * which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional. - * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this - * as it is only used when developing special versions of BayesTree, e.g. for ISAM2. - * - * \addtogroup Multifrontal - * \nosubgrouping - */ - template > - class BayesTreeOrdered { - - public: - - typedef BayesTreeOrdered This; - typedef boost::shared_ptr > shared_ptr; - typedef boost::shared_ptr sharedConditional; - typedef boost::shared_ptr > sharedBayesNet; - typedef CONDITIONAL ConditionalType; - typedef typename CONDITIONAL::FactorType FactorType; - typedef typename FactorGraphOrdered::Eliminate Eliminate; - - typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique - - // typedef for shared pointers to cliques - typedef boost::shared_ptr sharedClique; - - // A convenience class for a list of shared cliques - struct Cliques : public FastList { - void print(const std::string& s = "Cliques", - const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; - bool equals(const Cliques& other, double tol = 1e-9) const; - }; - - /** clique statistics */ - struct CliqueStats { - double avgConditionalSize; - std::size_t maxConditionalSize; - double avgSeparatorSize; - std::size_t maxSeparatorSize; - void print(const std::string& s = "") const ; - }; - - /** store all the sizes */ - struct CliqueData { - std::vector conditionalSizes; - std::vector separatorSizes; - CliqueStats getStats() const; - }; - - /** Map from indices to Clique */ - typedef std::vector Nodes; - - protected: - - /** Map from indices to Clique */ - Nodes nodes_; - - /** Root clique */ - sharedClique root_; - - public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty Bayes Tree */ - BayesTreeOrdered() {} - - /** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */ - explicit BayesTreeOrdered(const BayesNetOrdered& bayesNet); - - /** Copy constructor */ - BayesTreeOrdered(const This& other); - - /** Assignment operator */ - This& operator=(const This& other); - - /// @} - /// @name Advanced Constructors - /// @{ - - /** - * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the - * new root clique and the subtrees are connected to the root clique. - */ - BayesTreeOrdered(const BayesNetOrdered& bayesNet, std::list > subtrees); - - /** Destructor */ - virtual ~BayesTreeOrdered() {} - - /// @} - /// @name Testable - /// @{ - - /** check equality */ - bool equals(const BayesTreeOrdered& other, double tol = 1e-9) const; - - /** print */ - void print(const std::string& s = "", - const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Find parent clique of a conditional. It will look at all parents and - * return the one with the lowest index in the ordering. - */ - template - Index findParentClique(const CONTAINER& parents) const; - - /** number of cliques */ - inline size_t size() const { - if(root_) - return root_->treeSize(); - else - return 0; - } - - /** number of nodes */ - inline size_t nrNodes() const { return nodes_.size(); } - - /** Check if there are any cliques in the tree */ - inline bool empty() const { - return nodes_.empty(); - } - - /** return nodes */ - const Nodes& nodes() const { return nodes_; } - - /** return root clique */ - const sharedClique& root() const { return root_; } - - /** find the clique that contains the variable with Index j */ - inline sharedClique operator[](Index j) const { - return nodes_.at(j); - } - - /** alternate syntax for matlab: find the clique that contains the variable with Index j */ - inline sharedClique clique(Index j) const { - return nodes_.at(j); - } - - /** Gather data on all cliques */ - CliqueData getCliqueData() const; - - /** Collect number of cliques with cached separator marginals */ - size_t numCachedSeparatorMarginals() const; - - /** return marginal on any variable */ - typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const; - - /** - * return marginal on any variable, as a Bayes Net - * NOTE: this function calls marginal, and then eliminates it into a Bayes Net - * This is more expensive than the above function - */ - typename BayesNetOrdered::shared_ptr marginalBayesNet(Index j, Eliminate function) const; - - /** - * return joint on two variables - * Limitation: can only calculate joint if cliques are disjoint or one of them is root - */ - typename FactorGraphOrdered::shared_ptr joint(Index j1, Index j2, Eliminate function) const; - - /** - * return joint on two variables as a BayesNet - * Limitation: can only calculate joint if cliques are disjoint or one of them is root - */ - typename BayesNetOrdered::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const; - - /** - * Read only with side effects - */ - - /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** Access the root clique (non-const version) */ - sharedClique& root() { return root_; } - - /** Access the nodes (non-cost version) */ - Nodes& nodes() { return nodes_; } - - /** Remove all nodes */ - void clear(); - - /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ - void deleteCachedShortcuts() { - root_->deleteCachedShortcuts(); - } - - /** Apply a permutation to the full tree - also updates the nodes structure */ - void permuteWithInverse(const Permutation& inversePermutation); - - /** - * Remove path from clique to root and return that path as factors - * plus a list of orphaned subtree roots. Used in removeTop below. - */ - void removePath(sharedClique clique, BayesNetOrdered& bn, Cliques& orphans); - - /** - * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. - * Factors and orphans are added to the in/out arguments. - */ - template - void removeTop(const CONTAINER& indices, BayesNetOrdered& bn, Cliques& orphans); - - /** - * Remove the requested subtree. */ - Cliques removeSubtree(const sharedClique& subtree); - - /** - * Hang a new subtree off of the existing tree. This finds the appropriate - * parent clique for the subtree (which may be the root), and updates the - * nodes index with the new cliques in the subtree. None of the frontal - * variables in the subtree may appear in the separators of the existing - * BayesTree. - */ - void insert(const sharedClique& subtree); - - /** Insert a new conditional - * This function only applies for Symbolic case with IndexCondtional, - * We make it static so that it won't be compiled in GaussianConditional case. - * */ - static void insert(BayesTreeOrdered& bayesTree, const sharedConditional& conditional); - - /** - * Insert a new clique corresponding to the given Bayes net. - * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, - * i.e. all the variables (frontal and separator) are connected - */ - sharedClique insert(const sharedConditional& clique, - std::list& children, bool isRootClique = false); - - /** - * Create a clone of this object as a shared pointer - * Necessary for inheritance in matlab interface - */ - virtual shared_ptr clone() const { - return shared_ptr(new This(*this)); - } - - protected: - - /** private helper method for saving the Tree to a text file in GraphViz format */ - void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, - int parentnum = 0) const; - - /** Gather data on a single clique */ - void getCliqueData(CliqueData& stats, sharedClique clique) const; - - /** remove a clique: warning, can result in a forest */ - void removeClique(sharedClique clique); - - /** add a clique (top down) */ - sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique()); - - /** add a clique (top down) */ - void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); - - /** add a clique (bottom up) */ - sharedClique addClique(const sharedConditional& conditional, std::list& child_cliques); - - /** - * Add a conditional to the front of a clique, i.e. a conditional whose - * parents are already in the clique or its separators. This function does - * not check for this condition, it just updates the data structures. - */ - static void addToCliqueFront(BayesTreeOrdered& bayesTree, - const sharedConditional& conditional, const sharedClique& clique); - - /** Fill the nodes index for a subtree */ - void fillNodesIndex(const sharedClique& subtree); - - /** Helper function to build a non-symbolic tree (e.g. Gaussian) using a - * symbolic tree, used in the BT(BN) constructor. - */ - void recursiveTreeBuild(const boost::shared_ptr >& symbolic, - const std::vector >& conditionals, - const typename BayesTreeOrdered::sharedClique& parent); - - private: - - /** deep copy to another tree */ - void cloneTo(This& newTree) const; - - /** deep copy to another tree */ - void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(nodes_); - ar & BOOST_SERIALIZATION_NVP(root_); - } - - /// @} - - }; // BayesTree - - - /* ************************************************************************* */ - template - void _BayesTree_dim_adder( - std::vector& dims, - const typename BayesTreeOrdered::sharedClique& clique) { - - if(clique) { - // Add dims from this clique - for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it) - dims[*it] = (*clique)->dim(it); - - // Traverse children - typedef typename BayesTreeOrdered::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) { - _BayesTree_dim_adder(dims, child); - } - } - } - - /* ************************************************************************* */ - template - boost::shared_ptr allocateVectorValues(const BayesTreeOrdered& bt) { - std::vector dimensions(bt.nodes().size(), 0); - _BayesTree_dim_adder(dimensions, bt.root()); - return boost::shared_ptr(new VectorValuesOrdered(dimensions)); - } - - - /* ************************************************************************* */ - /** - * A Clique in the tree is an incomplete Bayes net: the variables - * in the Bayes net are the frontal nodes, and the variables conditioned - * on are the separator. We also have pointers up and down the tree. - * - * Since our Conditional class already handles multiple frontal variables, - * this Clique contains exactly 1 conditional. - * - * This is the default clique type in a BayesTree, but some algorithms, like - * iSAM2 (see ISAM2Clique), use a different clique type in order to store - * extra data along with the clique. - */ - template - struct BayesTreeClique : public BayesTreeCliqueBaseOrdered, CONDITIONAL> { - public: - typedef CONDITIONAL ConditionalType; - typedef BayesTreeClique This; - typedef BayesTreeCliqueBaseOrdered Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - BayesTreeClique() {} - BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} - BayesTreeClique(const std::pair& result) : Base(result) {} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - }; - -} /// namespace gtsam - -#include -#include diff --git a/gtsam/inference/ClusterTreeOrdered-inl.h b/gtsam/inference/ClusterTreeOrdered-inl.h deleted file mode 100644 index 193a6ba4e..000000000 --- a/gtsam/inference/ClusterTreeOrdered-inl.h +++ /dev/null @@ -1,112 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ClusterTree-inl.h - * @date July 13, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* * - * Cluster - * ************************************************************************* */ - template - template - ClusterTreeOrdered::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) : - FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - template - ClusterTreeOrdered::Cluster::Cluster( - const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : - FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - template - ClusterTreeOrdered::Cluster::Cluster( - FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : - frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} - - /* ************************************************************************* */ - template - void ClusterTreeOrdered::Cluster::addChild(typename ClusterTreeOrdered::Cluster::shared_ptr child) { - children_.push_back(child); - } - - /* ************************************************************************* */ - template - bool ClusterTreeOrdered::Cluster::equals(const Cluster& other) const { - if (frontal != other.frontal) return false; - if (separator != other.separator) return false; - if (children_.size() != other.children_.size()) return false; - - typename std::list::const_iterator it1 = children_.begin(); - typename std::list::const_iterator it2 = other.children_.begin(); - for (; it1 != children_.end(); it1++, it2++) - if (!(*it1)->equals(**it2)) return false; - - return true; - } - - /* ************************************************************************* */ - template - void ClusterTreeOrdered::Cluster::print(const std::string& indent, - const IndexFormatter& formatter) const { - std::cout << indent; - BOOST_FOREACH(const Index key, frontal) - std::cout << formatter(key) << " "; - std::cout << ": "; - BOOST_FOREACH(const Index key, separator) - std::cout << key << " "; - std::cout << std::endl; - } - - /* ************************************************************************* */ - template - void ClusterTreeOrdered::Cluster::printTree(const std::string& indent, - const IndexFormatter& formatter) const { - print(indent, formatter); - BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent + " ", formatter); - } - - /* ************************************************************************* * - * ClusterTree - * ************************************************************************* */ - template - void ClusterTreeOrdered::print(const std::string& str, - const IndexFormatter& formatter) const { - std::cout << str << std::endl; - if (root_) root_->printTree("", formatter); - } - - /* ************************************************************************* */ - template - bool ClusterTreeOrdered::equals(const ClusterTreeOrdered& other, double tol) const { - if (!root_ && !other.root_) return true; - if (!root_ || !other.root_) return false; - return root_->equals(*other.root_); - } - -} //namespace gtsam diff --git a/gtsam/inference/ClusterTreeOrdered.h b/gtsam/inference/ClusterTreeOrdered.h deleted file mode 100644 index 6e575e1be..000000000 --- a/gtsam/inference/ClusterTreeOrdered.h +++ /dev/null @@ -1,141 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ClusterTree.h - * @date July 13, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include - -#include - -namespace gtsam { - - /** - * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: - * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that - * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. - * \nosubgrouping - */ - template - class ClusterTreeOrdered { - public: - // Access to factor types - typedef typename FG::KeyType KeyType; - - protected: - - // the class for subgraphs that also include the pointers to the parents and two children - class Cluster : public FG { - public: - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; - - const std::vector frontal; // the frontal variables - const std::vector separator; // the separator variables - - protected: - - weak_ptr parent_; // the parent cluster - std::list children_; // the child clusters - const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent - - public: - - /// Construct empty clique - Cluster() {} - - /** Create a node with a single frontal variable */ - template - Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator); - - /** Create a node with several frontal variables */ - template - Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); - - /** Create a node with several frontal variables */ - template - Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); - - /// print - void print(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// print the enire tree - void printTree(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// check equality - bool equals(const Cluster& other) const; - - /// get a reference to the children - const std::list& children() const { return children_; } - - /// add a child - void addChild(shared_ptr child); - - /// get or set the parent - weak_ptr& parent() { return parent_; } - - }; - - /// @name Advanced Interface - /// @{ - - /// typedef for shared pointers to clusters - typedef typename Cluster::shared_ptr sharedCluster; - - /// Root cluster - sharedCluster root_; - - public: - - /// @} - /// @name Standard Constructors - /// @{ - - /// constructor of empty tree - ClusterTreeOrdered() {} - - /// @} - /// @name Standard Interface - /// @{ - - /// return the root cluster - sharedCluster root() const { return root_; } - - /// @} - /// @name Testable - /// @{ - - /// print the object - void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** check equality */ - bool equals(const ClusterTreeOrdered& other, double tol = 1e-9) const; - - /// @} - - }; // ClusterTree - -} // namespace gtsam - -#include diff --git a/gtsam/inference/ConditionalOrdered.h b/gtsam/inference/ConditionalOrdered.h deleted file mode 100644 index 69c0db79b..000000000 --- a/gtsam/inference/ConditionalOrdered.h +++ /dev/null @@ -1,266 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Conditional.h - * @brief Base class for conditional densities - * @author Frank Dellaert - */ - -// \callgraph -#pragma once - -#include - -#include - -#include -#include - -namespace gtsam { - - /** - * Base class for conditional densities, templated on KEY type. This class - * provides storage for the keys involved in a conditional, and iterators and - * access to the frontal and separator keys. - * - * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer - * to the associated factor type and shared_ptr type of the derived class. See - * IndexConditional and GaussianConditional for examples. - * \nosubgrouping - */ - template - class ConditionalOrdered: public gtsam::FactorOrdered { - - private: - - /** Create keys by adding key in front */ - template - static std::vector MakeKeys(KEY key, ITERATOR firstParent, - ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); - std::copy(firstParent, lastParent, keys.begin() + 1); - keys[0] = key; - return keys; - } - - protected: - - /** The first nFrontal variables are frontal and the rest are parents. */ - size_t nrFrontals_; - - // Calls the base class assertInvariants, which checks for unique keys - void assertInvariants() const { - FactorOrdered::assertInvariants(); - } - - public: - - typedef KEY KeyType; - typedef ConditionalOrdered This; - typedef FactorOrdered Base; - - /** - * Typedef to the factor type that produces this conditional and that this - * conditional can be converted to using a factor constructor. Derived - * classes must redefine this. - */ - typedef gtsam::FactorOrdered FactorType; - - /** A shared_ptr to this class. Derived classes must redefine this. */ - typedef boost::shared_ptr shared_ptr; - - /** Iterator over keys */ - typedef typename FactorType::iterator iterator; - - /** Const iterator over keys */ - typedef typename FactorType::const_iterator const_iterator; - - /** View of the frontal keys (call frontals()) */ - typedef boost::iterator_range Frontals; - - /** View of the separator keys (call parents()) */ - typedef boost::iterator_range Parents; - - /// @name Standard Constructors - /// @{ - - /** Empty Constructor to make serialization possible */ - ConditionalOrdered() : - nrFrontals_(0) { - assertInvariants(); - } - - /** No parents */ - ConditionalOrdered(KeyType key) : - FactorType(key), nrFrontals_(1) { - assertInvariants(); - } - - /** Single parent */ - ConditionalOrdered(KeyType key, KeyType parent) : - FactorType(key, parent), nrFrontals_(1) { - assertInvariants(); - } - - /** Two parents */ - ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2) : - FactorType(key, parent1, parent2), nrFrontals_(1) { - assertInvariants(); - } - - /** Three parents */ - ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : - FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { - assertInvariants(); - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Constructor from a frontal variable and a vector of parents */ - ConditionalOrdered(KeyType key, const std::vector& parents) : - FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_( - 1) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - ConditionalOrdered(const std::vector& keys, size_t nrFrontals) : - FactorType(keys), nrFrontals_(nrFrontals) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - ConditionalOrdered(const std::list& keys, size_t nrFrontals) : - FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) { - assertInvariants(); - } - - /// @} - /// @name Testable - /// @{ - - /** print with optional formatter */ - void print(const std::string& s = "Conditional", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** check equality */ - template - bool equals(const DERIVED& c, double tol = 1e-9) const { - return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** return the number of frontals */ - size_t nrFrontals() const { - return nrFrontals_; - } - - /** return the number of parents */ - size_t nrParents() const { - return FactorType::size() - nrFrontals_; - } - - /** Special accessor when there is only one frontal variable. */ - KeyType firstFrontalKey() const { - assert(nrFrontals_>0); - return FactorType::front(); - } - KeyType lastFrontalKey() const { - assert(nrFrontals_>0); - return *(endFrontals() - 1); - } - - /** return a view of the frontal keys */ - Frontals frontals() const { - return boost::make_iterator_range(beginFrontals(), endFrontals()); - } - - /** return a view of the parent keys */ - Parents parents() const { - return boost::make_iterator_range(beginParents(), endParents()); - } - - /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { - return FactorType::begin(); - } /// frontals() { - return boost::make_iterator_range(beginFrontals(), endFrontals()); - } - - ///TODO: comment - boost::iterator_range parents() { - return boost::make_iterator_range(beginParents(), endParents()); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(nrFrontals_); - } - - /// @} - - }; - - /* ************************************************************************* */ - template - void ConditionalOrdered::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << " P("; - BOOST_FOREACH(KeyType key, frontals()) - std::cout << " " << formatter(key); - if (nrParents() > 0) - std::cout << " |"; - BOOST_FOREACH(KeyType parent, parents()) - std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; - } - -} // gtsam diff --git a/gtsam/inference/EliminationTreeOrdered-inl.h b/gtsam/inference/EliminationTreeOrdered-inl.h deleted file mode 100644 index cfa01246e..000000000 --- a/gtsam/inference/EliminationTreeOrdered-inl.h +++ /dev/null @@ -1,234 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 EliminationTree-inl.h - * @author Frank Dellaert - * @date Oct 13, 2010 - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -typename EliminationTreeOrdered::sharedFactor EliminationTreeOrdered::eliminate_( - Eliminate function, Conditionals& conditionals) const { - - static const bool debug = false; - - if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; - - if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable - // Create the list of factors to be eliminated, initially empty, and reserve space - FactorGraphOrdered factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); - - // Add all factors associated with the current node - factors.push_back(this->factors_.begin(), this->factors_.end()); - - // for all subtrees, eliminate into Bayes net and a separator factor, added to [factors] - BOOST_FOREACH(const shared_ptr& child, subTrees_) - factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread - // TODO: wait for completion of all threads - - // Combine all factors (from this node and from subtrees) into a joint factor - typename FactorGraphOrdered::EliminationResult - eliminated(function(factors, 1)); - conditionals[this->key_] = eliminated.first; - - if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n"; - if(debug) eliminated.first->print("Conditional: "); - if(debug) eliminated.second->print("Factor: "); - - return eliminated.second; - } else { - // Eliminate each child but discard the result. - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - (void)child->eliminate_(function, conditionals); - } - return sharedFactor(); // Return a NULL factor - } -} - -/* ************************************************************************* */ -template -std::vector EliminationTreeOrdered::ComputeParents(const VariableIndexOrdered& structure) { - - // Number of factors and variables - const size_t m = structure.nFactors(); - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Allocate result parent vector and vector of last factor columns - std::vector parents(n, none); - std::vector prevCol(m, none); - - // 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(const size_t i, structure[j]) { - if (prevCol[i] != none) { - Index k = prevCol[i]; - // find root r of the current tree that contains k - Index r = k; - while (parents[r] != none) - r = parents[r]; - if (r != j) parents[r] = j; - } - prevCol[i] = j; - } - } - - return parents; -} - -/* ************************************************************************* */ -template -template -typename EliminationTreeOrdered::shared_ptr EliminationTreeOrdered::Create( - const FactorGraphOrdered& factorGraph, - const VariableIndexOrdered& structure) { - - static const bool debug = false; - gttic(ET_Create1); - - gttic(ET_ComputeParents); - // Compute the tree structure - std::vector parents(ComputeParents(structure)); - gttoc(ET_ComputeParents); - - // Number of variables - const size_t n = structure.size(); - - static const Index none = std::numeric_limits::max(); - - // Create tree structure - gttic(assemble_tree); - std::vector trees(n); - for (Index k = 1; k <= n; k++) { - Index j = n - k; // Start at the last variable and loop down to 0 - trees[j].reset(new EliminationTreeOrdered(j)); // Create a new node on this variable - if (parents[j] != none) // If this node has a parent, add it to the parent's children - trees[parents[j]]->add(trees[j]); - else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest - throw DisconnectedGraphException(); - } - gttoc(assemble_tree); - - // Hang factors in right places - gttic(hang_factors); - BOOST_FOREACH(const typename boost::shared_ptr& factor, factorGraph) { - if(factor && factor->size() > 0) { - Index j = *std::min_element(factor->begin(), factor->end()); - if(j < structure.size()) - trees[j]->add(factor); - } - } - gttoc(hang_factors); - - if(debug) - trees.back()->print("ETree: "); - - // Check that this is not null - assert(trees.back().get()); - return trees.back(); -} - -/* ************************************************************************* */ -template -template -typename EliminationTreeOrdered::shared_ptr -EliminationTreeOrdered::Create(const FactorGraphOrdered& factorGraph) { - - gttic(ET_Create2); - // Build variable index - const VariableIndexOrdered variableIndex(factorGraph); - - // Build elimination tree - return Create(factorGraph, variableIndex); -} - -/* ************************************************************************* */ -template -void EliminationTreeOrdered::print(const std::string& name, - const IndexFormatter& formatter) const { - std::cout << name << " (" << formatter(key_) << ")" << std::endl; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->print(name + " ", formatter); } - BOOST_FOREACH(const shared_ptr& child, subTrees_) { - child->print(name + " ", formatter); } -} - -/* ************************************************************************* */ -template -bool EliminationTreeOrdered::equals(const EliminationTreeOrdered& expected, double tol) const { - if(this->key_ == expected.key_ && this->factors_ == expected.factors_ - && this->subTrees_.size() == expected.subTrees_.size()) { - typename SubTrees::const_iterator this_subtree = this->subTrees_.begin(); - typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin(); - while(this_subtree != this->subTrees_.end()) - if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol)) - return false; - return true; - } else - return false; -} - -/* ************************************************************************* */ -template -typename EliminationTreeOrdered::BayesNet::shared_ptr - EliminationTreeOrdered::eliminatePartial(typename EliminationTreeOrdered::Eliminate function, size_t nrToEliminate) const { - - // call recursive routine - gttic(ET_recursive_eliminate); - if(nrToEliminate > this->key_ + 1) - throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist"); - Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers - (void)eliminate_(function, conditionals); // modify in place - gttoc(ET_recursive_eliminate); - - // Add conditionals to BayesNet - gttic(assemble_BayesNet); - typename BayesNet::shared_ptr bayesNet(new BayesNet); - BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) { - if(conditional) - bayesNet->push_back(conditional); - } - gttoc(assemble_BayesNet); - - return bayesNet; -} - -/* ************************************************************************* */ -template -typename EliminationTreeOrdered::BayesNet::shared_ptr -EliminationTreeOrdered::eliminate(Eliminate function) const { - size_t nrConditionals = this->key_ + 1; // root key has highest index - return eliminatePartial(function, nrConditionals); -} - -} diff --git a/gtsam/inference/EliminationTreeOrdered.h b/gtsam/inference/EliminationTreeOrdered.h deleted file mode 100644 index c11b2bc16..000000000 --- a/gtsam/inference/EliminationTreeOrdered.h +++ /dev/null @@ -1,187 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 EliminationTree.h - * @author Frank Dellaert - * @date Oct 13, 2010 - */ -#pragma once - -#include - -#include -#include -#include -#include - -class EliminationTreeOrderedTester; // for unit tests, see testEliminationTree -namespace gtsam { template class BayesNetOrdered; } - -namespace gtsam { - -/** - * An elimination tree is a data structure used intermediately during - * elimination. In future versions it will be used to save work between - * multiple eliminations. - * - * When a variable is eliminated, a new factor is created by combining that - * variable's neighboring factors. The new combined factor involves the combined - * factors' involved variables. When the lowest-ordered one of those variables - * is eliminated, it consumes that combined factor. In the elimination tree, - * that lowest-ordered variable is the parent of the variable that was eliminated to - * produce the combined factor. This yields a tree in general, and not a chain - * because of the implicit sparse structure of the resulting Bayes net. - * - * This structure is examined even more closely in a JunctionTree, which - * additionally identifies cliques in the chordal Bayes net. - * \nosubgrouping - */ -template -class EliminationTreeOrdered { - -public: - - typedef EliminationTreeOrdered This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef gtsam::BayesNetOrdered BayesNet; ///< The BayesNet corresponding to FACTOR - typedef FACTOR Factor; - typedef typename FACTOR::KeyType KeyType; - - /** Typedef for an eliminate subroutine */ - typedef typename FactorGraphOrdered::Eliminate Eliminate; - -private: - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - - typedef FastList Factors; - typedef FastList SubTrees; - typedef std::vector Conditionals; - - Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor? - Factors factors_; ///< factors associated with root - SubTrees subTrees_; ///< sub-trees - -public: - - /// @name Standard Constructors - /// @{ - - /** - * Named constructor to build the elimination tree of a factor graph using - * pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ - template - static shared_ptr Create(const FactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure); - - /** Named constructor to build the elimination tree of a factor graph. Note - * that this has to compute the column structure as a VariableIndex, so if you - * already have this precomputed, use the Create(const FactorGraph&, const VariableIndex&) - * named constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ - template - static shared_ptr Create(const FactorGraphOrdered& factorGraph); - - /// @} - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes Net - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesNet resulting from elimination - */ - typename BayesNet::shared_ptr eliminate(Eliminate function) const; - - /** Eliminate the factors to a Bayes Net and return the remaining factor - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesNet resulting from elimination and the remaining factor - */ - typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const; - - /// @} - /// @name Testable - /// @{ - - /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** Test whether the tree is equal to another */ - bool equals(const EliminationTreeOrdered& other, double tol = 1e-9) const; - - /// @} - -private: - - /** default constructor, private, as you should use Create below */ - EliminationTreeOrdered(Index key = 0) : key_(key) {} - - /** - * Static internal function to build a vector of parent pointers using the - * algorithm of Gilbert et al., 2001, BIT. - */ - static std::vector ComputeParents(const VariableIndexOrdered& structure); - - /** add a factor, for Create use only */ - void add(const sharedFactor& factor) { factors_.push_back(factor); } - - /** add a subtree, for Create use only */ - void add(const shared_ptr& child) { subTrees_.push_back(child); } - - /** - * Recursive routine that eliminates the factors arranged in an elimination tree - * @param Conditionals is a vector of shared pointers that will be modified in place - */ - sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const; - - /// Allow access to constructor and add methods for testing purposes - friend class ::EliminationTreeOrderedTester; - -}; - - -/** - * An exception thrown when attempting to eliminate a disconnected factor - * graph, which is not currently possible in gtsam. If you need to work with - * disconnected graphs, a workaround is to create zero-information factors to - * bridge the disconnects. To do this, create any factor type (e.g. - * BetweenFactor or RangeFactor) with the noise model - * sharedPrecision(dim, 0.0), where \c dim is the appropriate - * dimensionality for that factor. - */ -struct DisconnectedGraphException : public std::exception { - DisconnectedGraphException() {} - virtual ~DisconnectedGraphException() throw() {} - - /// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam." - virtual const char* what() const throw() { - return - "Attempting to eliminate a disconnected graph - this is not currently possible in\n" - "GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n" - "will affect the symbolic structure and solving complexity of the graph but not\n" - "the solution. To do this, create BetweenFactor's with zero-precision noise\n" - "models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; } -}; - -} - -#include diff --git a/gtsam/inference/FactorGraphOrdered-inl.h b/gtsam/inference/FactorGraphOrdered-inl.h deleted file mode 100644 index b2e92e5d0..000000000 --- a/gtsam/inference/FactorGraphOrdered-inl.h +++ /dev/null @@ -1,288 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 FactorGraph-inl.h - * This is a template definition file, include it where needed (only!) - * so that the appropriate code is generated and link errors avoided. - * @brief Factor Graph Base Class - * @author Carlos Nieto - * @author Frank Dellaert - * @author Alireza Fathi - * @author Michael Kaess - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - template - FactorGraphOrdered::FactorGraphOrdered(const BayesNetOrdered& bayesNet) { - factors_.reserve(bayesNet.size()); - BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { - this->push_back(cond->toFactor()); - } - } - - /* ************************************************************************* */ - template - void FactorGraphOrdered::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); - } - } - - /* ************************************************************************* */ - template - bool FactorGraphOrdered::equals(const This& fg, double tol) const { - /** check whether the two factor graphs have the same number of factors_ */ - if (factors_.size() != fg.size()) return false; - - /** check whether the factors_ are the same */ - for (size_t i = 0; i < factors_.size(); i++) { - // TODO: Doesn't this force order of factor insertion? - sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; - if (!f1->equals(*f2, tol)) return false; - } - return true; - } - - /* ************************************************************************* */ - template - size_t FactorGraphOrdered::nrFactors() const { - size_t size_ = 0; - BOOST_FOREACH(const sharedFactor& factor, factors_) - if (factor) size_++; - return size_; - } - - /* ************************************************************************* */ - template - std::set FactorGraphOrdered::keys() const { - std::set allKeys; - BOOST_FOREACH(const sharedFactor& factor, factors_) - if (factor) { - BOOST_FOREACH(Index j, factor->keys()) - allKeys.insert(j); - } - return allKeys; - } - - /* ************************************************************************* */ - template - std::pair::sharedConditional, FactorGraphOrdered > - FactorGraphOrdered::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const - { - // Build variable index - VariableIndexOrdered variableIndex(*this); - - // Find first variable - Index firstIndex = 0; - while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty()) - ++ firstIndex; - - // Check that number of variables is in bounds - if(firstIndex + nFrontals > variableIndex.size()) - throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph."); - - // Get set of involved factors - FastSet involvedFactorIs; - for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) { - BOOST_FOREACH(size_t i, variableIndex[j]) { - involvedFactorIs.insert(i); - } - } - - // Separate factors into involved and remaining - FactorGraphOrdered involvedFactors; - FactorGraphOrdered remainingFactors; - FastSet::const_iterator involvedFactorIsIt = involvedFactorIs.begin(); - for(size_t i = 0; i < this->size(); ++i) { - if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) { - // If the current factor is involved, add it to involved and increment involved iterator - involvedFactors.push_back((*this)[i]); - ++ involvedFactorIsIt; - } else { - // If not involved, add to remaining - remainingFactors.push_back((*this)[i]); - } - } - - // Do dense elimination on the involved factors - typename FactorGraphOrdered::EliminationResult eliminationResult = - eliminate(involvedFactors, nFrontals); - - // Add the remaining factor back into the factor graph - remainingFactors.push_back(eliminationResult.second); - - // Return the eliminated factor and remaining factor graph - return std::make_pair(eliminationResult.first, remainingFactors); - } - - /* ************************************************************************* */ - template - std::pair::sharedConditional, FactorGraphOrdered > - FactorGraphOrdered::eliminate(const std::vector& variables, const Eliminate& eliminateFcn, - boost::optional variableIndex_) const - { - const VariableIndexOrdered& variableIndex = - variableIndex_ ? *variableIndex_ : VariableIndexOrdered(*this); - - // First find the involved factors - FactorGraphOrdered involvedFactors; - Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors - - // First get the indices of the involved factors, but uniquely in a set - FastSet involvedFactorIndices; - BOOST_FOREACH(Index variable, variables) { - involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); } - - // Add the factors themselves to involvedFactors and update largest index - involvedFactors.reserve(involvedFactorIndices.size()); - BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) { - const sharedFactor factor = this->at(factorIndex); - involvedFactors.push_back(factor); // Add involved factor - highestInvolvedVariable = std::max( // Updated largest index - highestInvolvedVariable, - *std::max_element(factor->begin(), factor->end())); - } - - sharedConditional conditional; - sharedFactor remainingFactor; - if(involvedFactors.size() > 0) { - // Now permute the variables to be eliminated to the front of the ordering - Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1); - Permutation toFrontInverse = *toFront.inverse(); - BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { - factor->permuteWithInverse(toFrontInverse); } - - // Eliminate into conditional and remaining factor - EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size()); - conditional = eliminated.first; - remainingFactor = eliminated.second; - - // Undo the permutation - BOOST_FOREACH(const sharedFactor& factor, involvedFactors) { - factor->permuteWithInverse(toFront); } - conditional->permuteWithInverse(toFront); - remainingFactor->permuteWithInverse(toFront); - } else { - // Eliminate 0 variables - EliminationResult eliminated = eliminateFcn(involvedFactors, 0); - conditional = eliminated.first; - remainingFactor = eliminated.second; - } - - // Build the remaining graph, without the removed factors - FactorGraphOrdered remainingGraph; - remainingGraph.reserve(this->size() - involvedFactors.size() + 1); - FastSet::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin(); - for(size_t i = 0; i < this->size(); ++i) { - if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i) - ++ involvedFactorIndexIt; - else - remainingGraph.push_back(this->at(i)); - } - - // Add the remaining factor if it is not empty. - if(remainingFactor->size() != 0) - remainingGraph.push_back(remainingFactor); - - return std::make_pair(conditional, remainingGraph); - - } - /* ************************************************************************* */ - template - void FactorGraphOrdered::replace(size_t index, sharedFactor factor) { - if (index >= factors_.size()) throw std::invalid_argument(boost::str( - boost::format("Factor graph does not contain a factor with index %d.") - % index)); - // Replace the factor - factors_[index] = factor; - } - - /* ************************************************************************* */ - template - FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) { - // create new linear factor graph equal to the first one - FACTORGRAPH fg = fg1; - - // add the second factors_ in the graph - fg.push_back(fg2); - - return fg; - } - - /* ************************************************************************* */ - template - typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered& factors, - const FastMap >& variableSlots) { - - typedef const std::pair > KeySlotPair; - // Local functional for getting keys out of key-value pairs - struct Local { static KEY FirstOf(const KeySlotPair& pr) { return pr.first; } }; - - return typename DERIVEDFACTOR::shared_ptr(new DERIVEDFACTOR( - boost::make_transform_iterator(variableSlots.begin(), &Local::FirstOf), - boost::make_transform_iterator(variableSlots.end(), &Local::FirstOf))); - } - - /* ************************************************************************* */ - // Recursive function to add factors in cliques to vector of factors_io - template - void _FactorGraph_BayesTree_adder( - std::vector >& factors_io, - const typename BayesTreeOrdered::sharedClique& clique) { - - if(clique) { - // Add factor from this clique - factors_io.push_back((*clique)->toFactor()); - - // Traverse children - typedef typename BayesTreeOrdered::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) - _FactorGraph_BayesTree_adder(factors_io, child); - } - } - - /* ************************************************************************* */ - template - template - FactorGraphOrdered::FactorGraphOrdered(const BayesTreeOrdered& bayesTree) { - factors_.reserve(bayesTree.size()); - _FactorGraph_BayesTree_adder(factors_, bayesTree.root()); - } - - /* ************************************************************************* */ -} // namespace gtsam diff --git a/gtsam/inference/FactorGraphOrdered.h b/gtsam/inference/FactorGraphOrdered.h deleted file mode 100644 index d75308c62..000000000 --- a/gtsam/inference/FactorGraphOrdered.h +++ /dev/null @@ -1,273 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 FactorGraph.h - * @brief Factor Graph Base Class - * @author Carlos Nieto - * @author Christian Potthast - * @author Michael Kaess - */ - -// \callgraph - -#pragma once - -#include -#include -#include - -#include -#include -#include - -namespace gtsam { - -// Forward declarations -template class BayesTreeOrdered; -class VariableIndexOrdered; - - /** - * A factor graph is a bipartite graph with factor nodes connected to variable nodes. - * In this class, however, only factor nodes are kept around. - * \nosubgrouping - */ - template - class FactorGraphOrdered { - - public: - - typedef FACTOR FactorType; ///< factor type - typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables with - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - - typedef FactorGraphOrdered This; ///< Typedef for this class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class - typedef typename std::vector::iterator iterator; - typedef typename std::vector::const_iterator const_iterator; - - /** typedef for elimination result */ - typedef std::pair EliminationResult; - - /** typedef for an eliminate subroutine */ - typedef boost::function Eliminate; - - protected: - - /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - - /** Collection of factors */ - std::vector factors_; - - public: - - /// @name Standard Constructor - /// @{ - - /** Default constructor */ - FactorGraphOrdered() {} - - /// @} - /// @name Advanced Constructors - /// @{ - - /** - * @brief Constructor from a Bayes net - * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor - * @return a factor graph with all the conditionals, as factors - */ - template - FactorGraphOrdered(const BayesNetOrdered& bayesNet); - - /** convert from Bayes tree */ - template - FactorGraphOrdered(const BayesTreeOrdered& bayesTree); - - /** convert from a derived type */ - template - FactorGraphOrdered(const FactorGraphOrdered& factors) { - factors_.assign(factors.begin(), factors.end()); - } - - /// @} - /// @name Adding Factors - /// @{ - - /** - * Reserve space for the specified number of factors if you know in - * advance how many there will be (works like std::vector::reserve). - */ - void reserve(size_t size) { factors_.reserve(size); } - - /** Add a factor */ - template - void push_back(const boost::shared_ptr& factor) { - factors_.push_back(boost::shared_ptr(factor)); - } - - /** push back many factors */ - void push_back(const This& factors) { - factors_.insert(end(), factors.begin(), factors.end()); - } - - /** push back many factors with an iterator */ - template - void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - factors_.insert(end(), firstFactor, lastFactor); - } - - /** - * @brief Add a vector of derived factors - * @param factors to add - */ - template - void push_back(const std::vector >& factors) { - factors_.insert(end(), factors.begin(), factors.end()); - } - - /// @} - /// @name Testable - /// @{ - - /** print out graph */ - void print(const std::string& s = "FactorGraph", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** Check equality */ - bool equals(const This& fg, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** return the number of factors and NULLS */ - size_t size() const { return factors_.size();} - - /** Simple check for an empty graph - faster than comparing size() to zero */ - bool empty() const { return factors_.empty(); } - - /** const cast to the underlying vector of factors */ - operator const std::vector&() const { return factors_; } - - /** Get a specific factor by index */ - const sharedFactor at(size_t i) const { assert(i > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - */ - std::pair > eliminate( - const std::vector& variables, const Eliminate& eliminateFcn, - boost::optional variableIndex = boost::none) const; - - /** Eliminate a single variable, by calling FactorGraph::eliminate. */ - std::pair > eliminateOne( - KeyType variable, const Eliminate& eliminateFcn, - boost::optional variableIndex = boost::none) const { - std::vector variables(1, variable); - return eliminate(variables, eliminateFcn, variableIndex); - } - - /// @} - /// @name Modifying Factor Graphs (imperative, discouraged) - /// @{ - - /** non-const STL-style begin() */ - iterator begin() { return factors_.begin();} - - /** non-const STL-style end() */ - iterator end() { return factors_.end(); } - - /** resize the factor graph. TODO: effects? */ - void resize(size_t size) { factors_.resize(size); } - - /** delete factor without re-arranging indexes by inserting a NULL pointer */ - inline void remove(size_t i) { factors_[i].reset();} - - /** replace a factor by index */ - void replace(size_t index, sharedFactor factor); - - /// @} - /// @name Advanced Interface - /// @{ - - /** return the number valid factors */ - size_t nrFactors() const; - - /** Potentially very slow function to return all keys involved */ - std::set keys() const; - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(factors_); - } - - /// @} - - }; // FactorGraph - - /** Create a combined joint factor (new style for EliminationTree). */ - template - typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered& factors, - const FastMap >& variableSlots); - - /** - * static function that combines two factor graphs - * @param fg1 Linear factor graph - * @param fg2 Linear factor graph - * @return a new combined factor graph - */ - template - FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2); - -} // namespace gtsam - -#include diff --git a/gtsam/inference/FactorOrdered-inl.h b/gtsam/inference/FactorOrdered-inl.h deleted file mode 100644 index bf340f9c3..000000000 --- a/gtsam/inference/FactorOrdered-inl.h +++ /dev/null @@ -1,108 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Factor-inl.h - * @author Richard Roberts - * @date Sep 1, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - FactorOrdered::FactorOrdered(const FactorOrdered& f) : - keys_(f.keys_) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - FactorOrdered::FactorOrdered(const ConditionalType& c) : - keys_(c.keys()) { -// assert(c.nrFrontals() == 1); - assertInvariants(); - } - - /* ************************************************************************* */ - template - void FactorOrdered::assertInvariants() const { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Check that keys are all unique - std::multiset nonunique(keys_.begin(), keys_.end()); - std::set unique(keys_.begin(), keys_.end()); - bool correct = (nonunique.size() == unique.size()) - && std::equal(nonunique.begin(), nonunique.end(), unique.begin()); - if (!correct) throw std::logic_error( - "Factor::assertInvariants: Factor contains duplicate keys"); -#endif - } - - /* ************************************************************************* */ - template - void FactorOrdered::print(const std::string& s, - const IndexFormatter& formatter) const { - printKeys(s,formatter); - } - - /* ************************************************************************* */ - template - void FactorOrdered::printKeys(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); - std::cout << std::endl; - } - - /* ************************************************************************* */ - template - bool FactorOrdered::equals(const This& other, double tol) const { - return keys_ == other.keys_; - } - - /* ************************************************************************* */ -#ifdef TRACK_ELIMINATE - template - template - typename COND::shared_ptr FactorOrdered::eliminateFirst() { - assert(!keys_.empty()); - assertInvariants(); - KEY eliminated = keys_.front(); - keys_.erase(keys_.begin()); - assertInvariants(); - return typename COND::shared_ptr(new COND(eliminated, keys_)); - } - - /* ************************************************************************* */ - template - template - typename BayesNetOrdered::shared_ptr FactorOrdered::eliminate(size_t nrFrontals) { - assert(keys_.size() >= nrFrontals); - assertInvariants(); - typename BayesNetOrdered::shared_ptr fragment(new BayesNetOrdered ()); - const_iterator it = this->begin(); - for (KEY n = 0; n < nrFrontals; ++n, ++it) - fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1)); - if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(), - fragment->back()->endParents()); - assertInvariants(); - return fragment; - } -#endif - -} diff --git a/gtsam/inference/FactorOrdered.h b/gtsam/inference/FactorOrdered.h deleted file mode 100644 index a4512a1bd..000000000 --- a/gtsam/inference/FactorOrdered.h +++ /dev/null @@ -1,232 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Factor.h - * @brief The base class for all factors - * @author Kai Ni - * @author Frank Dellaert - * @author Richard Roberts - */ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -template class ConditionalOrdered; - -/** - * This is the base class for all factor types. It is templated on a KEY type, - * which will be the type used to label variables. Key types currently in use - * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and - * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), - * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). - * though currently only IndexFactor and IndexConditional derive from this - * class, using Index keys. This class does not store any data other than its - * keys. Derived classes store data such as matrices and probability tables. - * - * Note that derived classes *must* redefine the ConditionalType and shared_ptr - * typedefs to refer to the associated conditional and shared_ptr types of the - * derived class. See IndexFactor, JacobianFactor, etc. for examples. - * - * This class is \b not virtual for performance reasons - derived symbolic classes, - * IndexFactor and IndexConditional, need to be created and destroyed quickly - * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. - * \nosubgrouping - */ -template -class FactorOrdered { - -public: - - typedef KEY KeyType; ///< The KEY template parameter - typedef FactorOrdered This; ///< This class - - /** - * Typedef to the conditional type obtained by eliminating this factor, - * derived classes must redefine this. - */ - typedef ConditionalOrdered ConditionalType; - - /// A shared_ptr to this class, derived classes must redefine this. - typedef boost::shared_ptr shared_ptr; - - /// Iterator over keys - typedef typename std::vector::iterator iterator; - - /// Const iterator over keys - typedef typename std::vector::const_iterator const_iterator; - -protected: - - /// The keys involved in this factor - std::vector keys_; - -public: - - /// @name Standard Constructors - /// @{ - - /** Copy constructor */ - FactorOrdered(const This& f); - - /** Construct from conditional, calls ConditionalType::toFactor() */ - FactorOrdered(const ConditionalType& c); - - /** Default constructor for I/O */ - FactorOrdered() {} - - /** Construct unary factor */ - FactorOrdered(KeyType key) : keys_(1) { - keys_[0] = key; assertInvariants(); } - - /** Construct binary factor */ - FactorOrdered(KeyType key1, KeyType key2) : keys_(2) { - keys_[0] = key1; keys_[1] = key2; assertInvariants(); } - - /** Construct ternary factor */ - FactorOrdered(KeyType key1, KeyType key2, KeyType key3) : keys_(3) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } - - /** Construct 4-way factor */ - FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } - - /** Construct 5-way factor */ - FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); } - - /** Construct 6-way factor */ - FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) { - keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Construct n-way factor */ - FactorOrdered(const std::set& keys) { - BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key); - assertInvariants(); - } - - /** Construct n-way factor */ - FactorOrdered(const std::vector& keys) : keys_(keys) { - assertInvariants(); - } - - /** Constructor from a collection of keys */ - template FactorOrdered(KEYITERATOR beginKey, KEYITERATOR endKey) : - keys_(beginKey, endKey) { assertInvariants(); } - - /// @} - -#ifdef TRACK_ELIMINATE - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - template - typename CONDITIONAL::shared_ptr eliminateFirst(); - - /** - * eliminate the first nrFrontals frontal variables. - */ - template - typename BayesNetOrdered::shared_ptr eliminate(size_t nrFrontals = 1); -#endif - - /// @name Standard Interface - /// @{ - - /// First key - KeyType front() const { return keys_.front(); } - - /// Last key - KeyType back() const { return keys_.back(); } - - /// find - const_iterator find(KeyType key) const { return std::find(begin(), end(), key); } - - /// Access the factor's involved variable keys - const std::vector& keys() const { return keys_; } - - /** iterators */ - const_iterator begin() const { return keys_.begin(); } ///TODO: comment - const_iterator end() const { return keys_.end(); } ///TODO: comment - - /** - * @return the number of variables involved in this factor - */ - size_t size() const { return keys_.size(); } - - /// @} - /// @name Testable - /// @{ - - /// print - void print(const std::string& s = "Factor", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// print only keys - void printKeys(const std::string& s = "Factor", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /// check equality - bool equals(const This& other, double tol = 1e-9) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * @return keys involved in this factor - */ - std::vector& keys() { return keys_; } - - /** mutable iterators */ - iterator begin() { return keys_.begin(); } ///TODO: comment - iterator end() { return keys_.end(); } ///TODO: comment - -protected: - friend class JacobianFactorOrdered; - friend class HessianFactorOrdered; - - /// Internal consistency check that is run frequently when in debug mode. - /// If NDEBUG is defined, this is empty and optimized out. - void assertInvariants() const; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(keys_); - } - - /// @} - -}; - -} - -#include diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h deleted file mode 100644 index 9882a92dd..000000000 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - GenericMultifrontalSolver::GenericMultifrontalSolver( - const FactorGraphOrdered& graph) : - structure_(new VariableIndexOrdered(graph)), junctionTree_( - new JT(graph, *structure_)) { - } - - /* ************************************************************************* */ - template - GenericMultifrontalSolver::GenericMultifrontalSolver( - const sharedGraph& graph, - const VariableIndexOrdered::shared_ptr& variableIndex) : - structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) { - } - - /* ************************************************************************* */ - template - void GenericMultifrontalSolver::print(const std::string& s) const { - this->structure_->print(s + " structure:\n"); - this->junctionTree_->print(s + " jtree:"); - } - - /* ************************************************************************* */ - template - bool GenericMultifrontalSolver::equals( - const GenericMultifrontalSolver& expected, double tol) const { - if (!this->structure_->equals(*expected.structure_, tol)) return false; - if (!this->junctionTree_->equals(*expected.junctionTree_, tol)) return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericMultifrontalSolver::replaceFactors(const sharedGraph& graph) { - junctionTree_.reset(new JT(*graph, *structure_)); - } - - /* ************************************************************************* */ - template - typename BayesTreeOrdered::shared_ptr - GenericMultifrontalSolver::eliminate(Eliminate function) const { - - // eliminate junction tree, returns pointer to root - typename BayesTreeOrdered::sharedClique - root = junctionTree_->eliminate(function); - - // create an empty Bayes tree and insert root clique - typename BayesTreeOrdered::shared_ptr - bayesTree(new BayesTreeOrdered); - bayesTree->insert(root); - - // return the Bayes tree - return bayesTree; - } - - /* ************************************************************************* */ - template - typename FactorGraphOrdered::shared_ptr GenericMultifrontalSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const { - - // FIXME: joint for arbitrary sets of variables not present - // TODO: develop and implement theory for shortcuts of more than two variables - - if (js.size() != 2) throw std::domain_error( - "*MultifrontalSolver::joint(js) currently can only compute joint marginals\n" - "for exactly two variables. You can call marginal to compute the\n" - "marginal for one variable. *SequentialSolver::joint(js) can compute the\n" - "joint marginal over any number of variables, so use that if necessary.\n"); - - return eliminate(function)->joint(js[0], js[1], function); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr GenericMultifrontalSolver::marginalFactor( - Index j, Eliminate function) const { - return eliminate(function)->marginalFactor(j, function); - } - -} - diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h deleted file mode 100644 index 0f379d024..000000000 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ /dev/null @@ -1,124 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include - -#include -#include - -namespace gtsam { - - /** - * A Generic Multifrontal Solver class - * - * A solver is given a factor graph at construction, and implements - * a strategy to solve it (in this case, eliminate into a Bayes tree). - * This generic one will create a Bayes tree when eliminate() is called. - * - * Takes two template arguments: - * FACTOR the factor type, e.g., GaussianFactor, DiscreteFactor - * JUNCTIONTREE annoyingly, you also have to supply a compatible JT type - * i.e., one templated on a factor graph with the same factors - * TODO: figure why this is so and possibly fix it - * \nosubgrouping - */ - template - class GenericMultifrontalSolver { - - protected: - - /// Column structure of the factor graph - VariableIndexOrdered::shared_ptr structure_; - - /// Junction tree that performs elimination. - typename JUNCTIONTREE::shared_ptr junctionTree_; - - public: - - typedef typename FactorGraphOrdered::shared_ptr sharedGraph; - typedef typename FactorGraphOrdered::Eliminate Eliminate; - - /// @name Standard Constructors - /// @{ - - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - GenericMultifrontalSolver(const FactorGraphOrdered& factorGraph); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericMultifrontalSolver(const sharedGraph& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex); - - /// @} - /// @name Testable - /// @{ - - /** Print to cout */ - void print(const std::string& name = "GenericMultifrontalSolver: ") const; - - /** Test whether is equal to another */ - bool equals(const GenericMultifrontalSolver& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedGraph& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - typename BayesTreeOrdered::shared_ptr - eliminate(Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - */ - typename FactorGraphOrdered::shared_ptr jointFactorGraph( - const std::vector& js, Eliminate function) const; - - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename boost::shared_ptr marginalFactor(Index j, - Eliminate function) const; - - /// @} - - }; - -} // gtsam - -#include diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h deleted file mode 100644 index 455dcac80..000000000 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ /dev/null @@ -1,203 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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-inl.h - * @brief Implementation for generic sequential solver - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver(const FactorGraphOrdered& factorGraph) { - gttic(GenericSequentialSolver_constructor1); - assert(factorGraph.size()); - factors_.reset(new FactorGraphOrdered(factorGraph)); - structure_.reset(new VariableIndexOrdered(factorGraph)); - eliminationTree_ = EliminationTreeOrdered::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex) - { - gttic(GenericSequentialSolver_constructor2); - factors_ = factorGraph; - structure_ = variableIndex; - eliminationTree_ = EliminationTreeOrdered::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::print(const std::string& s) const { - this->factors_->print(s + " factors:"); - this->structure_->print(s + " structure:\n"); - this->eliminationTree_->print(s + " etree:"); - } - - /* ************************************************************************* */ - template - bool GenericSequentialSolver::equals( - const GenericSequentialSolver& expected, double tol) const { - if (!this->factors_->equals(*expected.factors_, tol)) - return false; - if (!this->structure_->equals(*expected.structure_, tol)) - return false; - if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) - return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::replaceFactors( - const sharedFactorGraph& factorGraph) { - // Reset this shared pointer first to deallocate if possible - for big - // problems there may not be enough memory to store two copies. - eliminationTree_.reset(); - factors_ = factorGraph; - eliminationTree_ = EliminationTreeOrdered::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::eliminate(Eliminate function) const { - return eliminationTree_->eliminate(function); - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function, boost::optional nrToEliminate) const - { - gttic(GenericSequentialSolver_eliminate); - // Create inverse permutation - Permutation::shared_ptr permutationInverse(permutation.inverse()); - - // Permute the factors - NOTE that this permutes the original factors, not - // copies. Other parts of the code may hold shared_ptr's to these factors so - // we must undo the permutation before returning. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) - factor->permuteWithInverse(*permutationInverse); - - // Eliminate using elimination tree provided - typename EliminationTreeOrdered::shared_ptr etree = EliminationTreeOrdered::Create(*factors_); - sharedBayesNet bayesNet; - if(nrToEliminate) - bayesNet = etree->eliminatePartial(function, *nrToEliminate); - else - bayesNet = etree->eliminate(function); - - // Undo the permutation on the original factors and on the structure. - BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) - factor->permuteWithInverse(permutation); - - // Undo the permutation on the conditionals - BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet) - c->permuteWithInverse(permutation); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::conditionalBayesNet( - const std::vector& js, size_t nrFrontals, - Eliminate function) const - { - gttic(GenericSequentialSolver_conditionalBayesNet); - // Compute a COLAMD permutation with the marginal variables constrained to the end. - // TODO in case of nrFrontals, the order of js has to be respected here ! - Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js, true)); - - // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = structure_->size(); - size_t nrMarginalized = nrVariables - js.size(); - size_t nrToEliminate = nrMarginalized + nrFrontals; - sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); - // Get rid of conditionals on variables that we want to marginalize out - for (size_t i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename GenericSequentialSolver::sharedBayesNet // - GenericSequentialSolver::jointBayesNet(const std::vector& js, - Eliminate function) const - { - gttic(GenericSequentialSolver_jointBayesNet); - // Compute a COLAMD permutation with the marginal variables constrained to the end. - Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js)); - - // Eliminate all variables - sharedBayesNet bayesNet = eliminate(*permutation, function); - - // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalized = bayesNet->size() - js.size(); - for (size_t i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); - - return bayesNet; - } - - /* ************************************************************************* */ - template - typename FactorGraphOrdered::shared_ptr // - GenericSequentialSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const - { - gttic(GenericSequentialSolver_jointFactorGraph); - // Eliminate all variables - typename BayesNetOrdered::shared_ptr bayesNet = jointBayesNet(js, function); - - return boost::make_shared >(*bayesNet); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { - gttic(GenericSequentialSolver_marginalFactor); - // Create a container for the one variable index - std::vector js(1); - js[0] = j; - - // Call joint and return the only factor in the factor graph it returns - // TODO: just call jointBayesNet and grab last conditional, then toFactor.... - return (*this->jointFactorGraph(js, function))[0]; - } - -} // namespace gtsam diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h deleted file mode 100644 index 2e157a4be..000000000 --- a/gtsam/inference/GenericSequentialSolver.h +++ /dev/null @@ -1,179 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 generic sequential elimination - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include - -#include -#include - -#include -#include - -namespace gtsam { - class VariableIndexOrdered; - class Permutation; -} -namespace gtsam { - template class EliminationTreeOrdered; -} -namespace gtsam { - template class FactorGraphOrdered; -} -namespace gtsam { - template class BayesNetOrdered; -} - -namespace gtsam { - - /** - * This solver implements sequential variable elimination for factor graphs. - * Underlying this is a column elimination tree, see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which examines and uses the clique structure. - * However, sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * \nosubgrouping - */ - template - class GenericSequentialSolver { - - protected: - - typedef boost::shared_ptr > sharedFactorGraph; - typedef typename FACTOR::ConditionalType Conditional; - typedef typename boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr > sharedBayesNet; - typedef std::pair, boost::shared_ptr > EliminationResult; - typedef boost::function< - EliminationResult(const FactorGraphOrdered&, size_t)> Eliminate; - - /** Store the original factors for computing marginals - * TODO Frank says: really? Marginals should be computed from result. - */ - sharedFactorGraph factors_; - - /** Store column structure of the factor graph. Why? */ - boost::shared_ptr structure_; - - /** Elimination tree that performs elimination */ - boost::shared_ptr > eliminationTree_; - - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTreeOrdered) - - /** - * Eliminate in a different order, given a permutation - */ - sharedBayesNet eliminate(const Permutation& permutation, Eliminate function, - boost::optional nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many - ) const; - - public: - - /// @name Standard Constructors - /// @{ - - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GenericSequentialSolver(const FactorGraphOrdered& factorGraph); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericSequentialSolver(const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex); - - /// @} - /// @name Testable - /// @{ - - /** Print to cout */ - void print(const std::string& name = "GenericSequentialSolver: ") const; - - /** Test whether is equal to another */ - bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedFactorGraph& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - sharedBayesNet eliminate(Eliminate function) const; - - /** - * Compute a conditional density P(F|S) while marginalizing out variables J - * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) - * Returns the result as a Bayes net. - */ - sharedBayesNet - conditionalBayesNet(const std::vector& js, size_t nrFrontals, - Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a Bayes net - */ - sharedBayesNet - jointBayesNet(const std::vector& js, Eliminate function) const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - typename FactorGraphOrdered::shared_ptr - jointFactorGraph(const std::vector& js, Eliminate function) 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. - */ - typename boost::shared_ptr - marginalFactor(Index j, Eliminate function) const; - - /// @} - - } - ; -// GenericSequentialSolver - -}// namespace gtsam - -#include diff --git a/gtsam/inference/ISAMOrdered-inl.h b/gtsam/inference/ISAMOrdered-inl.h deleted file mode 100644 index 8183538a0..000000000 --- a/gtsam/inference/ISAMOrdered-inl.h +++ /dev/null @@ -1,74 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ISAM-inl.h - * @brief Incremental update functionality (iSAM) for BayesTree. - * @author Michael Kaess - */ - -#pragma once - -#include -#include // for operator += - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - ISAMOrdered::ISAMOrdered() : BayesTreeOrdered() {} - - /* ************************************************************************* */ - template - template void ISAMOrdered::update_internal( - const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) { - - // Remove the contaminated part of the Bayes tree - // Throw exception if disconnected - BayesNetOrdered bn; - if (!this->empty()) { - this->removeTop(newFactors.keys(), bn, orphans); - if (bn.empty()) - throw std::runtime_error( - "ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!"); - } - FG factors(bn); - - // add the factors themselves - factors.push_back(newFactors); - - // eliminate into a Bayes net - GenericMultifrontalSolver > solver(factors); - boost::shared_ptr > bayesTree; - bayesTree = solver.eliminate(function); - this->root_ = bayesTree->root(); - this->nodes_ = bayesTree->nodes(); - - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) - this->insert(orphan); - } - - /* ************************************************************************* */ - template - template - void ISAMOrdered::update(const FG& newFactors, - typename FG::Eliminate function) { - Cliques orphans; - this->update_internal(newFactors, orphans, function); - } - -} -/// namespace gtsam diff --git a/gtsam/inference/ISAMOrdered.h b/gtsam/inference/ISAMOrdered.h deleted file mode 100644 index 9ceb3f83e..000000000 --- a/gtsam/inference/ISAMOrdered.h +++ /dev/null @@ -1,76 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ISAM.h - * @brief Incremental update functionality (iSAM) for BayesTree. - * @author Michael Kaess - */ - -// \callgraph -#pragma once - -#include - -namespace gtsam { - - /** - * A Bayes tree with an update methods that implements the iSAM algorithm. - * Given a set of new factors, it re-eliminates the invalidated part of the tree. - * \nosubgrouping - */ - template - class ISAMOrdered: public BayesTreeOrdered { - - private: - - typedef BayesTreeOrdered Base; - - public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty Bayes Tree */ - ISAMOrdered(); - - /** Copy constructor */ - ISAMOrdered(const Base& bayesTree) : - Base(bayesTree) { - } - - /// @} - /// @name Advanced Interface Interface - /// @{ - - /** - * update the Bayes tree with a set of new factors, typically derived from measurements - * @param newFactors is a factor graph that contains the new factors - * @param function an elimination routine - */ - template - void update(const FG& newFactors, typename FG::Eliminate function); - - typedef typename BayesTreeOrdered::sharedClique sharedClique; ///::Cliques Cliques; /// - void update_internal(const FG& newFactors, Cliques& orphans, - typename FG::Eliminate function); - - /// @} - - }; - -}/// namespace gtsam - -#include diff --git a/gtsam/inference/IndexConditionalOrdered.cpp b/gtsam/inference/IndexConditionalOrdered.cpp deleted file mode 100644 index 5acaf4c9b..000000000 --- a/gtsam/inference/IndexConditionalOrdered.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexConditional.cpp - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - -namespace gtsam { - - using namespace std; - using namespace boost::lambda; - - /* ************************************************************************* */ - void IndexConditionalOrdered::assertInvariants() const { - // Checks for uniqueness of keys - Base::assertInvariants(); - } - - /* ************************************************************************* */ - bool IndexConditionalOrdered::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } -#endif - bool parentChanged = false; - BOOST_FOREACH(KeyType& parent, parents()) { - internal::Reduction::const_iterator it = inverseReduction.find(parent); - if(it != inverseReduction.end()) { - parentChanged = true; - parent = it->second; - } - } - assertInvariants(); - return parentChanged; - } - - /* ************************************************************************* */ - void IndexConditionalOrdered::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys()) - key = inversePermutation[key]; - assertInvariants(); - } - /* ************************************************************************* */ - -} diff --git a/gtsam/inference/IndexConditionalOrdered.h b/gtsam/inference/IndexConditionalOrdered.h deleted file mode 100644 index 42b560da5..000000000 --- a/gtsam/inference/IndexConditionalOrdered.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexConditional.h - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - - /** - * IndexConditional serves two purposes. It is the base class for - * GaussianConditional, and also functions as a symbolic conditional with - * Index keys, produced by symbolic elimination of IndexFactor. - * - * It derives from Conditional with a key type of Index, which is an - * unsigned integer. - * \nosubgrouping - */ - class IndexConditionalOrdered : public ConditionalOrdered { - - protected: - - // Checks that frontal indices are sorted and lower than parent indices - GTSAM_EXPORT void assertInvariants() const; - - public: - - typedef IndexConditionalOrdered This; - typedef ConditionalOrdered Base; - typedef IndexFactorOrdered FactorType; - typedef boost::shared_ptr shared_ptr; - - /// @name Standard Constructors - /// @{ - - /** Empty Constructor to make serialization possible */ - IndexConditionalOrdered() { assertInvariants(); } - - /** No parents */ - IndexConditionalOrdered(Index j) : Base(j) { assertInvariants(); } - - /** Single parent */ - IndexConditionalOrdered(Index j, Index parent) : Base(j, parent) { assertInvariants(); } - - /** Two parents */ - IndexConditionalOrdered(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); } - - /** Three parents */ - IndexConditionalOrdered(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Constructor from a frontal variable and a vector of parents */ - IndexConditionalOrdered(Index j, const std::vector& parents) : Base(j, parents) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - IndexConditionalOrdered(const std::vector& keys, size_t nrFrontals) : - Base(keys, nrFrontals) { - assertInvariants(); - } - - /** Constructor from keys and nr of frontal variables */ - IndexConditionalOrdered(const std::list& keys, size_t nrFrontals) : - Base(keys, nrFrontals) { - assertInvariants(); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** Named constructor directly returning a shared pointer */ - template - static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) { - shared_ptr conditional(new IndexConditionalOrdered()); - conditional->keys_.assign(keys.begin(), keys.end()); - conditional->nrFrontals_ = nrFrontals; - return conditional; - } - - /** Convert to a factor */ - IndexFactorOrdered::shared_ptr toFactor() const { - return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(*this)); - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** Permute the variables when only separator variables need to be permuted. - * Returns true if any reordered variables appeared in the separator and - * false if not. - */ - GTSAM_EXPORT bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); - - /** - * Permutes the Conditional, but for efficiency requires the permutation - * to already be inverted. - */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - - /// @} - - }; - -} diff --git a/gtsam/inference/IndexFactorOrdered.cpp b/gtsam/inference/IndexFactorOrdered.cpp deleted file mode 100644 index 26159dd36..000000000 --- a/gtsam/inference/IndexFactorOrdered.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexFactor.cpp - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#include -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - void IndexFactorOrdered::assertInvariants() const { - Base::assertInvariants(); - } - - /* ************************************************************************* */ - IndexFactorOrdered::IndexFactorOrdered(const IndexConditionalOrdered& c) : - Base(c) { - assertInvariants(); - } - - /* ************************************************************************* */ -#ifdef TRACK_ELIMINATE - boost::shared_ptr IndexFactorOrdered::eliminateFirst() { - boost::shared_ptr result(Base::eliminateFirst< - IndexConditionalOrdered>()); - assertInvariants(); - return result; - } - - /* ************************************************************************* */ - boost::shared_ptr > IndexFactorOrdered::eliminate( - size_t nrFrontals) { - boost::shared_ptr > result(Base::eliminate< - IndexConditionalOrdered>(nrFrontals)); - assertInvariants(); - return result; - } -#endif - - /* ************************************************************************* */ - void IndexFactorOrdered::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys()) - key = inversePermutation[key]; - assertInvariants(); - } - - /* ************************************************************************* */ - void IndexFactorOrdered::reduceWithInverse(const internal::Reduction& inverseReduction) { - BOOST_FOREACH(Index& key, keys()) - key = inverseReduction[key]; - assertInvariants(); - } - - /* ************************************************************************* */ -} // gtsam diff --git a/gtsam/inference/IndexFactorOrdered.h b/gtsam/inference/IndexFactorOrdered.h deleted file mode 100644 index ded98bf0a..000000000 --- a/gtsam/inference/IndexFactorOrdered.h +++ /dev/null @@ -1,179 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 IndexFactor.h - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#pragma once - -#include -#include - -namespace gtsam { - - // Forward declaration of IndexConditional - class IndexConditionalOrdered; - - /** - * IndexFactor serves two purposes. It is the base class for all linear - * factors (GaussianFactor, JacobianFactor, HessianFactor), and also - * functions as a symbolic factor with Index keys, used to do symbolic - * elimination by JunctionTree. - * - * It derives from Factor with a key type of Index, an unsigned integer. - * \nosubgrouping - */ - class IndexFactorOrdered: public FactorOrdered { - - protected: - - /// @name Advanced Interface - /// @{ - - /// Internal function for checking class invariants (unique keys for this factor) - GTSAM_EXPORT void assertInvariants() const; - - /// @} - - public: - - typedef IndexFactorOrdered This; - typedef FactorOrdered Base; - - /** Elimination produces an IndexConditional */ - typedef IndexConditionalOrdered ConditionalType; - - /** Overriding the shared_ptr typedef */ - typedef boost::shared_ptr shared_ptr; - - /// @name Standard Interface - /// @{ - - /** Copy constructor */ - IndexFactorOrdered(const This& f) : - Base(f) { - assertInvariants(); - } - - /** Construct from derived type */ - GTSAM_EXPORT IndexFactorOrdered(const IndexConditionalOrdered& c); - - /** Default constructor for I/O */ - IndexFactorOrdered() { - assertInvariants(); - } - - /** Construct unary factor */ - IndexFactorOrdered(Index j) : - Base(j) { - assertInvariants(); - } - - /** Construct binary factor */ - IndexFactorOrdered(Index j1, Index j2) : - Base(j1, j2) { - assertInvariants(); - } - - /** Construct ternary factor */ - IndexFactorOrdered(Index j1, Index j2, Index j3) : - Base(j1, j2, j3) { - assertInvariants(); - } - - /** Construct 4-way factor */ - IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4) : - Base(j1, j2, j3, j4) { - assertInvariants(); - } - - /** Construct 5-way factor */ - IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5) : - Base(j1, j2, j3, j4, j5) { - assertInvariants(); - } - - /** Construct 6-way factor */ - IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) : - Base(j1, j2, j3, j4, j5, j6) { - assertInvariants(); - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /** Construct n-way factor */ - IndexFactorOrdered(const std::set& js) : - Base(js) { - assertInvariants(); - } - - /** Construct n-way factor */ - IndexFactorOrdered(const std::vector& js) : - Base(js) { - assertInvariants(); - } - - /** Constructor from a collection of keys */ - template IndexFactorOrdered(KeyIterator beginKey, - KeyIterator endKey) : - Base(beginKey, endKey) { - assertInvariants(); - } - - /// @} - -#ifdef TRACK_ELIMINATE - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - GTSAM_EXPORT boost::shared_ptr eliminateFirst(); - - /** eliminate the first nrFrontals frontal variables.*/ - GTSAM_EXPORT boost::shared_ptr > eliminate(size_t nrFrontals = - 1); -#endif - - /// @name Advanced Interface - /// @{ - - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); - - virtual ~IndexFactorOrdered() { - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - - /// @} - - }; // IndexFactor - -} diff --git a/gtsam/inference/JunctionTreeOrdered-inl.h b/gtsam/inference/JunctionTreeOrdered-inl.h deleted file mode 100644 index de648571c..000000000 --- a/gtsam/inference/JunctionTreeOrdered-inl.h +++ /dev/null @@ -1,207 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JunctionTree-inl.h - * @date Feb 4, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief The junction tree, template bodies - */ - -#pragma once - -#include -#include -#include -#include - -#include - -namespace gtsam { - - /* ************************************************************************* */ - template - void JunctionTreeOrdered::construct(const FG& fg, const VariableIndexOrdered& variableIndex) { - gttic(JT_construct); - gttic(JT_symbolic_ET); - const typename EliminationTreeOrdered::shared_ptr symETree = - EliminationTreeOrdered::Create(fg, variableIndex); - assert(symETree.get()); - gttoc(JT_symbolic_ET); - gttic(JT_symbolic_eliminate); - SymbolicBayesNetOrdered::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic); - assert(sbn.get()); - gttoc(JT_symbolic_eliminate); - gttic(symbolic_BayesTree); - SymbolicBayesTree sbt(*sbn); - gttoc(symbolic_BayesTree); - - // distribute factors - gttic(distributeFactors); - this->root_ = distributeFactors(fg, sbt.root()); - gttoc(distributeFactors); - } - - /* ************************************************************************* */ - template - JunctionTreeOrdered::JunctionTreeOrdered(const FG& fg) { - gttic(VariableIndexOrdered); - VariableIndexOrdered varIndex(fg); - gttoc(VariableIndexOrdered); - construct(fg, varIndex); - } - - /* ************************************************************************* */ - template - JunctionTreeOrdered::JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex) { - construct(fg, variableIndex); - } - - /* ************************************************************************* */ - template - typename JunctionTreeOrdered::sharedClique JunctionTreeOrdered::distributeFactors( - const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) { - - // Build "target" index. This is an index for each variable of the factors - // that involve this variable as their *lowest-ordered* variable. For each - // factor, it is the lowest-ordered variable of that factor that pulls the - // factor into elimination, after which all of the information in the - // factor is contained in the eliminated factors that are passed up the - // tree as elimination continues. - - // Two stages - first build an array of the lowest-ordered variable in each - // factor and find the last variable to be eliminated. - std::vector lowestOrdered(fg.size(), std::numeric_limits::max()); - Index maxVar = 0; - for(size_t i=0; ibegin(), fg[i]->end()); - if(min != fg[i]->end()) { - lowestOrdered[i] = *min; - maxVar = std::max(maxVar, *min); - } - } - - // Now add each factor to the list corresponding to its lowest-ordered - // variable. - std::vector > targets(maxVar+1); - for(size_t i=0; i::max()) - targets[lowestOrdered[i]].push_back(i); - - // Now call the recursive distributeFactors - return distributeFactors(fg, targets, bayesClique); - } - - /* ************************************************************************* */ - template - typename JunctionTreeOrdered::sharedClique JunctionTreeOrdered::distributeFactors(const FG& fg, - const std::vector >& targets, - const SymbolicBayesTree::sharedClique& bayesClique) { - - if(bayesClique) { - // create a new clique in the junction tree - sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(), - (*bayesClique)->beginParents(), (*bayesClique)->endParents())); - - // count the factors for this cluster to pre-allocate space - { - size_t nFactors = 0; - BOOST_FOREACH(const Index frontal, clique->frontal) { - // There may be less variables in "targets" than there really are if - // some of the highest-numbered variables do not pull in any factors. - if(frontal < targets.size()) - nFactors += targets[frontal].size(); } - clique->reserve(nFactors); - } - // add the factors to this cluster - BOOST_FOREACH(const Index frontal, clique->frontal) { - if(frontal < targets.size()) { - BOOST_FOREACH(const size_t factorI, targets[frontal]) { - clique->push_back(fg[factorI]); } } } - - // recursively call the children - BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) { - sharedClique child = distributeFactors(fg, targets, bayesChild); - clique->addChild(child); - child->parent() = clique; - } - return clique; - } else - return sharedClique(); - } - - /* ************************************************************************* */ - template - std::pair::BTClique::shared_ptr, - typename FG::sharedFactor> JunctionTreeOrdered::eliminateOneClique( - typename FG::Eliminate function, - const boost::shared_ptr& current) const { - - FG fg; // factor graph will be assembled from local factors and marginalized children - fg.reserve(current->size() + current->children().size()); - fg.push_back(*current); // add the local factors - - // receive the factors from the child and its clique point - std::list children; - BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { - std::pair tree_factor( - eliminateOneClique(function, child)); - children.push_back(tree_factor.first); - fg.push_back(tree_factor.second); - } - - // eliminate the combined factors - // warning: fg is being eliminated in-place and will contain marginal afterwards - - // Now that we know which factors and variables, and where variables - // come from and go to, create and eliminate the new joint factor. - gttic(CombineAndEliminate); - typename FG::EliminationResult eliminated(function(fg, - current->frontal.size())); - gttoc(CombineAndEliminate); - -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin())); -#endif - - gttic(Update_tree); - // create a new clique corresponding the combined factors - typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated)); - new_clique->children_ = children; - - BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) { - childRoot->parent_ = new_clique; - } - gttoc(Update_tree); - - return std::make_pair(new_clique, eliminated.second); - } - - /* ************************************************************************* */ - template - typename BTCLIQUE::shared_ptr JunctionTreeOrdered::eliminate( - typename FG::Eliminate function) const - { - if (this->root()) { - gttic(JT_eliminate); - std::pair ret = - this->eliminateOneClique(function, this->root()); - if (ret.second->size() != 0) throw std::runtime_error( - "JuntionTree::eliminate: elimination failed because of factors left over!"); - gttoc(JT_eliminate); - return ret.first; - } else - return typename BTClique::shared_ptr(); - } - -} //namespace gtsam diff --git a/gtsam/inference/JunctionTreeOrdered.h b/gtsam/inference/JunctionTreeOrdered.h deleted file mode 100644 index ae91ab661..000000000 --- a/gtsam/inference/JunctionTreeOrdered.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JunctionTree.h - * @date Feb 4, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: The junction tree - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with - * the additional property that it represents the clique tree associated with a Bayes net. - * - * In GTSAM a junction tree is an intermediate data structure in multifrontal - * variable elimination. Each node is a cluster of factors, along with a - * clique of variables that are eliminated all at once. In detail, every node k represents - * a clique (maximal fully connected subset) of an associated chordal graph, such as a - * chordal Bayes net resulting from elimination. - * - * The difference with the BayesTree is that a JunctionTree stores factors, whereas a - * BayesTree stores conditionals, that are the product of eliminating the factors in the - * corresponding JunctionTree cliques. - * - * The tree structure and elimination method are exactly analagous to the EliminationTree, - * except that in the JunctionTree, at each node multiple variables are eliminated at a time. - * - * - * \addtogroup Multifrontal - * \nosubgrouping - */ - template::Clique> - class JunctionTreeOrdered: public ClusterTreeOrdered { - - public: - - /// In a junction tree each cluster is associated with a clique - typedef typename ClusterTreeOrdered::Cluster Clique; - typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique - - /// The BayesTree type produced by elimination - typedef BTCLIQUE BTClique; - - /// Shared pointer to this class - typedef boost::shared_ptr > shared_ptr; - - /// We will frequently refer to a symbolic Bayes tree, used to find the clique structure - typedef gtsam::BayesTreeOrdered SymbolicBayesTree; - - private: - - /// @name Advanced Interface - /// @{ - - /// distribute the factors along the cluster tree - sharedClique distributeFactors(const FG& fg, - const SymbolicBayesTree::sharedClique& clique); - - /// distribute the factors along the cluster tree - sharedClique distributeFactors(const FG& fg, const std::vector >& targets, - const SymbolicBayesTree::sharedClique& clique); - - /// recursive elimination function - std::pair - eliminateOneClique(typename FG::Eliminate function, - const boost::shared_ptr& clique) const; - - /// internal constructor - void construct(const FG& fg, const VariableIndexOrdered& variableIndex); - - /// @} - - public: - - /// @name Standard Constructors - /// @{ - - /** Default constructor */ - JunctionTreeOrdered() {} - - /** Named constructor to build the junction tree of a factor graph. Note - * that this has to compute the column structure as a VariableIndex, so if you - * already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&) - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ - JunctionTreeOrdered(const FG& factorGraph); - - /** Construct from a factor graph and pre-computed variable index. - * @param fg The factor graph for which to build the junction tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the JunctionTree(const FG&) - * constructor instead. - */ - JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex); - - /// @} - /// @name Standard Interface - /// @{ - - /** Eliminate the factors in the subgraphs to produce a BayesTree. - * @param function The function used to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The BayesTree resulting from elimination - */ - typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const; - - /// @} - - }; // JunctionTree - -} // namespace gtsam - -#include diff --git a/gtsam/inference/PermutationOrdered.cpp b/gtsam/inference/PermutationOrdered.cpp deleted file mode 100644 index e8e510568..000000000 --- a/gtsam/inference/PermutationOrdered.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Permutation.cpp - * @author Richard Roberts - * @date Oct 9, 2010 - */ - -#include - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -Permutation Permutation::Identity(Index nVars) { - Permutation ret(nVars); - for(Index i=0; i& toFront, size_t size, bool filterDuplicates) { - - Permutation ret(size); - - // Mask of which variables have been pulled, used to reorder - vector pulled(size, false); - - // Put the pulled variables at the front of the permutation and set up the - // pulled flags. - size_t toFrontUniqueSize = 0; - for(Index j=0; j& js) const { - BOOST_FOREACH(Index& j, js) { - j = this->find(j)->second; - } - } - - /* ************************************************************************* */ - Permutation Reduction::inverse() const { - Index maxIndex = 0; - BOOST_FOREACH(const value_type& target_source, *this) { - if(target_source.second > maxIndex) - maxIndex = target_source.second; - } - Permutation result(maxIndex + 1); - BOOST_FOREACH(const value_type& target_source, *this) { - result[target_source.second] = target_source.first; - } - return result; - } - - /* ************************************************************************* */ - const Index& Reduction::operator[](const Index& j) { - iterator it = this->find(j); - if(it == this->end()) - return j; - else - return it->second; - } - - /* ************************************************************************* */ - const Index& Reduction::operator[](const Index& j) const { - const_iterator it = this->find(j); - if(it == this->end()) - return j; - else - return it->second; - } - - /* ************************************************************************* */ - void Reduction::print(const std::string& s) const { - cout << s << " reduction:" << endl; - BOOST_FOREACH(const value_type& p, *this) - cout << " " << p.first << " : " << p.second << endl; - } - - /* ************************************************************************* */ - bool Reduction::equals(const Reduction& other, double tol) const { - return (const Base&)(*this) == (const Base&)other; - } - - /* ************************************************************************* */ - Permutation createReducingPermutation(const std::set& indices) { - Permutation p(indices.size()); - Index newJ = 0; - BOOST_FOREACH(Index j, indices) { - p[newJ] = j; - ++ newJ; - } - return p; - } -} // \namespace internal - -/* ************************************************************************* */ -} // \namespace gtsam diff --git a/gtsam/inference/PermutationOrdered.h b/gtsam/inference/PermutationOrdered.h deleted file mode 100644 index 213055781..000000000 --- a/gtsam/inference/PermutationOrdered.h +++ /dev/null @@ -1,215 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Permutation.h - * @author Richard Roberts - * @date Sep 12, 2010 - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -namespace gtsam { - - /** - * A permutation reorders variables, for example to reduce fill-in during - * elimination. To save computation, the permutation can be applied to - * the necessary data structures only once, then multiple computations - * performed on the permuted problem. For example, in an iterative - * non-linear setting, a permutation can be created from the symbolic graph - * structure and applied to the ordering of nonlinear variables once, so - * that linearized factor graphs are already correctly ordered and need - * not be permuted. - * - * Consistent with their mathematical definition, permutations are functions - * that accept the destination index and return the source index. For example, - * to permute data structure A into a new data structure B using permutation P, - * the mapping is B[i] = A[P[i]]. This is the behavior implemented by - * B = A.permute(P). - * - * For some data structures in GTSAM, for efficiency it is necessary to have - * the inverse of the desired permutation. In this case, the data structure - * will implement permuteWithInverse(P) instead of permute(P). You may already - * have the inverse permutation by construction, or may instead compute it with - * Pinv = P.inverse(). - * - * Permutations can be composed and inverted - * in order to create new indexing for a structure. - * \nosubgrouping - */ -class GTSAM_EXPORT Permutation { -protected: - std::vector rangeIndices_; - -public: - typedef boost::shared_ptr shared_ptr; - typedef std::vector::const_iterator const_iterator; - typedef std::vector::iterator iterator; - - /// @name Standard Constructors - /// @{ - - /** - * Create an empty permutation. This cannot do anything, but you can later - * assign to it. - */ - Permutation() {} - - /** - * Create an uninitialized permutation. You must assign all values using the - * square bracket [] operator or they will be undefined! - */ - Permutation(Index nVars) : rangeIndices_(nVars) {} - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& str = "Permutation: ") const; - - /** Check equality */ - bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; } - - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Return the source index of the supplied destination index. - */ - Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; } - - /** - * Return the source index of the supplied destination index. This version allows modification. - */ - Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; } - - /** - * Return the source index of the supplied destination index. Synonym for operator[](Index). - */ - Index at(Index variable) const { return operator[](variable); } - - /** - * Return the source index of the supplied destination index. This version allows modification. Synonym for operator[](Index). - */ - Index& at(Index variable) { return operator[](variable); } - - /** - * The number of variables in the range of this permutation, i.e. the - * destination space. - */ - Index size() const { return rangeIndices_.size(); } - - /** Whether the permutation contains any entries */ - bool empty() const { return rangeIndices_.empty(); } - - /** - * Resize the permutation. You must fill in the new entries if new new size - * is larger than the old one. If the new size is smaller, entries from the - * end are discarded. - */ - void resize(size_t newSize) { rangeIndices_.resize(newSize); } - - /** - * Return an identity permutation. - */ - static Permutation Identity(Index nVars); - - /** - * Create a permutation that pulls the given variables to the front while - * pushing the rest to the back. - */ - static Permutation PullToFront(const std::vector& toFront, size_t size, bool filterDuplicates = false); - - /** - * Create a permutation that pushes the given variables to the back while - * pulling the rest to the front. - */ - static Permutation PushToBack(const std::vector& toBack, size_t size, bool filterDuplicates = false); - - - /** - * Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]]. - */ - Permutation::shared_ptr permute(const Permutation& permutation) const; - - /** - * Return the inverse permutation. This is only possible if this is a non- - * reducing permutation, that is, (*this)[i] < this->size() for all - * i < size(). If NDEBUG is not defined, this conditional will be checked. - */ - Permutation::shared_ptr inverse() const; - - const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices - const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices - - /** Apply the permutation to a collection, which must have operator[] defined. - * Note that permutable gtsam data structures typically have their own - * permute function to apply a permutation. Permutation::applyToCollection is - * a generic function, e.g. for STL classes. - * @param input The input collection. - * @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]] - */ - template - void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const { - for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; } - - - /// @} - /// @name Advanced Interface - /// @{ - - iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices - iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices - -protected: - void check(Index variable) const { assert(variable < rangeIndices_.size()); } - - /// @} -}; - - -namespace internal { - /** - * An internal class used for storing and applying a permutation from a map - */ - class Reduction : public gtsam::FastMap { - public: - typedef gtsam::FastMap Base; - - GTSAM_EXPORT static Reduction CreateAsInverse(const Permutation& p); - GTSAM_EXPORT static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p); - GTSAM_EXPORT void applyInverse(std::vector& js) const; - GTSAM_EXPORT Permutation inverse() const; - GTSAM_EXPORT const Index& operator[](const Index& j); - GTSAM_EXPORT const Index& operator[](const Index& j) const; - - GTSAM_EXPORT void print(const std::string& s="") const; - GTSAM_EXPORT bool equals(const Reduction& other, double tol = 1e-9) const; - }; - - /** - * Reduce the variable indices so that those in the set are mapped to start at zero - */ - GTSAM_EXPORT Permutation createReducingPermutation(const std::set& indices); -} // \namespace internal -} // \namespace gtsam diff --git a/gtsam/inference/SymbolicFactorGraphOrdered.cpp b/gtsam/inference/SymbolicFactorGraphOrdered.cpp deleted file mode 100644 index 76c1e42fb..000000000 --- a/gtsam/inference/SymbolicFactorGraphOrdered.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicFactorGraph.cpp - * @date Oct 29, 2009 - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -#include - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ - SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet) : - FactorGraphOrdered(bayesNet) {} - - /* ************************************************************************* */ - SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree) : - FactorGraphOrdered(bayesTree) {} - - /* ************************************************************************* */ - void SymbolicFactorGraphOrdered::push_factor(Index key) { - push_back(boost::make_shared(key)); - } - - /** Push back binary factor */ - void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2) { - push_back(boost::make_shared(key1,key2)); - } - - /** Push back ternary factor */ - void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3) { - push_back(boost::make_shared(key1,key2,key3)); - } - - /** Push back 4-way factor */ - void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3, Index key4) { - push_back(boost::make_shared(key1,key2,key3,key4)); - } - - /* ************************************************************************* */ - FastSet - SymbolicFactorGraphOrdered::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(factor) keys.insert(factor->begin(), factor->end()); } - return keys; - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraphOrdered::eliminateFrontals(size_t nFrontals) const - { - return FactorGraphOrdered::eliminateFrontals(nFrontals, EliminateSymbolic); - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraphOrdered::eliminate(const std::vector& variables) const - { - return FactorGraphOrdered::eliminate(variables, EliminateSymbolic); - } - - /* ************************************************************************* */ - std::pair - SymbolicFactorGraphOrdered::eliminateOne(Index variable) const - { - return FactorGraphOrdered::eliminateOne(variable, EliminateSymbolic); - } - - /* ************************************************************************* */ - void SymbolicFactorGraphOrdered::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void SymbolicFactorGraphOrdered::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); - } - } - - /* ************************************************************************* */ - IndexFactorOrdered::shared_ptr CombineSymbolic( - const FactorGraphOrdered& factors, const FastMap >& variableSlots) { - IndexFactorOrdered::shared_ptr combined(Combine (factors, variableSlots)); -// combined->assertInvariants(); - return combined; - } - - /* ************************************************************************* */ - pair // - EliminateSymbolic(const FactorGraphOrdered& factors, size_t nrFrontals) { - - FastSet keys; - BOOST_FOREACH(const IndexFactorOrdered::shared_ptr& factor, factors) - BOOST_FOREACH(Index var, *factor) - keys.insert(var); - - if (keys.size() < nrFrontals) throw invalid_argument( - "EliminateSymbolic requested to eliminate more variables than exist in graph."); - - vector newKeys(keys.begin(), keys.end()); - return make_pair(boost::make_shared(newKeys, nrFrontals), - boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); - } - - /* ************************************************************************* */ -} diff --git a/gtsam/inference/SymbolicFactorGraphOrdered.h b/gtsam/inference/SymbolicFactorGraphOrdered.h deleted file mode 100644 index 97ef21810..000000000 --- a/gtsam/inference/SymbolicFactorGraphOrdered.h +++ /dev/null @@ -1,151 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SymbolicFactorGraph.h - * @date Oct 29, 2009 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { template class EliminationTreeOrdered; } -namespace gtsam { template class BayesNetOrdered; } -namespace gtsam { template class BayesTreeOrdered; } -namespace gtsam { class IndexConditionalOrdered; } - -namespace gtsam { - - typedef EliminationTreeOrdered SymbolicEliminationTreeOrdered; - typedef BayesNetOrdered SymbolicBayesNetOrdered; - typedef BayesTreeOrdered SymbolicBayesTreeOrdered; - - /** Symbolic IndexFactor Graph - * \nosubgrouping - */ - class SymbolicFactorGraphOrdered: public FactorGraphOrdered { - - public: - - /// @name Standard Constructors - /// @{ - - /** Construct empty factor graph */ - SymbolicFactorGraphOrdered() { - } - - /** Construct from a BayesNet */ - GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet); - - /** Construct from a BayesTree */ - GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree); - - /** - * Construct from a factor graph of any type - */ - template - SymbolicFactorGraphOrdered(const FactorGraphOrdered& fg); - - /** Eliminate the first \c n frontal variables, returning the resulting - * conditional and remaining factor graph - this is very inefficient for - * eliminating all variables, to do that use EliminationTree or - * JunctionTree. Note that this version simply calls - * FactorGraph::eliminateFrontals with EliminateSymbolic - * as the eliminate function argument. - */ - GTSAM_EXPORT std::pair eliminateFrontals(size_t nFrontals) const; - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - * Note that this version simply calls - * FactorGraph::eliminate with EliminateSymbolic as the eliminate - * function argument. - */ - GTSAM_EXPORT std::pair eliminate(const std::vector& variables) const; - - /** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ - GTSAM_EXPORT std::pair eliminateOne(Index variable) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** - * Return the set of variables involved in the factors (computes a set - * union). - */ - GTSAM_EXPORT FastSet keys() const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** Push back unary factor */ - GTSAM_EXPORT void push_factor(Index key); - - /** Push back binary factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2); - - /** Push back ternary factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3); - - /** Push back 4-way factor */ - GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4); - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); - - }; - - /** Create a combined joint factor (new style for EliminationTree). */ - GTSAM_EXPORT IndexFactorOrdered::shared_ptr CombineSymbolic(const FactorGraphOrdered& factors, - const FastMap >& variableSlots); - - /** - * CombineAndEliminate provides symbolic elimination. - * Combine and eliminate can also be called separately, but for this and - * derived classes calling them separately generally does extra work. - */ - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateSymbolic(const FactorGraphOrdered&, size_t nrFrontals = 1); - - /// @} - - /** Template function implementation */ - template - SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const FactorGraphOrdered& fg) { - for (size_t i = 0; i < fg.size(); i++) { - if (fg[i]) { - IndexFactorOrdered::shared_ptr factor(new IndexFactorOrdered(*fg[i])); - push_back(factor); - } else - push_back(IndexFactorOrdered::shared_ptr()); - } - } - -} // namespace gtsam diff --git a/gtsam/inference/SymbolicMultifrontalSolverOrdered.cpp b/gtsam/inference/SymbolicMultifrontalSolverOrdered.cpp deleted file mode 100644 index b1e142d4b..000000000 --- a/gtsam/inference/SymbolicMultifrontalSolverOrdered.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 22, 2010 - */ - -#include -#include - -namespace gtsam { - -//template class GenericMultifrontalSolver > >; - -} diff --git a/gtsam/inference/SymbolicMultifrontalSolverOrdered.h b/gtsam/inference/SymbolicMultifrontalSolverOrdered.h deleted file mode 100644 index fc36c3388..000000000 --- a/gtsam/inference/SymbolicMultifrontalSolverOrdered.h +++ /dev/null @@ -1,71 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 22, 2010 - */ - -#pragma once - -#include -#include - -namespace gtsam { - - class SymbolicMultifrontalSolver : GenericMultifrontalSolver > > { - - protected: - typedef GenericMultifrontalSolver > > Base; - - public: - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {}; - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; - - /** Print to cout */ - void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); }; - - /** Test whether is equal to another */ - bool equals(const SymbolicMultifrontalSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); }; - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - SymbolicBayesTreeOrdered::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); }; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; - }; - -} diff --git a/gtsam/inference/SymbolicSequentialSolverOrdered.cpp b/gtsam/inference/SymbolicSequentialSolverOrdered.cpp deleted file mode 100644 index 0c4fc8386..000000000 --- a/gtsam/inference/SymbolicSequentialSolverOrdered.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#include -#include - -namespace gtsam { - -// An explicit instantiation to be compiled into the library -//template class GenericSequentialSolver; - -} diff --git a/gtsam/inference/SymbolicSequentialSolverOrdered.h b/gtsam/inference/SymbolicSequentialSolverOrdered.h deleted file mode 100644 index 2bf245e81..000000000 --- a/gtsam/inference/SymbolicSequentialSolverOrdered.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ -#pragma once - -#include -#include - -namespace gtsam { - - class SymbolicSequentialSolver : GenericSequentialSolver { - - protected: - typedef GenericSequentialSolver Base; - - public: - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which does the symbolic elimination, identifies the cliques, - * and distributes all the factors to the right cliques. - */ - SymbolicSequentialSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {}; - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - SymbolicSequentialSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {}; - - /** Print to cout */ - void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); }; - - /** Test whether is equal to another */ - bool equals(const SymbolicSequentialSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); }; - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - SymbolicBayesNetOrdered::shared_ptr eliminate() const - { return Base::eliminate(&EliminateSymbolic); }; - - /** - * Compute a conditional density P(F|S) while marginalizing out variables J - * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) - * Returns the result as a Bayes net. - */ - SymbolicBayesNetOrdered::shared_ptr conditionalBayesNet(const std::vector& js, size_t nrFrontals) const - { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a Bayes net. - */ - SymbolicBayesNetOrdered::shared_ptr jointBayesNet(const std::vector& js) const - { return Base::jointBayesNet(js, &EliminateSymbolic); }; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. Returns the result as a factor graph. - */ - SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector& js) const - { return Base::jointFactorGraph(js, &EliminateSymbolic); }; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); }; - }; - -} - diff --git a/gtsam/inference/VariableIndexOrdered.cpp b/gtsam/inference/VariableIndexOrdered.cpp deleted file mode 100644 index 70b6afa70..000000000 --- a/gtsam/inference/VariableIndexOrdered.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 22, 2010 - */ - -#include - -#include -#include - -namespace gtsam { - -using namespace std; - -/* ************************************************************************* */ -bool VariableIndexOrdered::equals(const VariableIndexOrdered& other, double tol) const { - if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { - for(size_t var=0; var < 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 VariableIndexOrdered::print(const string& str) const { - cout << str; - cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - for(Index var = 0; var < size(); ++var) { - cout << "var " << var << ":"; - BOOST_FOREACH(const size_t factor, index_[var]) - cout << " " << factor; - cout << "\n"; - } - cout << flush; -} - -/* ************************************************************************* */ -void VariableIndexOrdered::outputMetisFormat(ostream& os) const { - os << size() << " " << nFactors() << "\n"; - // run over variables, which will be hyper-edges. - BOOST_FOREACH(const Factors& variable, index_) { - // every variable is a hyper-edge covering its factors - BOOST_FOREACH(const size_t factor, variable) - os << (factor+1) << " "; // base 1 - os << "\n"; - } - os << flush; -} - -/* ************************************************************************* */ -void VariableIndexOrdered::permuteInPlace(const Permutation& permutation) { - // Create new index and move references to data into it in permuted order - vector newIndex(this->size()); - for(Index i = 0; i < newIndex.size(); ++i) - newIndex[i].swap(this->index_[permutation[i]]); - - // Move reference to entire index into the VariableIndex - index_.swap(newIndex); -} - -/* ************************************************************************* */ -void VariableIndexOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - vector newIndex(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]); - // Put the affected entries back in the new order - for(size_t slot = 0; slot < selector.size(); ++slot) - this->index_[selector[slot]].swap(newIndex[slot]); -} - -/* ************************************************************************* */ -void VariableIndexOrdered::removeUnusedAtEnd(size_t nToRemove) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(size_t i = this->size() - nToRemove; i < this->size(); ++i) - if(!(*this)[i].empty()) - throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); -#endif - - index_.resize(this->size() - nToRemove); -} - -} diff --git a/gtsam/inference/VariableIndexOrdered.h b/gtsam/inference/VariableIndexOrdered.h deleted file mode 100644 index cf6867dd2..000000000 --- a/gtsam/inference/VariableIndexOrdered.h +++ /dev/null @@ -1,292 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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.h - * @author Richard Roberts - * @date Sep 12, 2010 - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -namespace gtsam { - - class Permutation; - -/** - * The VariableIndex class computes and stores the block column structure of a - * factor graph. The factor graph stores a collection of factors, each of - * which involves a set of variables. In contrast, the VariableIndex is built - * from a factor graph prior to elimination, and stores the list of factors - * that involve each variable. This information is stored as a deque of - * lists of factor indices. - * \nosubgrouping - */ -class GTSAM_EXPORT VariableIndexOrdered { -public: - - typedef boost::shared_ptr shared_ptr; - typedef FastList Factors; - typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; - -protected: - std::vector index_; - size_t nFactors_; // Number of factors in the original factor graph. - size_t nEntries_; // Sum of involved variable counts of each factor. - -public: - - /// @name Standard Constructors - /// @{ - - /** Default constructor, creates an empty VariableIndex */ - VariableIndexOrdered() : nFactors_(0), nEntries_(0) {} - - /** - * Create a VariableIndex that computes and stores the block column structure - * of a factor graph. This constructor is used when the number of variables - * is known beforehand. - */ - template VariableIndexOrdered(const FG& factorGraph, Index nVariables); - - /** - * Create a VariableIndex that computes and stores the block column structure - * of a factor graph. - */ - template VariableIndexOrdered(const FG& factorGraph); - - /// @} - /// @name Standard Interface - /// @{ - - /** - * The number of variable entries. This is one greater than the variable - * with the highest index. - */ - Index size() const { return index_.size(); } - - /** The number of factors in the original factor graph */ - size_t nFactors() const { return nFactors_; } - - /** The number of nonzero blocks, i.e. the number of variable-factor entries */ - size_t nEntries() const { return nEntries_; } - - /** Access a list of factors by variable */ - const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; } - - /// @} - /// @name Testable - /// @{ - - /** Test for equality (for unit tests and debug assertions). */ - bool equals(const VariableIndexOrdered& other, double tol=0.0) const; - - /** Print the variable index (for unit tests and debugging). */ - void print(const std::string& str = "VariableIndex: ") const; - - /** - * Output dual hypergraph to Metis file format for use with hmetis - * In the dual graph, variables are hyperedges, factors are nodes. - */ - void outputMetisFormat(std::ostream& os) const; - - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template void augment(const FG& factors); - - /** - * Remove entries corresponding to the specified factors. - * NOTE: We intentionally do not decrement nFactors_ because the factor - * indices need to remain consistent. Removing factors from a factor graph - * does not shift the indices of other factors. Also, we keep nFactors_ - * one greater than the highest-numbered factor referenced in a VariableIndex. - * - * @param indices The indices of the factors to remove, which must match \c factors - * @param factors The factors being removed, which must symbolically correspond - * exactly to the factors with the specified \c indices that were added. - */ - template void remove(const CONTAINER& indices, const FG& factors); - - /// Permute the variables in the VariableIndex according to the given permutation - void permuteInPlace(const Permutation& permutation); - - /// Permute the variables in the VariableIndex according to the given partial permutation - void permuteInPlace(const Permutation& selector, const Permutation& permutation); - - /** Remove unused empty variables at the end of the ordering (in debug mode - * verifies they are empty). - * @param nToRemove The number of unused variables at the end to remove - */ - void removeUnusedAtEnd(size_t nToRemove); - -protected: - Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } - Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } - - Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } - Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } - - /// Internal constructor to allocate a VariableIndex of the requested size - VariableIndexOrdered(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {} - - /// Internal check of the validity of a variable - void checkVar(Index variable) const { assert(variable < index_.size()); } - - /// Internal function to populate the variable index from a factor graph - template void fill(const FG& factorGraph); - - /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(index_); - ar & BOOST_SERIALIZATION_NVP(nFactors_); - ar & BOOST_SERIALIZATION_NVP(nEntries_); - } -}; - -/* ************************************************************************* */ -template -void VariableIndexOrdered::fill(const FG& factorGraph) { - gttic(VariableIndex_fill); - // Build index mapping from variable id to factor index - for(size_t fi=0; fikeys()) { - if(key < index_.size()) { - index_[key].push_back(fi); - ++ nEntries_; - } - } - } - ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent - } -} - -/* ************************************************************************* */ -template -VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph) : - nFactors_(0), nEntries_(0) -{ - gttic(VariableIndex_constructor); - // 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) { - gttic(VariableIndex_constructor_find_highest); - // Find highest-numbered variable - Index maxVar = 0; - BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { - if(factor) { - BOOST_FOREACH(const Index key, factor->keys()) { - if(key > maxVar) - maxVar = key; - } - } - } - gttoc(VariableIndex_constructor_find_highest); - - // Allocate array - gttic(VariableIndex_constructor_allocate); - index_.resize(maxVar+1); - gttoc(VariableIndex_constructor_allocate); - - fill(factorGraph); - } -} - -/* ************************************************************************* */ -template -VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph, Index nVariables) : - nFactors_(0), nEntries_(0) -{ - gttic(VariableIndex_constructor_allocate); - index_.resize(nVariables); - gttoc(VariableIndex_constructor_allocate); - fill(factorGraph); -} - -/* ************************************************************************* */ -template -void VariableIndexOrdered::augment(const FG& factors) { - gttic(VariableIndex_augment); - // If the factor graph is empty, return an empty index because inside this - // if block we assume at least one factor. - if(factors.size() > 0) { - // Find highest-numbered variable - Index maxVar = 0; - BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) { - if(factor) { - BOOST_FOREACH(const Index key, factor->keys()) { - if(key > maxVar) - maxVar = key; - } - } - } - - // Allocate index - index_.resize(std::max(index_.size(), maxVar+1)); - - // Augment index mapping from variable id to factor index - size_t orignFactors = nFactors_; - for(size_t fi=0; fikeys()) { - index_[key].push_back(orignFactors + fi); - ++ nEntries_; - } - } - ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent - } - } -} - -/* ************************************************************************* */ -template -void VariableIndexOrdered::remove(const CONTAINER& indices, const FG& factors) { - gttic(VariableIndex_remove); - // NOTE: We intentionally do not decrement nFactors_ because the factor - // indices need to remain consistent. Removing factors from a factor graph - // does not shift the indices of other factors. Also, we keep nFactors_ - // one greater than the highest-numbered factor referenced in a VariableIndex. - for(size_t fi=0; fikeys().size(); ++ji) { - Factors& factorEntries = index_[factors[fi]->keys()[ji]]; - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]); - if(entry == factorEntries.end()) - throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); - factorEntries.erase(entry); - -- nEntries_; - } - } -} - -} diff --git a/gtsam/inference/inferenceOrdered-inl.h b/gtsam/inference/inferenceOrdered-inl.h deleted file mode 100644 index 1a7612d92..000000000 --- a/gtsam/inference/inferenceOrdered-inl.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference-inl.h - * @brief - * @author Richard Roberts - * @date Mar 3, 2012 - */ - -#pragma once - -#include - -// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h -#include - -#include - -namespace gtsam { - -namespace inference { - -/* ************************************************************************* */ -template -Permutation::shared_ptr PermutationCOLAMD( - const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) { - gttic(PermutationCOLAMD_constrained); - - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - // If at least some variables are not constrained to be last, constrain the - // ones that should be constrained. - if(constrainLast.size() < n) { - BOOST_FOREACH(Index var, constrainLast) { - assert(var < n); - cmember[var] = 1; - } - } - - Permutation::shared_ptr permutation = PermutationCOLAMD_(variableIndex, cmember); - if (forceOrder) { - Index j=n; - BOOST_REVERSE_FOREACH(Index c, constrainLast) - permutation->operator[](--j) = c; - } - return permutation; -} - -/* ************************************************************************* */ -template -Permutation::shared_ptr PermutationCOLAMDGrouped( - const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints) { - gttic(PermutationCOLAMD_grouped); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - - typedef typename CONSTRAINED_MAP::value_type constraint_pair; - BOOST_FOREACH(const constraint_pair& p, constraints) { - assert(p.first < n); - // FIXME: check that no groups are skipped - cmember[p.first] = p.second; - } - - return PermutationCOLAMD_(variableIndex, cmember); -} - -/* ************************************************************************* */ -inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndexOrdered& variableIndex) { - gttic(PermutationCOLAMD_unconstrained); - size_t n = variableIndex.size(); - std::vector cmember(n, 0); - return PermutationCOLAMD_(variableIndex, cmember); -} - -} // namespace inference -} // namespace gtsam - - diff --git a/gtsam/inference/inferenceOrdered.cpp b/gtsam/inference/inferenceOrdered.cpp deleted file mode 100644 index a5e546cb8..000000000 --- a/gtsam/inference/inferenceOrdered.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference.cpp - * @brief inference definitions - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include -#include - -#include -#include -#include -#include - -#include - -using namespace std; - -namespace gtsam { -namespace inference { - -/* ************************************************************************* */ -Permutation::shared_ptr PermutationCOLAMD_(const VariableIndexOrdered& variableIndex, std::vector& cmember) { - gttic(PermutationCOLAMD_internal); - - gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ - vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ - vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ - - static const bool debug = false; - - p[0] = 0; - int count = 0; - for(Index var = 0; var < variableIndex.size(); ++var) { - const VariableIndexOrdered::Factors& column(variableIndex[var]); - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if(lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); - A[count++] = (int)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 - } - - assert((size_t)count == variableIndex.nEntries()); - - if(debug) - for(size_t i=0; ioperator[](j) = j; - // else - permutation->operator[](j) = p[j]; - if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl; - } - if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl; - gttoc(Create_permutation); - - return permutation; -} - -/* ************************************************************************* */ -} // \namespace inference -} // \namespace gtsam diff --git a/gtsam/inference/inferenceOrdered.h b/gtsam/inference/inferenceOrdered.h deleted file mode 100644 index ee3bcad85..000000000 --- a/gtsam/inference/inferenceOrdered.h +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 inference.h - * @brief Contains *generic* inference algorithms that convert between templated - * graphical models, i.e., factor graphs, Bayes nets, and Bayes trees - * @author Frank Dellaert - * @author Richard Roberts - */ - -#pragma once - -#include -#include - -#include -#include - -#include - -namespace gtsam { - - namespace inference { - - /** - * Compute a permutation (variable ordering) using colamd - */ - GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD( - const VariableIndexOrdered& variableIndex); - - /** - * Compute a permutation (variable ordering) using constrained colamd to move - * a set of variables to the end of the ordering - * @param variableIndex is the variable index lookup from a graph - * @param constrainlast is a vector of keys that should be constrained - * @tparam constrainLast is a std::vector (or similar structure) - * @param forceOrder if true, will not allow re-ordering of constrained variables - */ - template - Permutation::shared_ptr PermutationCOLAMD( - const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false); - - /** - * Compute a permutation of variable ordering using constrained colamd to - * move variables to the end in groups (0 = unconstrained, higher numbers at - * the end). - * @param variableIndex is the variable index lookup from a graph - * @param constraintMap is a map from variable index -> group number for constrained variables - * @tparam CONSTRAINED_MAP is an associative structure (like std::map), from size_t->int - */ - template - Permutation::shared_ptr PermutationCOLAMDGrouped( - const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints); - - /** - * Compute a CCOLAMD permutation using the constraint groups in cmember. - * The format for cmember is a part of ccolamd. - * - * @param variableIndex is the variable structure from a graph - * @param cmember is the constraint group list for each variable, where - * 0 is the default, unconstrained group, and higher numbers move further to - * the back of the list - * - * AGC: does cmember change? - */ - GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_( - const VariableIndexOrdered& variableIndex, std::vector& cmember); - - } // \namespace inference - -} // \namespace gtsam - -#include diff --git a/gtsam/inference/tests/testBayesTreeObsolete.cpp b/gtsam/inference/tests/testBayesTreeObsolete.cpp deleted file mode 100644 index 54bd2922f..000000000 --- a/gtsam/inference/tests/testBayesTreeObsolete.cpp +++ /dev/null @@ -1,563 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBayesTree.cpp - * @brief Unit tests for Bayes Tree - * @author Frank Dellaert - * @author Michael Kaess - * @author Viorela Ila - */ - -#include // for operator += -#include -#include -#include -using namespace boost::assign; - -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -///* ************************************************************************* */ -//// SLAM example from RSS sqrtSAM paper -static const Index _x3_=0, _x2_=1; -//static const Index _x1_=2, _l2_=3, _l1_=4; // unused -//IndexConditionalOrdered::shared_ptr -// x3(new IndexConditionalOrdered(_x3_)), -// x2(new IndexConditionalOrdered(_x2_,_x3_)), -// x1(new IndexConditionalOrdered(_x1_,_x2_,_x3_)), -// l1(new IndexConditionalOrdered(_l1_,_x1_,_x2_)), -// l2(new IndexConditionalOrdered(_l2_,_x1_,_x3_)); -// -//// Bayes Tree for sqrtSAM example -//SymbolicBayesTreeOrdered createSlamSymbolicBayesTree(){ -// // Create using insert -//// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_; -// SymbolicBayesTreeOrdered bayesTree_slam; -// bayesTree_slam.insert(x3); -// bayesTree_slam.insert(x2); -// bayesTree_slam.insert(x1); -// bayesTree_slam.insert(l2); -// bayesTree_slam.insert(l1); -// return bayesTree_slam; -//} - -/* ************************************************************************* */ -// Conditionals for ASIA example from the tutorial with A and D evidence -static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; -static IndexConditionalOrdered::shared_ptr - B(new IndexConditionalOrdered(_B_)), - L(new IndexConditionalOrdered(_L_, _B_)), - E(new IndexConditionalOrdered(_E_, _L_, _B_)), - S(new IndexConditionalOrdered(_S_, _L_, _B_)), - T(new IndexConditionalOrdered(_T_, _E_, _L_)), - X(new IndexConditionalOrdered(_X_, _E_)); - -// Cliques -static IndexConditionalOrdered::shared_ptr - ELB(IndexConditionalOrdered::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3)); - -// Bayes Tree for Asia example -static SymbolicBayesTreeOrdered createAsiaSymbolicBayesTree() { - SymbolicBayesTreeOrdered bayesTree; -// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_; - SymbolicBayesTreeOrdered::insert(bayesTree, B); - SymbolicBayesTreeOrdered::insert(bayesTree, L); - SymbolicBayesTreeOrdered::insert(bayesTree, E); - SymbolicBayesTreeOrdered::insert(bayesTree, S); - SymbolicBayesTreeOrdered::insert(bayesTree, T); - SymbolicBayesTreeOrdered::insert(bayesTree, X); - return bayesTree; -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, constructor ) -{ - // Create using insert - SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); - - bayesTree.print("bayesTree (ordered): "); - - // Check Size - LONGS_EQUAL(4,bayesTree.size()); - EXPECT(!bayesTree.empty()); - - // Check root - boost::shared_ptr actual_root = bayesTree.root()->conditional(); - CHECK(assert_equal(*ELB,*actual_root)); - - // Create from symbolic Bayes chain in which we want to discover cliques - BayesNetOrdered ASIA; - ASIA.push_back(X); - ASIA.push_back(T); - ASIA.push_back(S); - ASIA.push_back(E); - ASIA.push_back(L); - ASIA.push_back(B); - SymbolicBayesTreeOrdered bayesTree2(ASIA); - - // Check whether the same - CHECK(assert_equal(bayesTree,bayesTree2)); - - // CHECK findParentClique, should *not depend on order of parents* -// Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_; -// IndexTable index(ordering); - - list parents1; parents1 += _E_, _L_; - CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1))); - - list parents2; parents2 += _L_, _E_; - CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2))); - - list parents3; parents3 += _L_, _B_; - CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3))); -} - -/* ************************************************************************* */ -TEST(BayesTreeOrdered, clear) -{ -// SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); -// bayesTree.clear(); -// -// SymbolicBayesTreeOrdered expected; -// -// // Check whether cleared BayesTreeOrdered is equal to a new BayesTreeOrdered -// CHECK(assert_equal(expected, bayesTree)); -} - -/* ************************************************************************* * -Bayes Tree for testing conversion to a forest of orphans needed for incremental. - A,B - C|A E|B - D|C F|E - */ -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath ) -{ - const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_)), - B(new IndexConditionalOrdered(_B_, _A_)), - C(new IndexConditionalOrdered(_C_, _A_)), - D(new IndexConditionalOrdered(_D_, _C_)), - E(new IndexConditionalOrdered(_E_, _B_)), - F(new IndexConditionalOrdered(_F_, _E_)); - SymbolicBayesTreeOrdered bayesTree; - EXPECT(bayesTree.empty()); -// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; - SymbolicBayesTreeOrdered::insert(bayesTree, A); - SymbolicBayesTreeOrdered::insert(bayesTree, B); - SymbolicBayesTreeOrdered::insert(bayesTree, C); - SymbolicBayesTreeOrdered::insert(bayesTree, D); - SymbolicBayesTreeOrdered::insert(bayesTree, E); - SymbolicBayesTreeOrdered::insert(bayesTree, F); - - // remove C, expected outcome: factor graph with ABC, - // Bayes Tree now contains two orphan trees: D|C and E|B,F|E - SymbolicFactorGraphOrdered expected; - expected.push_factor(_B_,_A_); -// expected.push_factor(_A_); - expected.push_factor(_C_,_A_); - SymbolicBayesTreeOrdered::Cliques expectedOrphans; - expectedOrphans += bayesTree[_D_], bayesTree[_E_]; - - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - bayesTree.removePath(bayesTree[_C_], bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - CHECK(assert_equal((SymbolicFactorGraphOrdered)expected, factors)); - CHECK(assert_equal(expectedOrphans, orphans)); - - // remove E: factor graph with EB; E|B removed from second orphan tree - SymbolicFactorGraphOrdered expected2; - expected2.push_factor(_E_,_B_); - SymbolicBayesTreeOrdered::Cliques expectedOrphans2; - expectedOrphans2 += bayesTree[_F_]; - - BayesNetOrdered bn2; - SymbolicBayesTreeOrdered::Cliques orphans2; - bayesTree.removePath(bayesTree[_E_], bn2, orphans2); - SymbolicFactorGraphOrdered factors2(bn2); - CHECK(assert_equal((SymbolicFactorGraphOrdered)expected2, factors2)); - CHECK(assert_equal(expectedOrphans2, orphans2)); -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath2 ) -{ - SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); - - // Call remove-path with clique B - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - bayesTree.removePath(bayesTree[_B_], bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - - // Check expected outcome - SymbolicFactorGraphOrdered expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTreeOrdered::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removePath3 ) -{ - SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); - - // Call remove-path with clique S - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - bayesTree.removePath(bayesTree[_S_], bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - - // Check expected outcome - SymbolicFactorGraphOrdered expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTreeOrdered::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -void getAllCliques(const SymbolicBayesTreeOrdered::sharedClique& subtree, SymbolicBayesTreeOrdered::Cliques& cliques) { - // Check if subtree exists - if (subtree) { - cliques.push_back(subtree); - // Recursive call over all child cliques - BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& childClique, subtree->children()) { - getAllCliques(childClique,cliques); - } - } -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, shortcutCheck ) -{ - const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_)), - B(new IndexConditionalOrdered(_B_, _A_)), - C(new IndexConditionalOrdered(_C_, _A_)), - D(new IndexConditionalOrdered(_D_, _C_)), - E(new IndexConditionalOrdered(_E_, _B_)), - F(new IndexConditionalOrdered(_F_, _E_)), - G(new IndexConditionalOrdered(_G_, _F_)); - SymbolicBayesTreeOrdered bayesTree; -// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; - SymbolicBayesTreeOrdered::insert(bayesTree, A); - SymbolicBayesTreeOrdered::insert(bayesTree, B); - SymbolicBayesTreeOrdered::insert(bayesTree, C); - SymbolicBayesTreeOrdered::insert(bayesTree, D); - SymbolicBayesTreeOrdered::insert(bayesTree, E); - SymbolicBayesTreeOrdered::insert(bayesTree, F); - SymbolicBayesTreeOrdered::insert(bayesTree, G); - - //bayesTree.print("BayesTreeOrdered"); - //bayesTree.saveGraph("BT1.dot"); - - SymbolicBayesTreeOrdered::sharedClique rootClique= bayesTree.root(); - //rootClique->printTree(); - SymbolicBayesTreeOrdered::Cliques allCliques; - getAllCliques(rootClique,allCliques); - - BayesNetOrdered bn; - BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) { - //clique->print("Clique#"); - bn = clique->shortcut(rootClique, &EliminateSymbolic); - //bn.print("Shortcut:\n"); - //cout << endl; - } - - // Check if all the cached shortcuts are cleared - rootClique->deleteCachedShortcuts(); - BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) { - bool notCleared = clique->cachedSeparatorMarginal(); - CHECK( notCleared == false); - } - EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals()); - -// BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) { -// clique->print("Clique#"); -// if(clique->cachedShortcut()){ -// bn = clique->cachedShortcut().get(); -// bn.print("Shortcut:\n"); -// } -// else -// cout << "Not Initialized" << endl; -// cout << endl; -// } -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop ) -{ - SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); - - // create a new factor to be inserted - boost::shared_ptr newFactor(new IndexFactorOrdered(_S_,_B_)); - - // Remove the contaminated part of the Bayes tree - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - list keys; keys += _B_,_S_; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - - // Check expected outcome - SymbolicFactorGraphOrdered expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTreeOrdered::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); - - // Try removeTop again with a factor that should not change a thing - boost::shared_ptr newFactor2(new IndexFactorOrdered(_B_)); - BayesNetOrdered bn2; - SymbolicBayesTreeOrdered::Cliques orphans2; - keys.clear(); keys += _B_; - bayesTree.removeTop(keys, bn2, orphans2); - SymbolicFactorGraphOrdered factors2(bn2); - SymbolicFactorGraphOrdered expected2; - CHECK(assert_equal(expected2, factors2)); - SymbolicBayesTreeOrdered::Cliques expectedOrphans2; - CHECK(assert_equal(expectedOrphans2, orphans2)); -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop2 ) -{ - SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree(); - - // create two factors to be inserted - SymbolicFactorGraphOrdered newFactors; - newFactors.push_factor(_B_); - newFactors.push_factor(_S_); - - // Remove the contaminated part of the Bayes tree - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - list keys; keys += _B_,_S_; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - - // Check expected outcome - SymbolicFactorGraphOrdered expected; - expected.push_factor(_E_,_L_,_B_); -// expected.push_factor(_L_,_B_); -// expected.push_factor(_B_); - expected.push_factor(_S_,_L_,_B_); - CHECK(assert_equal(expected, factors)); - SymbolicBayesTreeOrdered::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_equal(expectedOrphans, orphans)); -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, removeTop3 ) -{ - const Index _x4_=5, _l5_=6; - // simple test case that failed after COLAMD was fixed/activated - IndexConditionalOrdered::shared_ptr - X(new IndexConditionalOrdered(_l5_)), - A(new IndexConditionalOrdered(_x4_, _l5_)), - B(new IndexConditionalOrdered(_x2_, _x4_)), - C(new IndexConditionalOrdered(_x3_, _x2_)); - -// Ordering newOrdering; -// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; - SymbolicBayesTreeOrdered bayesTree; - SymbolicBayesTreeOrdered::insert(bayesTree, X); - SymbolicBayesTreeOrdered::insert(bayesTree, A); - SymbolicBayesTreeOrdered::insert(bayesTree, B); - SymbolicBayesTreeOrdered::insert(bayesTree, C); - - // remove all - list keys; - keys += _l5_, _x2_, _x3_, _x4_; - BayesNetOrdered bn; - SymbolicBayesTreeOrdered::Cliques orphans; - bayesTree.removeTop(keys, bn, orphans); - SymbolicFactorGraphOrdered factors(bn); - - CHECK(orphans.size() == 0); -} - -/* ************************************************************************* */ -TEST( BayesTreeOrdered, permute ) -{ - // creates a permutation and ensures that the nodes listing is updated - - // initial keys - more than just 6 variables - for a system with 9 variables - const Index _A0_=8, _B0_=7, _C0_=6, _D0_=5, _E0_=4, _F0_=0; - - // reduced keys - back to just 6 variables - const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; - - // Create and verify the permutation - std::set indices; indices += _A0_, _B0_, _C0_, _D0_, _E0_, _F0_; - Permutation actReducingPermutation = gtsam::internal::createReducingPermutation(indices); - Permutation expReducingPermutation(6); - expReducingPermutation[_A_] = _A0_; - expReducingPermutation[_B_] = _B0_; - expReducingPermutation[_C_] = _C0_; - expReducingPermutation[_D_] = _D0_; - expReducingPermutation[_E_] = _E0_; - expReducingPermutation[_F_] = _F0_; - EXPECT(assert_equal(expReducingPermutation, actReducingPermutation)); - - // Invert the permutation - gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation); - - // Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_)), - B(new IndexConditionalOrdered(_B_, _A_)), - C(new IndexConditionalOrdered(_C_, _A_)), - D(new IndexConditionalOrdered(_D_, _C_)), - E(new IndexConditionalOrdered(_E_, _B_)), - F(new IndexConditionalOrdered(_F_, _E_)); - SymbolicBayesTreeOrdered bayesTreeReduced; - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, A); - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, B); - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, C); - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, D); - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, E); - SymbolicBayesTreeOrdered::insert(bayesTreeReduced, F); - -// bayesTreeReduced.print("Reduced bayes tree"); -// P( 4 5) -// P( 3 | 5) -// P( 2 | 3) -// P( 1 | 4) -// P( 0 | 1) - - // Apply the permutation - should add placeholders for variables not present in nodes - SymbolicBayesTreeOrdered actBayesTree = *bayesTreeReduced.clone(); - actBayesTree.permuteWithInverse(expReducingPermutation); - -// actBayesTree.print("Full bayes tree"); -// P( 7 8) -// P( 6 | 8) -// P( 5 | 6) -// P( 4 | 7) -// P( 0 | 4) - - // check keys in cliques - std::vector expRootIndices; expRootIndices += _B0_, _A0_; - IndexConditionalOrdered::shared_ptr - expRoot(new IndexConditionalOrdered(expRootIndices, 2)), // root - A0(new IndexConditionalOrdered(_A0_)), - B0(new IndexConditionalOrdered(_B0_, _A0_)), - C0(new IndexConditionalOrdered(_C0_, _A0_)), // leaf level 1 - D0(new IndexConditionalOrdered(_D0_, _C0_)), // leaf level 2 - E0(new IndexConditionalOrdered(_E0_, _B0_)), // leaf level 2 - F0(new IndexConditionalOrdered(_F0_, _E0_)); // leaf level 3 - - CHECK(actBayesTree.root()); - EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional())); - EXPECT(assert_equal(*C0, *actBayesTree.root()->children().front()->conditional())); - EXPECT(assert_equal(*D0, *actBayesTree.root()->children().front()->children().front()->conditional())); - EXPECT(assert_equal(*E0, *actBayesTree.root()->children().back()->conditional())); - EXPECT(assert_equal(*F0, *actBayesTree.root()->children().back()->children().front()->conditional())); - - // check nodes structure - LONGS_EQUAL(9, actBayesTree.nodes().size()); - - SymbolicBayesTreeOrdered expFullTree; - SymbolicBayesTreeOrdered::insert(expFullTree, A0); - SymbolicBayesTreeOrdered::insert(expFullTree, B0); - SymbolicBayesTreeOrdered::insert(expFullTree, C0); - SymbolicBayesTreeOrdered::insert(expFullTree, D0); - SymbolicBayesTreeOrdered::insert(expFullTree, E0); - SymbolicBayesTreeOrdered::insert(expFullTree, F0); - - EXPECT(assert_equal(expFullTree, actBayesTree)); -} - -///* ************************************************************************* */ -///** -// * x2 - x3 - x4 - x5 -// * | / \ | -// * x1 / \ x6 -// */ -//TEST( BayesTreeOrdered, insert ) -//{ -// // construct bayes tree by split the graph along the separator x3 - x4 -// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; -// SymbolicFactorGraphOrdered fg1, fg2, fg3; -// fg1.push_factor(_x3_, _x4_); -// fg2.push_factor(_x1_, _x2_); -// fg2.push_factor(_x2_, _x3_); -// fg2.push_factor(_x1_, _x3_); -// fg3.push_factor(_x5_, _x4_); -// fg3.push_factor(_x6_, _x5_); -// fg3.push_factor(_x6_, _x4_); -// -//// Ordering ordering1; ordering1 += _x3_, _x4_; -//// Ordering ordering2; ordering2 += _x1_, _x2_; -//// Ordering ordering3; ordering3 += _x6_, _x5_; -// -// BayesNetOrdered bn1, bn2, bn3; -// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1); -// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1); -// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1); -// -// // insert child cliques -// SymbolicBayesTreeOrdered actual; -// list children; -// SymbolicBayesTreeOrdered::sharedClique r1 = actual.insert(bn2, children); -// SymbolicBayesTreeOrdered::sharedClique r2 = actual.insert(bn3, children); -// -// // insert root clique -// children.push_back(r1); -// children.push_back(r2); -// actual.insert(bn1, children, true); -// -// // traditional way -// SymbolicFactorGraphOrdered fg; -// fg.push_factor(_x3_, _x4_); -// fg.push_factor(_x1_, _x2_); -// fg.push_factor(_x2_, _x3_); -// fg.push_factor(_x1_, _x3_); -// fg.push_factor(_x5_, _x4_); -// fg.push_factor(_x6_, _x5_); -// fg.push_factor(_x6_, _x4_); -// -//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; -// BayesNetOrdered bn(*SymbolicSequentialSolver(fg).eliminate()); -// SymbolicBayesTreeOrdered expected(bn); -// CHECK(assert_equal(expected, actual)); -// -//} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testClusterTreeObsolete.cpp b/gtsam/inference/tests/testClusterTreeObsolete.cpp deleted file mode 100644 index 1f0ef4d1f..000000000 --- a/gtsam/inference/tests/testClusterTreeObsolete.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testClusterTree.cpp - * @brief Unit tests for Bayes Tree - * @author Kai Ni - * @author Frank Dellaert - */ - -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include - -using namespace gtsam; - -// explicit instantiation and typedef -namespace gtsam { template class ClusterTreeOrdered; } -typedef ClusterTreeOrdered SymbolicClusterTree; - -/* ************************************************************************* */ -TEST(ClusterTreeOrdered, constructor) { - SymbolicClusterTree tree; -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testConditionalObsolete.cpp b/gtsam/inference/tests/testConditionalObsolete.cpp deleted file mode 100644 index b70f41ddd..000000000 --- a/gtsam/inference/tests/testConditionalObsolete.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testConditional.cpp - * @brief Unit tests for IndexConditional class - * @author Frank Dellaert - */ - -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, empty ) -{ - IndexConditionalOrdered c0; - LONGS_EQUAL(0,c0.nrFrontals()) - LONGS_EQUAL(0,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, noParents ) -{ - IndexConditionalOrdered c0(0); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(0,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, oneParents ) -{ - IndexConditionalOrdered c0(0,1); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(1,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, twoParents ) -{ - IndexConditionalOrdered c0(0,1,2); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(2,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, threeParents ) -{ - IndexConditionalOrdered c0(0,1,2,3); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(3,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, fourParents ) -{ - vector parents; - parents += 1,2,3,4; - IndexConditionalOrdered c0(0,parents); - LONGS_EQUAL(1,c0.nrFrontals()) - LONGS_EQUAL(4,c0.nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, FromRange ) -{ - vector keys; - keys += 1,2,3,4,5; - IndexConditionalOrdered::shared_ptr c0(new IndexConditionalOrdered(keys,2)); - LONGS_EQUAL(2,c0->nrFrontals()) - LONGS_EQUAL(3,c0->nrParents()) -} - -/* ************************************************************************* */ -TEST( IndexConditionalOrdered, equals ) -{ - IndexConditionalOrdered c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); - CHECK(c0.equals(c1)); - CHECK(c1.equals(c0)); - CHECK(!c0.equals(c2)); - CHECK(!c2.equals(c0)); - CHECK(!c0.equals(c3)); - CHECK(!c3.equals(c0)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testEliminationTreeObsolete.cpp b/gtsam/inference/tests/testEliminationTreeObsolete.cpp deleted file mode 100644 index 76c8c2172..000000000 --- a/gtsam/inference/tests/testEliminationTreeObsolete.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testEliminationTree.cpp - * @brief - * @author Richard Roberts - * @date Oct 14, 2010 - */ - -#include -#include - -#include -#include - -using namespace gtsam; -using namespace std; - -class EliminationTreeOrderedTester { -public: - // build hardcoded tree - static SymbolicEliminationTreeOrdered::shared_ptr buildHardcodedTree(const SymbolicFactorGraphOrdered& fg) { - - SymbolicEliminationTreeOrdered::shared_ptr leaf0(new SymbolicEliminationTreeOrdered); - leaf0->add(fg[0]); - leaf0->add(fg[1]); - - SymbolicEliminationTreeOrdered::shared_ptr node1(new SymbolicEliminationTreeOrdered(1)); - node1->add(fg[2]); - node1->add(leaf0); - - SymbolicEliminationTreeOrdered::shared_ptr node2(new SymbolicEliminationTreeOrdered(2)); - node2->add(fg[3]); - node2->add(node1); - - SymbolicEliminationTreeOrdered::shared_ptr leaf3(new SymbolicEliminationTreeOrdered(3)); - leaf3->add(fg[4]); - - SymbolicEliminationTreeOrdered::shared_ptr etree(new SymbolicEliminationTreeOrdered(4)); - etree->add(leaf3); - etree->add(node2); - - return etree; - } -}; - -TEST(EliminationTreeOrdered, Create) -{ - // create example factor graph - SymbolicFactorGraphOrdered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - SymbolicEliminationTreeOrdered::shared_ptr expected = EliminationTreeOrderedTester::buildHardcodedTree(fg); - - // Build from factor graph - SymbolicEliminationTreeOrdered::shared_ptr actual = SymbolicEliminationTreeOrdered::Create(fg); - - CHECK(assert_equal(*expected,*actual)); -} - -/* ************************************************************************* */ -// Test to drive elimination tree development -// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -/* ************************************************************************* */ - -TEST(EliminationTreeOrdered, eliminate ) -{ - // create expected Chordal bayes Net - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(4)); - expected.push_front(boost::make_shared(3,4)); - expected.push_front(boost::make_shared(2,4)); - expected.push_front(boost::make_shared(1,2,4)); - expected.push_front(boost::make_shared(0,1,2)); - - // Create factor graph - SymbolicFactorGraphOrdered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - // eliminate - SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(EliminationTreeOrdered, disconnected_graph) { - SymbolicFactorGraphOrdered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 2); - fg.push_factor(3, 4); - - CHECK_EXCEPTION(SymbolicEliminationTreeOrdered::Create(fg), DisconnectedGraphException); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testFactorGraphObsolete.cpp b/gtsam/inference/tests/testFactorGraphObsolete.cpp deleted file mode 100644 index 8dac0e3ec..000000000 --- a/gtsam/inference/tests/testFactorGraphObsolete.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testFactorgraph.cpp - * @brief Unit tests for IndexFactor Graphs - * @author Christian Potthast - **/ - -/*STL/C++*/ -#include -#include -#include -#include -#include // for operator += -#include -using namespace boost::assign; - -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(FactorGraphOrdered, eliminateFrontals) { - - SymbolicFactorGraphOrdered sfgOrig; - sfgOrig.push_factor(0,1); - sfgOrig.push_factor(0,2); - sfgOrig.push_factor(1,3); - sfgOrig.push_factor(1,4); - sfgOrig.push_factor(2,3); - sfgOrig.push_factor(4,5); - - IndexConditionalOrdered::shared_ptr actualCond; - SymbolicFactorGraphOrdered actualSfg; - boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); - - vector condIndices; - condIndices += 0,1,2,3,4; - IndexConditionalOrdered expectedCond(condIndices, 2); - - SymbolicFactorGraphOrdered expectedSfg; - expectedSfg.push_factor(2,3); - expectedSfg.push_factor(4,5); - expectedSfg.push_factor(2,3,4); - - EXPECT(assert_equal(expectedSfg, actualSfg)); - EXPECT(assert_equal(expectedCond, *actualCond)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testISAMObsolete.cpp b/gtsam/inference/tests/testISAMObsolete.cpp deleted file mode 100644 index c57a9be2f..000000000 --- a/gtsam/inference/tests/testISAMObsolete.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testISAM.cpp - * @brief Unit tests for ISAM - * @author Michael Kaess - */ - -#include -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -typedef ISAMOrdered SymbolicISAM; - -/* ************************************************************************* */ -// Some numbers that should be consistent among all smoother tests - -//static double sigmax1 = 0.786153, sigmax2 = 0.687131 ,sigmax3 = 0.671512, -//sigmax4 = 0.669534, sigmax5 = sigmax3,, sigmax7 = sigmax1, sigmax6 = sigmax2; - -/* ************************************************************************* */ - -//// SLAM example from RSS sqrtSAM paper -//SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), -// x2(new SymbolicConditional("x2","x3")), -// x1(new SymbolicConditional("x1","x2","x3")), -// l1(new SymbolicConditional("l1","x1","x2")), -// l2(new SymbolicConditional("l2","x1","x3")); -// -//// ISAM for sqrtSAM example -//SymbolicISAM createSlamSymbolicISAM(){ -// // Create using insert -// SymbolicISAM bayesTree_slam; -// bayesTree_slam.insert(x3); -// bayesTree_slam.insert(x2); -// bayesTree_slam.insert(x1); -// bayesTree_slam.insert(l2); -// bayesTree_slam.insert(l1); -// return bayesTree_slam; -//} - -/* ************************************************************************* */ - -//// Conditionals for ASIA example from the tutorial with A and D evidence -//SymbolicConditional::shared_ptr -// B(new SymbolicConditional("B")), -// L(new SymbolicConditional("L", "B")), -// E(new SymbolicConditional("E", "B", "L")), -// S(new SymbolicConditional("S", "L", "B")), -// T(new SymbolicConditional("T", "E", "L")), -// X(new SymbolicConditional("X", "E")); -// -//// ISAM for Asia example -//SymbolicISAM createAsiaSymbolicISAM() { -// SymbolicISAM bayesTree; -// bayesTree.insert(B); -// bayesTree.insert(L); -// bayesTree.insert(E); -// bayesTree.insert(S); -// bayesTree.insert(T); -// bayesTree.insert(X); -// return bayesTree; -//} -// -///* ************************************************************************* */ -//TEST( ISAM, iSAM ) -//{ -// SymbolicISAM bayesTree = createAsiaSymbolicISAM(); -// -// // Now we modify the Bayes tree by inserting a new factor over B and S -// -// // New conditionals in modified top of the tree -// SymbolicConditional::shared_ptr -// S_(new SymbolicConditional("S")), -// B_(new SymbolicConditional("B", "S")), -// L_(new SymbolicConditional("L", "B", "S")); -// -// // Create expected Bayes tree -// SymbolicISAM expected; -// expected.insert(S_); -// expected.insert(B_); -// expected.insert(L_); -// expected.insert(E); -// expected.insert(T); -// expected.insert(X); -// -// // create new factors to be inserted -// SymbolicFactorGraph factorGraph; -// factorGraph.push_factor("B","S"); -// factorGraph.push_factor("B"); -// -// // do incremental inference -// bayesTree.update(factorGraph); -// -// // Check whether the same -// CHECK(assert_equal(expected,bayesTree)); -//} -// -///* ************************************************************************* */ -//TEST( ISAM, iSAM_slam ) -//{ -// // Create using insert -// SymbolicISAM bayesTree_slam = createSlamSymbolicISAM(); -// -// //New conditionals for the expected Bayes tree -// SymbolicConditional::shared_ptr -// l1_(new SymbolicConditional("l1","x1","x2","x3")); -// -// // Create expected Bayes tree -// SymbolicISAM expected_slam; -// expected_slam.insert(x3); -// expected_slam.insert(x2); -// expected_slam.insert(x1); -// expected_slam.insert(l1_); -// expected_slam.insert(l2); -// -// -// // create new factors to be inserted -// SymbolicFactorGraph factorGraph_slam; -// factorGraph_slam.push_factor("x3","l1"); -// factorGraph_slam.push_factor("x3"); -// -// // do incremental inference -// bayesTree_slam.update(factorGraph_slam); -// -// // Check whether the same -// CHECK(assert_equal(expected_slam,bayesTree_slam)); -//} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testJunctionTreeObsolete.cpp b/gtsam/inference/tests/testJunctionTreeObsolete.cpp deleted file mode 100644 index b64d596f5..000000000 --- a/gtsam/inference/tests/testJunctionTreeObsolete.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testJunctionTree.cpp - * @brief Unit tests for Junction Tree - * @author Kai Ni - * @author Frank Dellaert - */ - -#include // for operator += -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include - -#include -#include -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; - -typedef JunctionTreeOrdered SymbolicJunctionTree; - -/* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - ****************************************************************************/ -TEST( JunctionTreeOrdered, constructor ) -{ - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraphOrdered fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); - - SymbolicJunctionTree actual(fg); - - vector frontal1; frontal1 += x3, x4; - vector frontal2; frontal2 += x2, x1; - vector sep1; - vector sep2; sep2 += x3; - CHECK(assert_equal(frontal1, actual.root()->frontal)); - CHECK(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(1, actual.root()->size()); - CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal)); - CHECK(assert_equal(sep2, actual.root()->children().front()->separator)); - LONGS_EQUAL(2, actual.root()->children().front()->size()); - CHECK(assert_equal(*fg[2], *(*actual.root())[0])); - CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0])); - CHECK(assert_equal(*fg[1], *(*actual.root()->children().front())[1])); -} - -/* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - ****************************************************************************/ -TEST( JunctionTreeOrdered, eliminate) -{ - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraphOrdered fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); - - SymbolicJunctionTree jt(fg); - SymbolicBayesTreeOrdered::sharedClique actual = jt.eliminate(&EliminateSymbolic); - - BayesNetOrdered bn(*SymbolicSequentialSolver(fg).eliminate()); - SymbolicBayesTreeOrdered expected(bn); - -// cout << "BT from JT:\n"; -// actual->printTree(""); - - CHECK(assert_equal(*expected.root(), *actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testPermutationObsolete.cpp b/gtsam/inference/tests/testPermutationObsolete.cpp deleted file mode 100644 index a14fa7a43..000000000 --- a/gtsam/inference/tests/testPermutationObsolete.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testPermutation.cpp - * @date Sep 22, 2011 - * @author richard - */ - -#include - -#include -#include - -#include - -using namespace gtsam; -using namespace std; - -/* ************************************************************************* */ -TEST(Permutation, Identity) { - Permutation expected(5); - expected[0] = 0; - expected[1] = 1; - expected[2] = 2; - expected[3] = 3; - expected[4] = 4; - - Permutation actual = Permutation::Identity(5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, PullToFront) { - Permutation expected(5); - expected[0] = 4; - expected[1] = 0; - expected[2] = 2; - expected[3] = 1; - expected[4] = 3; - - std::vector toFront; - toFront.push_back(4); - toFront.push_back(0); - toFront.push_back(2); - Permutation actual = Permutation::PullToFront(toFront, 5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, PushToBack) { - Permutation expected(5); - expected[0] = 1; - expected[1] = 3; - expected[2] = 4; - expected[3] = 0; - expected[4] = 2; - - std::vector toBack; - toBack.push_back(4); - toBack.push_back(0); - toBack.push_back(2); - Permutation actual = Permutation::PushToBack(toBack, 5); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(Permutation, compose) { - Permutation p1(5); - p1[0] = 1; - p1[1] = 2; - p1[2] = 3; - p1[3] = 4; - p1[4] = 0; - - Permutation p2(5); - p2[0] = 1; - p2[1] = 2; - p2[2] = 4; - p2[3] = 3; - p2[4] = 0; - - Permutation expected(5); - expected[0] = 2; - expected[1] = 3; - expected[2] = 0; - expected[3] = 4; - expected[4] = 1; - - Permutation actual = *p1.permute(p2); - - EXPECT(assert_equal(expected, actual)); - LONGS_EQUAL(p1[p2[0]], actual[0]); - LONGS_EQUAL(p1[p2[1]], actual[1]); - LONGS_EQUAL(p1[p2[2]], actual[2]); - LONGS_EQUAL(p1[p2[3]], actual[3]); - LONGS_EQUAL(p1[p2[4]], actual[4]); -} - -/* ************************************************************************* */ -TEST(Reduction, CreateFromPartialPermutation) { - Permutation selector(3); - selector[0] = 2; - selector[1] = 4; - selector[2] = 6; - Permutation p(3); - p[0] = 2; - p[1] = 0; - p[2] = 1; - - internal::Reduction expected; - expected.insert(make_pair(2,6)); - expected.insert(make_pair(4,2)); - expected.insert(make_pair(6,4)); - - internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } - - diff --git a/gtsam/inference/tests/testSymbolicBayesNetObsolete.cpp b/gtsam/inference/tests/testSymbolicBayesNetObsolete.cpp deleted file mode 100644 index 0ce99c1b8..000000000 --- a/gtsam/inference/tests/testSymbolicBayesNetObsolete.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesNet.cpp - * @brief Unit tests for a symbolic Bayes chain - * @author Frank Dellaert - */ - -#include // for 'insert()' -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index _L_ = 0; -static const Index _A_ = 1; -static const Index _B_ = 2; -static const Index _C_ = 3; -static const Index _D_ = 4; -static const Index _E_ = 5; - -static IndexConditionalOrdered::shared_ptr - B(new IndexConditionalOrdered(_B_)), - L(new IndexConditionalOrdered(_L_, _B_)); - -/* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, equals ) -{ - BayesNetOrdered f1; - f1.push_back(B); - f1.push_back(L); - BayesNetOrdered f2; - f2.push_back(L); - f2.push_back(B); - CHECK(f1.equals(f1)); - CHECK(!f1.equals(f2)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, pop_front ) -{ - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_,_B_,_C_)), - B(new IndexConditionalOrdered(_B_,_C_)), - C(new IndexConditionalOrdered(_C_)); - - // Expected after pop_front - BayesNetOrdered expected; - expected.push_back(B); - expected.push_back(C); - - // Actual - BayesNetOrdered actual; - actual.push_back(A); - actual.push_back(B); - actual.push_back(C); - actual.pop_front(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, combine ) -{ - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_,_B_,_C_)), - B(new IndexConditionalOrdered(_B_,_C_)), - C(new IndexConditionalOrdered(_C_)); - - // p(A|BC) - BayesNetOrdered p_ABC; - p_ABC.push_back(A); - - // P(BC)=P(B|C)P(C) - BayesNetOrdered p_BC; - p_BC.push_back(B); - p_BC.push_back(C); - - // P(ABC) = P(A|BC) P(BC) - p_ABC.push_back(p_BC); - - BayesNetOrdered expected; - expected.push_back(A); - expected.push_back(B); - expected.push_back(C); - - CHECK(assert_equal(expected,p_ABC)); -} - -/* ************************************************************************* */ -TEST(SymbolicBayesNetOrdered, find) { - SymbolicBayesNetOrdered bn; - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_A_, _B_)); - std::vector keys; - keys.push_back(_B_); - keys.push_back(_C_); - keys.push_back(_D_); - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(keys,2)); - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_D_)); - - SymbolicBayesNetOrdered::iterator expected = bn.begin(); ++ expected; - SymbolicBayesNetOrdered::iterator actual = bn.find(_C_); - EXPECT(assert_equal(**expected, **actual)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(SymbolicBayesNetOrdered, popLeaf) { - IndexConditionalOrdered::shared_ptr - A(new IndexConditionalOrdered(_A_,_E_)), - B(new IndexConditionalOrdered(_B_,_E_)), - C(new IndexConditionalOrdered(_C_,_D_)), - D(new IndexConditionalOrdered(_D_,_E_)), - E(new IndexConditionalOrdered(_E_)); - - // BayesNet after popping A - SymbolicBayesNetOrdered expected1; - expected1 += B, C, D, E; - - // BayesNet after popping C - SymbolicBayesNetOrdered expected2; - expected2 += A, B, D, E; - - // BayesNet after popping C and D - SymbolicBayesNetOrdered expected3; - expected3 += A, B, E; - - // BayesNet after popping C and A - SymbolicBayesNetOrdered expected4; - expected4 += B, D, E; - - - // BayesNet after popping A - SymbolicBayesNetOrdered actual1; - actual1 += A, B, C, D, E; - actual1.popLeaf(actual1.find(_A_)); - - // BayesNet after popping C - SymbolicBayesNetOrdered actual2; - actual2 += A, B, C, D, E; - actual2.popLeaf(actual2.find(_C_)); - - // BayesNet after popping C and D - SymbolicBayesNetOrdered actual3; - actual3 += A, B, C, D, E; - actual3.popLeaf(actual3.find(_C_)); - actual3.popLeaf(actual3.find(_D_)); - - // BayesNet after popping C and A - SymbolicBayesNetOrdered actual4; - actual4 += A, B, C, D, E; - actual4.popLeaf(actual4.find(_C_)); - actual4.popLeaf(actual4.find(_A_)); - - EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(expected2, actual2)); - EXPECT(assert_equal(expected3, actual3)); - EXPECT(assert_equal(expected4, actual4)); - - // Try to remove a non-leaf node (this test is not working in non-debug mode) -//#undef NDEBUG_SAVED -//#ifdef NDEBUG -//#define NDEBUG_SAVED -//#endif -// -//#undef NDEBUG -// SymbolicBayesNetOrdered actual5; -// actual5 += A, B, C, D, E; -// CHECK_EXCEPTION(actual5.popLeaf(actual5.find(_D_)), std::invalid_argument); -// -//#ifdef NDEBUG_SAVED -//#define NDEBUG -//#endif -} - -/* ************************************************************************* */ -TEST(SymbolicBayesNetOrdered, saveGraph) { - SymbolicBayesNetOrdered bn; - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_A_, _B_)); - std::vector keys; - keys.push_back(_B_); - keys.push_back(_C_); - keys.push_back(_D_); - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(keys,2)); - bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_D_)); - - bn.saveGraph("SymbolicBayesNet.dot"); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicBayesTreeObsolete.cpp b/gtsam/inference/tests/testSymbolicBayesTreeObsolete.cpp deleted file mode 100644 index dcc877d9b..000000000 --- a/gtsam/inference/tests/testSymbolicBayesTreeObsolete.cpp +++ /dev/null @@ -1,323 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesTree.cpp - * @date sept 15, 2012 - * @author Frank Dellaert - */ - -#include -#include -#include - -#include -#include -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -static bool debug = false; - -/* ************************************************************************* */ - -TEST_UNSAFE( SymbolicBayesTreeOrdered, thinTree ) { - - // create a thin-tree Bayesnet, a la Jean-Guillaume - SymbolicBayesNetOrdered bayesNet; - bayesNet.push_front(boost::make_shared(14)); - - bayesNet.push_front(boost::make_shared(13, 14)); - bayesNet.push_front(boost::make_shared(12, 14)); - - bayesNet.push_front(boost::make_shared(11, 13, 14)); - bayesNet.push_front(boost::make_shared(10, 13, 14)); - bayesNet.push_front(boost::make_shared(9, 12, 14)); - bayesNet.push_front(boost::make_shared(8, 12, 14)); - - bayesNet.push_front(boost::make_shared(7, 11, 13)); - bayesNet.push_front(boost::make_shared(6, 11, 13)); - bayesNet.push_front(boost::make_shared(5, 10, 13)); - bayesNet.push_front(boost::make_shared(4, 10, 13)); - - bayesNet.push_front(boost::make_shared(3, 9, 12)); - bayesNet.push_front(boost::make_shared(2, 9, 12)); - bayesNet.push_front(boost::make_shared(1, 8, 12)); - bayesNet.push_front(boost::make_shared(0, 8, 12)); - - if (debug) { - GTSAM_PRINT(bayesNet); - bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); - } - - // create a BayesTree out of a Bayes net - SymbolicBayesTreeOrdered bayesTree(bayesNet); - if (debug) { - GTSAM_PRINT(bayesTree); - bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTreeOrdered::Clique::shared_ptr R = bayesTree.root(); - - { - // check shortcut P(S9||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[9]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S8||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[8]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S4||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[4]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(10, 13, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S2||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[2]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(12, 14)); - expected.push_front(boost::make_shared(9, 12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S0||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[0]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(12, 14)); - expected.push_front(boost::make_shared(8, 12, 14)); - EXPECT(assert_equal(expected, shortcut)); - } - - SymbolicBayesNetOrdered::shared_ptr actualJoint; - - // Check joint P(8,2) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(8)); - expected.push_front(boost::make_shared(2, 8)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(1,2) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(2)); - expected.push_front(boost::make_shared(1, 2)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(2,6) - if (true) { - actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(6)); - expected.push_front(boost::make_shared(2, 6)); - EXPECT(assert_equal(expected, *actualJoint)); - } - - // Check joint P(4,6) - if (false) { // TODO, not disjoint - actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(6)); - expected.push_front(boost::make_shared(4, 6)); - EXPECT(assert_equal(expected, *actualJoint)); - } -} - -/* ************************************************************************* * - Bayes tree for smoother with "natural" ordering: - C1 5 6 - C2 4 : 5 - C3 3 : 4 - C4 2 : 3 - C5 1 : 2 - C6 0 : 1 - **************************************************************************** */ - -TEST_UNSAFE( SymbolicBayesTreeOrdered, linear_smoother_shortcuts ) { - // Create smoother with 7 nodes - SymbolicFactorGraphOrdered smoother; - smoother.push_factor(0); - smoother.push_factor(0, 1); - smoother.push_factor(1, 2); - smoother.push_factor(2, 3); - smoother.push_factor(3, 4); - smoother.push_factor(4, 5); - smoother.push_factor(5, 6); - - BayesNetOrdered bayesNet = - *SymbolicSequentialSolver(smoother).eliminate(); - - if (debug) { - GTSAM_PRINT(bayesNet); - bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); - } - - // create a BayesTree out of a Bayes net - SymbolicBayesTreeOrdered bayesTree(bayesNet); - if (debug) { - GTSAM_PRINT(bayesTree); - bayesTree.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTreeOrdered::Clique::shared_ptr R = bayesTree.root(); - - { - // check shortcut P(S2||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S3||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(4, 5)); - EXPECT(assert_equal(expected, shortcut)); - } - - { - // check shortcut P(S4||R) to root - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(3, 5)); - EXPECT(assert_equal(expected, shortcut)); - } -} - -/* ************************************************************************* */ -// from testSymbolicJunctionTree, which failed at one point -TEST(SymbolicBayesTreeOrdered, complicatedMarginal) { - - // Create the conditionals to go in the BayesTree - list L; - L = list_of(1)(2)(5); - IndexConditionalOrdered::shared_ptr R_1_2(new IndexConditionalOrdered(L, 2)); - L = list_of(3)(4)(6); - IndexConditionalOrdered::shared_ptr R_3_4(new IndexConditionalOrdered(L, 2)); - L = list_of(5)(6)(7)(8); - IndexConditionalOrdered::shared_ptr R_5_6(new IndexConditionalOrdered(L, 2)); - L = list_of(7)(8)(11); - IndexConditionalOrdered::shared_ptr R_7_8(new IndexConditionalOrdered(L, 2)); - L = list_of(9)(10)(11)(12); - IndexConditionalOrdered::shared_ptr R_9_10(new IndexConditionalOrdered(L, 2)); - L = list_of(11)(12); - IndexConditionalOrdered::shared_ptr R_11_12(new IndexConditionalOrdered(L, 2)); - - // Symbolic Bayes Tree - typedef SymbolicBayesTreeOrdered::Clique Clique; - typedef SymbolicBayesTreeOrdered::sharedClique sharedClique; - - // Create Bayes Tree - SymbolicBayesTreeOrdered bt; - bt.insert(sharedClique(new Clique(R_11_12))); - bt.insert(sharedClique(new Clique(R_9_10))); - bt.insert(sharedClique(new Clique(R_7_8))); - bt.insert(sharedClique(new Clique(R_5_6))); - bt.insert(sharedClique(new Clique(R_3_4))); - bt.insert(sharedClique(new Clique(R_1_2))); - if (debug) { - GTSAM_PRINT(bt); - bt.saveGraph("/tmp/symbolicBayesTree.dot"); - } - - SymbolicBayesTreeOrdered::Clique::shared_ptr R = bt.root(); - SymbolicBayesNetOrdered empty; - - // Shortcut on 9 - { - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[9]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - EXPECT(assert_equal(empty, shortcut)); - } - - // Shortcut on 7 - { - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[7]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - EXPECT(assert_equal(empty, shortcut)); - } - - // Shortcut on 5 - { - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[5]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(8, 11)); - expected.push_front(boost::make_shared(7, 8, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Shortcut on 3 - { - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[3]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(6, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Shortcut on 1 - { - SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[1]; - SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(5, 11)); - EXPECT(assert_equal(expected, shortcut)); - } - - // Marginal on 5 - { - IndexFactorOrdered::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic); - EXPECT(assert_equal(IndexFactorOrdered(5), *actual, 1e-1)); - } - - // Shortcut on 6 - { - IndexFactorOrdered::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic); - EXPECT(assert_equal(IndexFactorOrdered(6), *actual, 1e-1)); - } - -} -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/inference/tests/testSymbolicFactorGraphObsolete.cpp b/gtsam/inference/tests/testSymbolicFactorGraphObsolete.cpp deleted file mode 100644 index 403cbf5f8..000000000 --- a/gtsam/inference/tests/testSymbolicFactorGraphObsolete.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicFactorGraph.cpp - * @brief Unit tests for a symbolic IndexFactor Graph - * @author Frank Dellaert - */ - -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index vx2 = 0; -static const Index vx1 = 1; -static const Index vl1 = 2; - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraphOrdered, EliminateOne ) -//{ -// // create a test graph -// SymbolicFactorGraphOrdered fg; -// fg.push_factor(vx2, vx1); -// -// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1); -// SymbolicFactorGraphOrdered expected; -// expected.push_back(boost::shared_ptr()); -// expected.push_factor(vx1); -// -// CHECK(assert_equal(expected, fg)); -//} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, constructFromBayesNet ) -{ - // create expected factor graph - SymbolicFactorGraphOrdered expected; - expected.push_factor(vx2, vx1, vl1); - expected.push_factor(vx1, vl1); - expected.push_factor(vx1); - - // create Bayes Net - IndexConditionalOrdered::shared_ptr x2(new IndexConditionalOrdered(vx2, vx1, vl1)); - IndexConditionalOrdered::shared_ptr l1(new IndexConditionalOrdered(vx1, vl1)); - IndexConditionalOrdered::shared_ptr x1(new IndexConditionalOrdered(vx1)); - - BayesNetOrdered bayesNet; - bayesNet.push_back(x2); - bayesNet.push_back(l1); - bayesNet.push_back(x1); - - // create actual factor graph from a Bayes Net - SymbolicFactorGraphOrdered actual(bayesNet); - - CHECK(assert_equal((SymbolicFactorGraphOrdered)expected,actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, push_back ) -{ - // Create two factor graphs and expected combined graph - SymbolicFactorGraphOrdered fg1, fg2, expected; - - fg1.push_factor(vx1); - fg1.push_factor(vx2, vx1); - - fg2.push_factor(vx1, vl1); - fg2.push_factor(vx2, vl1); - - expected.push_factor(vx1); - expected.push_factor(vx2, vx1); - expected.push_factor(vx1, vl1); - expected.push_factor(vx2, vl1); - - // combine - SymbolicFactorGraphOrdered actual = combine(fg1, fg2); - CHECK(assert_equal(expected, actual)); - - // combine using push_back - fg1.push_back(fg2); - CHECK(assert_equal(expected, fg1)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicFactorObsolete.cpp b/gtsam/inference/tests/testSymbolicFactorObsolete.cpp deleted file mode 100644 index 80e1eb83b..000000000 --- a/gtsam/inference/tests/testSymbolicFactorObsolete.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicFactor.cpp - * @brief Unit tests for a symbolic IndexFactor - * @author Frank Dellaert - */ - -#include -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; - -/* ************************************************************************* */ -TEST(SymbolicFactor, constructor) { - - // Frontals sorted, parents not sorted - vector keys1; keys1 += 3, 4, 5, 9, 7, 8; - (void)IndexConditionalOrdered(keys1, 3); - -// // Frontals not sorted -// vector keys2; keys2 += 3, 5, 4, 9, 7, 8; -// (void)IndexConditionalOrdered::FromRange(keys2.begin(), keys2.end(), 3); - -// // Frontals not before parents -// vector keys3; keys3 += 3, 4, 5, 1, 7, 8; -// (void)IndexConditionalOrdered::FromRange(keys3.begin(), keys3.end(), 3); -} - -/* ************************************************************************* */ -#ifdef TRACK_ELIMINATE -TEST(SymbolicFactor, eliminate) { - vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; - IndexFactorOrdered actual(keys.begin(), keys.end()); - BayesNetOrdered fragment = *actual.eliminate(3); - - IndexFactorOrdered expected(keys.begin()+3, keys.end()); - IndexConditionalOrdered::shared_ptr expected0 = IndexConditionalOrdered::FromRange(keys.begin(), keys.end(), 1); - IndexConditionalOrdered::shared_ptr expected1 = IndexConditionalOrdered::FromRange(keys.begin()+1, keys.end(), 1); - IndexConditionalOrdered::shared_ptr expected2 = IndexConditionalOrdered::FromRange(keys.begin()+2, keys.end(), 1); - - CHECK(assert_equal(fragment.size(), size_t(3))); - CHECK(assert_equal(expected, actual)); - BayesNetOrdered::const_iterator fragmentCond = fragment.begin(); - CHECK(assert_equal(**fragmentCond++, *expected0)); - CHECK(assert_equal(**fragmentCond++, *expected1)); - CHECK(assert_equal(**fragmentCond++, *expected2)); -} -#endif -/* ************************************************************************* */ -TEST(SymbolicFactor, EliminateSymbolic) { - SymbolicFactorGraphOrdered factors; - factors.push_factor(2,4,6); - factors.push_factor(1,2,5); - factors.push_factor(0,3); - - IndexFactorOrdered expectedFactor(4,5,6); - std::vector keys; keys += 0,1,2,3,4,5,6; - IndexConditionalOrdered::shared_ptr expectedConditional(new IndexConditionalOrdered(keys, 4)); - - IndexFactorOrdered::shared_ptr actualFactor; - IndexConditionalOrdered::shared_ptr actualConditional; - boost::tie(actualConditional, actualFactor) = EliminateSymbolic(factors, 4); - - CHECK(assert_equal(*expectedConditional, *actualConditional)); - CHECK(assert_equal(expectedFactor, *actualFactor)); - -// BayesNetOrdered expected_bn; -// vector parents; -// -// parents.clear(); parents += 1,2,3,4,5,6; -// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(0, parents))); -// -// parents.clear(); parents += 2,3,4,5,6; -// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(1, parents))); -// -// parents.clear(); parents += 3,4,5,6; -// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(2, parents))); -// -// parents.clear(); parents += 4,5,6; -// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(3, parents))); -// -// BayesNetOrdered::shared_ptr actual_bn; -// IndexFactor::shared_ptr actual_factor; -// boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4); -// -// CHECK(assert_equal(expected_bn, *actual_bn)); -// CHECK(assert_equal(expected_factor, *actual_factor)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbolicSequentialSolverObsolete.cpp b/gtsam/inference/tests/testSymbolicSequentialSolverObsolete.cpp deleted file mode 100644 index abcad74cf..000000000 --- a/gtsam/inference/tests/testSymbolicSequentialSolverObsolete.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicSequentialSolver.cpp - * @brief Unit tests for a symbolic sequential solver routines - * @author Frank Dellaert - * @date Sept 16, 2012 - */ - -#include - -#include - -#include // for operator += -using namespace boost::assign; - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ - -TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) { - // create factor graph - SymbolicFactorGraphOrdered g; - g.push_factor(2, 1, 0); - g.push_factor(2, 0); - g.push_factor(2); - // test solver is Testable - SymbolicSequentialSolver solver(g); -// GTSAM_PRINT(solver); - EXPECT(assert_equal(solver,solver)); -} - -/* ************************************************************************* */ - -TEST( SymbolicSequentialSolver, inference ) { - // Create factor graph - SymbolicFactorGraphOrdered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - // eliminate - SymbolicSequentialSolver solver(fg); - SymbolicBayesNetOrdered::shared_ptr actual = solver.eliminate(); - SymbolicBayesNetOrdered expected; - expected.push_front(boost::make_shared(4)); - expected.push_front(boost::make_shared(3, 4)); - expected.push_front(boost::make_shared(2, 4)); - expected.push_front(boost::make_shared(1, 2, 4)); - expected.push_front(boost::make_shared(0, 1, 2)); - EXPECT(assert_equal(expected,*actual)); - - { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(4); - js.push_back(3); - SymbolicBayesNetOrdered::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNetOrdered expectedBN; - expectedBN.push_front(boost::make_shared(3)); - expectedBN.push_front(boost::make_shared(4, 3)); - expectedBN.push_front(boost::make_shared(0, 4)); - EXPECT( assert_equal(expectedBN,*actualBN)); - - // jointFactorGraph - SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraphOrdered expectedFG; - expectedFG.push_factor(0, 4); - expectedFG.push_factor(4, 3); - expectedFG.push_factor(3); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG))); - } - - { - // jointBayesNet - vector js; - js.push_back(0); - js.push_back(2); - js.push_back(3); - SymbolicBayesNetOrdered::shared_ptr actualBN = solver.jointBayesNet(js); - SymbolicBayesNetOrdered expectedBN; - expectedBN.push_front(boost::make_shared(2)); - expectedBN.push_front(boost::make_shared(3, 2)); - expectedBN.push_front(boost::make_shared(0, 3, 2)); - EXPECT( assert_equal(expectedBN,*actualBN)); - - // jointFactorGraph - SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js); - SymbolicFactorGraphOrdered expectedFG; - expectedFG.push_factor(0, 3, 2); - expectedFG.push_factor(3, 2); - expectedFG.push_factor(2); - EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG))); - } - - { - // conditionalBayesNet - vector js; - js.push_back(0); - js.push_back(2); - js.push_back(3); - size_t nrFrontals = 2; - SymbolicBayesNetOrdered::shared_ptr actualBN = // - solver.conditionalBayesNet(js, nrFrontals); - SymbolicBayesNetOrdered expectedBN; - expectedBN.push_front(boost::make_shared(2, 3)); - expectedBN.push_front(boost::make_shared(0, 2, 3)); - EXPECT( assert_equal(expectedBN,*actualBN)); - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testVariableIndexObsolete.cpp b/gtsam/inference/tests/testVariableIndexObsolete.cpp deleted file mode 100644 index a844879ad..000000000 --- a/gtsam/inference/tests/testVariableIndexObsolete.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testVariableIndex.cpp - * @brief - * @author Richard Roberts - * @date Sep 26, 2010 - */ - -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(VariableIndexOrdered, augment) { - - SymbolicFactorGraphOrdered fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); - - SymbolicFactorGraphOrdered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); - - VariableIndexOrdered expected(fgCombined); - VariableIndexOrdered actual(fg1); - actual.augment(fg2); - - LONGS_EQUAL(16, actual.nEntries()); - LONGS_EQUAL(8, actual.nFactors()); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(VariableIndexOrdered, remove) { - - SymbolicFactorGraphOrdered fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); - - SymbolicFactorGraphOrdered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); - - // Create a factor graph containing only the factors from fg2 and with null - // factors in the place of those of fg1, so that the factor indices are correct. - SymbolicFactorGraphOrdered fg2removed(fgCombined); - fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); - - // The expected VariableIndex has the same factor indices as fgCombined but - // with entries from fg1 removed, and still has all 10 variables. - VariableIndexOrdered expected(fg2removed, 10); - VariableIndexOrdered actual(fgCombined); - vector indices; - indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - actual.remove(indices, fg1); - - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(VariableIndexOrdered, deep_copy) { - - SymbolicFactorGraphOrdered fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); - - // Create original graph and VariableIndex - SymbolicFactorGraphOrdered fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); - VariableIndexOrdered original(fgOriginal); - VariableIndexOrdered expectedOriginal(fgOriginal); - - // Create a factor graph containing only the factors from fg2 and with null - // factors in the place of those of fg1, so that the factor indices are correct. - SymbolicFactorGraphOrdered fg2removed(fgOriginal); - fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); - VariableIndexOrdered expectedRemoved(fg2removed); - - // Create a clone and modify the clone - the original should not change - VariableIndexOrdered clone(original); - vector indices; - indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - clone.remove(indices, fg1); - - // When modifying the clone, the original should have stayed the same - EXPECT(assert_equal(expectedOriginal, original)); - EXPECT(assert_equal(expectedRemoved, clone)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNetOrdered.cpp b/gtsam/linear/GaussianBayesNetOrdered.cpp deleted file mode 100644 index eaa7edb0f..000000000 --- a/gtsam/linear/GaussianBayesNetOrdered.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianBayesNet.cpp - * @brief Chordal Bayes Net, the result of eliminating a factor graph - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) - -namespace gtsam { - -// Explicitly instantiate so we don't have to include everywhere -template class BayesNetOrdered; - -/* ************************************************************************* */ -GaussianBayesNetOrdered scalarGaussian(Index key, double mu, double sigma) { - GaussianBayesNetOrdered bn; - GaussianConditionalOrdered::shared_ptr - conditional(new GaussianConditionalOrdered(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1))); - bn.push_back(conditional); - return bn; -} - -/* ************************************************************************* */ -GaussianBayesNetOrdered simpleGaussian(Index key, const Vector& mu, double sigma) { - GaussianBayesNetOrdered bn; - size_t n = mu.size(); - GaussianConditionalOrdered::shared_ptr - conditional(new GaussianConditionalOrdered(key, mu/sigma, eye(n)/sigma, ones(n))); - bn.push_back(conditional); - return bn; -} - -/* ************************************************************************* */ -void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Vector sigmas) { - GaussianConditionalOrdered::shared_ptr cg(new GaussianConditionalOrdered(key, d, R, name1, S, sigmas)); - bn.push_front(cg); -} - -/* ************************************************************************* */ -void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) { - GaussianConditionalOrdered::shared_ptr cg(new GaussianConditionalOrdered(key, d, R, name1, S, name2, T, sigmas)); - bn.push_front(cg); -} - -/* ************************************************************************* */ -boost::shared_ptr allocateVectorValues(const GaussianBayesNetOrdered& bn) { - vector dimensions(bn.size()); - Index var = 0; - BOOST_FOREACH(const boost::shared_ptr conditional, bn) { - dimensions[var++] = conditional->dim(); - } - return boost::shared_ptr(new VectorValuesOrdered(dimensions)); -} - -/* ************************************************************************* */ -VectorValuesOrdered optimize(const GaussianBayesNetOrdered& bn) { - VectorValuesOrdered x = *allocateVectorValues(bn); - optimizeInPlace(bn, x); - return x; -} - -/* ************************************************************************* */ -// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) -void optimizeInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& x) { - /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { - // i^th part of R*x=y, x=inv(R)*y - // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) - cg->solveInPlace(x); - } -} - -/* ************************************************************************* */ -VectorValuesOrdered backSubstitute(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& input) { - VectorValuesOrdered output = input; - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { - const Index key = *(cg->beginFrontals()); - Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); - xS = input[key] - cg->get_S() * xS; - output[key] = cg->get_R().triangularView().solve(xS); - } - - BOOST_FOREACH(const boost::shared_ptr cg, bn) { - cg->scaleFrontalsBySigma(output); - } - - return output; -} - - -/* ************************************************************************* */ -// gy=inv(L)*gx by solving L*gy=gx. -// gy=inv(R'*inv(Sigma))*gx -// gz'*R'=gx', gy = gz.*sigmas -VectorValuesOrdered backSubstituteTranspose(const GaussianBayesNetOrdered& bn, - const VectorValuesOrdered& gx) { - - // Initialize gy from gx - // TODO: used to insert zeros if gx did not have an entry for a variable in bn - VectorValuesOrdered gy = gx; - - // we loop from first-eliminated to last-eliminated - // i^th part of L*gy=gx is done block-column by block-column of L - BOOST_FOREACH(const boost::shared_ptr cg, bn) - cg->solveTransposeInPlace(gy); - - // Scale gy - BOOST_FOREACH(GaussianConditionalOrdered::shared_ptr cg, bn) - cg->scaleFrontalsBySigma(gy); - - return gy; -} - -/* ************************************************************************* */ -VectorValuesOrdered optimizeGradientSearch(const GaussianBayesNetOrdered& Rd) { - gttic(Allocate_VectorValues); - VectorValuesOrdered grad = *allocateVectorValues(Rd); - gttoc(Allocate_VectorValues); - - optimizeGradientSearchInPlace(Rd, grad); - - return grad; -} - -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const GaussianBayesNetOrdered& Rd, VectorValuesOrdered& grad) { - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_Rg); - // Compute R * g - FactorGraphOrdered Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - gttoc(Compute_Rg); - - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - gttoc(Compute_minimizing_step_size); - - gttic(Compute_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} - -/* ************************************************************************* */ -pair matrix(const GaussianBayesNetOrdered& bn) { - - // add the dimensions of all variables to get matrix dimension - // and at the same time create a mapping from keys to indices - size_t N=0; map mapping; - BOOST_FOREACH(GaussianConditionalOrdered::shared_ptr cg,bn) { - GaussianConditionalOrdered::const_iterator it = cg->beginFrontals(); - for (; it != cg->endFrontals(); ++it) { - mapping.insert(make_pair(*it,N)); - N += cg->dim(it); - } - } - - // create matrix and copy in values - Matrix R = zeros(N,N); - Vector d(N); - Index key; size_t I; - FOREACH_PAIR(key,I,mapping) { - // find corresponding conditional - boost::shared_ptr cg = bn[key]; - - // get sigmas - Vector sigmas = cg->get_sigmas(); - - // get RHS and copy to d - GaussianConditionalOrdered::const_d_type d_ = cg->get_d(); - const size_t n = d_.size(); - for (size_t i=0;iget_R(); - for (size_t i=0;ibeginParents(); - for (; keyS!=cg->endParents(); keyS++) { - Matrix S = cg->get_S(keyS); // get S matrix - const size_t m = S.rows(), n = S.cols(); // find S size - const size_t J = mapping[*keyS]; // find column index - for (size_t i=0;i cg, bayesNet){ - logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); - } - - return exp(logDet); -} - -/* ************************************************************************* */ -VectorValuesOrdered gradient(const GaussianBayesNetOrdered& bayesNet, const VectorValuesOrdered& x0) { - return gradient(GaussianFactorGraphOrdered(bayesNet), x0); -} - -/* ************************************************************************* */ -void gradientAtZero(const GaussianBayesNetOrdered& bayesNet, VectorValuesOrdered& g) { - gradientAtZero(GaussianFactorGraphOrdered(bayesNet), g); -} - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNetOrdered.h b/gtsam/linear/GaussianBayesNetOrdered.h deleted file mode 100644 index f95565da4..000000000 --- a/gtsam/linear/GaussianBayesNetOrdered.h +++ /dev/null @@ -1,161 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianBayesNet.h - * @brief Chordal Bayes Net, the result of eliminating a factor graph - * @brief GaussianBayesNet - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /** A Bayes net made from linear-Gaussian densities */ - typedef BayesNetOrdered GaussianBayesNetOrdered; - - /** Create a scalar Gaussian */ - GTSAM_EXPORT GaussianBayesNetOrdered scalarGaussian(Index key, double mu=0.0, double sigma=1.0); - - /** Create a simple Gaussian on a single multivariate variable */ - GTSAM_EXPORT GaussianBayesNetOrdered simpleGaussian(Index key, const Vector& mu, double sigma=1.0); - - /** - * Add a conditional node with one parent - * |Rx+Sy-d| - */ - GTSAM_EXPORT void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Vector sigmas); - - /** - * Add a conditional node with two parents - * |Rx+Sy+Tz-d| - */ - GTSAM_EXPORT void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R, - Index name1, Matrix S, Index name2, Matrix T, Vector sigmas); - - /** - * Allocate a VectorValues for the variables in a BayesNet - */ - GTSAM_EXPORT boost::shared_ptr allocateVectorValues(const GaussianBayesNetOrdered& bn); - - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution. - */ - GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianBayesNetOrdered& bn); - - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution, writes the solution \f$ x \f$ into a pre-allocated - * VectorValues. You can use allocateVectorValues(const GaussianBayesNet&) - * allocate it. See also optimize(const GaussianBayesNet&), which does not - * require pre-allocation. - */ - GTSAM_EXPORT void optimizeInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& x); - - /** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - * - * @param bn The GaussianBayesNet on which to perform this computation - * @return The resulting \f$ \delta x \f$ as described above - */ - GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const GaussianBayesNetOrdered& bn); - - /** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad - * - * @param bn The GaussianBayesNet on which to perform this computation - * @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&) - * */ - GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& grad); - - /** - * Backsubstitute - * gy=inv(R*inv(Sigma))*gx - */ - GTSAM_EXPORT VectorValuesOrdered backSubstitute(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& gx); - - /** - * Transpose Backsubstitute - * gy=inv(L)*gx by solving L*gy=gx. - * gy=inv(R'*inv(Sigma))*gx - * gz'*R'=gx', gy = gz.*sigmas - */ - GTSAM_EXPORT VectorValuesOrdered backSubstituteTranspose(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& gx); - - /** - * Return (dense) upper-triangular matrix representation - */ - GTSAM_EXPORT std::pair matrix(const GaussianBayesNetOrdered&); - - /** - * Computes the determinant of a GassianBayesNet - * A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements and take the exponent at the end - * because this is more numerically stable. - * @param bayesNet The input GaussianBayesNet - * @return The determinant - */ - GTSAM_EXPORT double determinant(const GaussianBayesNetOrdered& bayesNet); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * @param bayesNet The Gaussian Bayes net $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianBayesNetOrdered& bayesNet, const VectorValuesOrdered& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * @param bayesNet The Gaussian Bayes net $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT void gradientAtZero(const GaussianBayesNetOrdered& bayesNet, VectorValuesOrdered& g); - -} /// namespace gtsam diff --git a/gtsam/linear/GaussianBayesTreeOrdered.cpp b/gtsam/linear/GaussianBayesTreeOrdered.cpp deleted file mode 100644 index abaef4a76..000000000 --- a/gtsam/linear/GaussianBayesTreeOrdered.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianBayesTree.cpp - * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree - * @brief GaussianBayesTree - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -VectorValuesOrdered optimize(const GaussianBayesTreeOrdered& bayesTree) { - VectorValuesOrdered result = *allocateVectorValues(bayesTree); - optimizeInPlace(bayesTree, result); - return result; -} - -/* ************************************************************************* */ -void optimizeInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& result) { - internal::optimizeInPlace(bayesTree.root(), result); -} - -/* ************************************************************************* */ -VectorValuesOrdered optimizeGradientSearch(const GaussianBayesTreeOrdered& bayesTree) { - gttic(Allocate_VectorValues); - VectorValuesOrdered grad = *allocateVectorValues(bayesTree); - gttoc(Allocate_VectorValues); - - optimizeGradientSearchInPlace(bayesTree, grad); - - return grad; -} - -/* ************************************************************************* */ -void optimizeGradientSearchInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& grad) { - gttic(Compute_Gradient); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(bayesTree, grad); - double gradientSqNorm = grad.dot(grad); - gttoc(Compute_Gradient); - - gttic(Compute_Rg); - // Compute R * g - FactorGraphOrdered Rd_jfg(bayesTree); - Errors Rg = Rd_jfg * grad; - gttoc(Compute_Rg); - - gttic(Compute_minimizing_step_size); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - gttoc(Compute_minimizing_step_size); - - gttic(Compute_point); - // Compute steepest descent point - scal(step, grad); - gttoc(Compute_point); -} - -/* ************************************************************************* */ -VectorValuesOrdered gradient(const GaussianBayesTreeOrdered& bayesTree, const VectorValuesOrdered& x0) { - return gradient(FactorGraphOrdered(bayesTree), x0); -} - -/* ************************************************************************* */ -void gradientAtZero(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& g) { - gradientAtZero(FactorGraphOrdered(bayesTree), g); -} - -/* ************************************************************************* */ -double determinant(const GaussianBayesTreeOrdered& bayesTree) { - if (!bayesTree.root()) - return 0.0; - - return exp(internal::logDeterminant(bayesTree.root())); -} -/* ************************************************************************* */ - -} // \namespace gtsam - - - - diff --git a/gtsam/linear/GaussianBayesTreeOrdered.h b/gtsam/linear/GaussianBayesTreeOrdered.h deleted file mode 100644 index 7df2d3d5a..000000000 --- a/gtsam/linear/GaussianBayesTreeOrdered.h +++ /dev/null @@ -1,114 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianBayesTree.h - * @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree - * @brief GaussianBayesTree - * @author Frank Dellaert - * @author Richard Roberts - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/// A Bayes Tree representing a Gaussian density -GTSAM_EXPORT typedef BayesTreeOrdered GaussianBayesTreeOrdered; - -/// optimize the BayesTree, starting from the root -GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianBayesTreeOrdered& bayesTree); - -/// recursively optimize this conditional and all subtrees -GTSAM_EXPORT void optimizeInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& result); - -namespace internal { -template -void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result); -} - -/** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - */ -GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const GaussianBayesTreeOrdered& bayesTree); - -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& grad); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianBayesTreeOrdered& bayesTree, const VectorValuesOrdered& x0); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ -GTSAM_EXPORT void gradientAtZero(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& g); - -/** - * Computes the determinant of a GassianBayesTree - * A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix - * determinant is the product of the diagonal elements. Instead of actually multiplying - * we add the logarithms of the diagonal elements and take the exponent at the end - * because this is more numerically stable. - * @param bayesTree The input GassianBayesTree - * @return The determinant - */ -GTSAM_EXPORT double determinant(const GaussianBayesTreeOrdered& bayesTree); - - -namespace internal { -template -double logDeterminant(const typename BAYESTREE::sharedClique& clique); -} - -} - -#include - diff --git a/gtsam/linear/GaussianConditionalOrdered.cpp b/gtsam/linear/GaussianConditionalOrdered.cpp deleted file mode 100644 index dca2d8aa7..000000000 --- a/gtsam/linear/GaussianConditionalOrdered.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianConditional.cpp - * @brief Conditional Gaussian Base class - * @author Christian Potthast - */ - -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered() : rsd_(matrix_) {} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(Index key) : IndexConditionalOrdered(key), rsd_(matrix_) {} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) : - IndexConditionalOrdered(key), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size())); - rsd_(0) = R.triangularView(); - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const Vector& sigmas) : - IndexConditionalOrdered(key,name1), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), S.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size())); - rsd_(0) = R.triangularView(); - rsd_(1) = S; - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) : - IndexConditionalOrdered(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t dims[] = { R.cols(), S.cols(), T.cols(), 1 }; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size())); - rsd_(0) = R.triangularView(); - rsd_(1) = S; - rsd_(2) = T; - get_d_() = d; -} - -/* ************************************************************************* */ - GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d, - const Matrix& R, const list >& parents, const Vector& sigmas) : - IndexConditionalOrdered(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) { - assert(R.rows() <= R.cols()); - size_t* dims = (size_t*)alloca(sizeof(size_t)*(1+parents.size()+1)); // FIXME: alloca is bad, just ask Google. - dims[0] = R.cols(); - size_t j=1; - std::list >::const_iterator parent=parents.begin(); - for(; parent!=parents.end(); ++parent,++j) - dims[j] = parent->second.cols(); - dims[j] = 1; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size())); - rsd_(0) = R.triangularView(); - j = 1; - for(std::list >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) { - rsd_(j).noalias() = parent->second; - ++ j; - } - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(const std::list >& terms, - const size_t nrFrontals, const Vector& d, const Vector& sigmas) : - IndexConditionalOrdered(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals), - rsd_(matrix_), sigmas_(sigmas) { - size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. - size_t j=0; - typedef pair Index_Matrix; - BOOST_FOREACH(const Index_Matrix& term, terms) { - dims[j] = term.second.cols(); - ++ j; - } - dims[j] = 1; - rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size())); - j=0; - BOOST_FOREACH(const Index_Matrix& term, terms) { - rsd_(j) = term.second; - ++ j; - } - get_d_() = d; -} - -/* ************************************************************************* */ -GaussianConditionalOrdered::GaussianConditionalOrdered(const GaussianConditionalOrdered& rhs) : - rsd_(matrix_) { - *this = rhs; -} - -/* ************************************************************************* */ -GaussianConditionalOrdered& GaussianConditionalOrdered::operator=(const GaussianConditionalOrdered& rhs) { - if(this != &rhs) { - this->Base::operator=(rhs); // Copy keys - rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration - sigmas_ = rhs.sigmas_; // Copy sigmas - } - return *this; -} - -/* ************************************************************************* */ -void GaussianConditionalOrdered::print(const string &s, const IndexFormatter& formatter) const -{ - cout << s << ": density on "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; - } - cout << endl; - gtsam::print(Matrix(get_R()),"R"); - for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { - gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str()); - } - gtsam::print(Vector(get_d()),"d"); - gtsam::print(sigmas_,"sigmas"); -} - -/* ************************************************************************* */ -bool GaussianConditionalOrdered::equals(const GaussianConditionalOrdered &c, double tol) const { - // check if the size of the parents_ map is the same - if (parents().size() != c.parents().size()) - return false; - - // check if R_ and d_ are linear independent - for (size_t i=0; i rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c.get_R().row(i))); - - // check if the matrices are the same - // iterate over the parents_ map - for (const_iterator it = beginParents(); it != endParents(); ++it) { - const_iterator it2 = c.beginParents() + (it-beginParents()); - if(*it != *(it2)) - return false; - rows1.push_back(row(get_S(it), i)); - rows2.push_back(row(c.get_S(it2), i)); - } - - Vector row1 = concatVectors(rows1); - Vector row2 = concatVectors(rows2); - if (!linear_dependent(row1, row2, tol)) - return false; - } - - // check if sigmas are equal - if (!(equal_with_abs_tol(sigmas_, c.sigmas_, tol))) return false; - - return true; -} - -/* ************************************************************************* */ -JacobianFactorOrdered::shared_ptr GaussianConditionalOrdered::toFactor() const { - return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*this)); -} - -/* ************************************************************************* */ -void GaussianConditionalOrdered::solveInPlace(VectorValuesOrdered& x) const { - static const bool debug = false; - if(debug) this->print("Solving conditional in place"); - Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); - xS = this->get_d() - this->get_S() * xS; - Vector soln = this->get_R().triangularView().solve(xS); - - // Check for indeterminant solution - if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) - throw IndeterminantLinearSystemException(this->keys().front()); - - if(debug) { - gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); - gtsam::print(soln, "full back-substitution solution: "); - } - internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals()); -} - -/* ************************************************************************* */ -void GaussianConditionalOrdered::solveTransposeInPlace(VectorValuesOrdered& gy) const { - Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); - frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); - - // Check for indeterminant solution - if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) - throw IndeterminantLinearSystemException(this->keys().front()); - - GaussianConditionalOrdered::const_iterator it; - for (it = beginParents(); it!= endParents(); it++) { - const Index i = *it; - transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]); - } - internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); -} - -/* ************************************************************************* */ -void GaussianConditionalOrdered::scaleFrontalsBySigma(VectorValuesOrdered& gy) const { - Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); - frontalVec = emul(frontalVec, get_sigmas()); - internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); -} - -} - diff --git a/gtsam/linear/GaussianConditionalOrdered.h b/gtsam/linear/GaussianConditionalOrdered.h deleted file mode 100644 index 0c9f17f80..000000000 --- a/gtsam/linear/GaussianConditionalOrdered.h +++ /dev/null @@ -1,300 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianConditional.h - * @brief Conditional Gaussian Base class - * @author Christian Potthast - */ - -// \callgraph - -#pragma once - -#include - -#include -#include -#include -#include - -// Forward declaration to friend unit tests -class JacobianFactorOrderedeliminate2Test; -class GaussianConditionalOrderedconstructorTest; -class GaussianFactorGraphOrderedeliminationTest; -class GaussianJunctionTreeOrderedcomplicatedMarginalTest; -class GaussianConditionalOrderedinformationTest; -class GaussianConditionalOrderedisGaussianFactorTest; - -namespace gtsam { - -// Forward declarations -class GaussianFactorOrdered; -class JacobianFactorOrdered; - -/** - * A conditional Gaussian functions as the node in a Bayes network - * It has a set of parents y,z, etc. and implements a probability density on x. - * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ - */ -class GTSAM_EXPORT GaussianConditionalOrdered : public IndexConditionalOrdered { - -public: - typedef GaussianFactorOrdered FactorType; - typedef boost::shared_ptr shared_ptr; - - /** Store the conditional matrix as upper-triangular column-major */ - typedef Matrix RdMatrix; - typedef VerticalBlockView rsd_type; - - typedef rsd_type::Block r_type; - typedef rsd_type::constBlock const_r_type; - typedef rsd_type::Column d_type; - typedef rsd_type::constColumn const_d_type; - -protected: - - /** Store the conditional as one big upper-triangular wide matrix, arranged - * as \f$ [ R S1 S2 ... d ] \f$. Access these blocks using a VerticalBlockView. - * */ - RdMatrix matrix_; - rsd_type rsd_; - - /** vector of standard deviations */ - Vector sigmas_; - - /** typedef to base class */ - typedef IndexConditionalOrdered Base; - -public: - - /** default constructor needed for serialization */ - GaussianConditionalOrdered(); - - /** constructor */ - explicit GaussianConditionalOrdered(Index key); - - /** constructor with no parents - * |Rx-d| - */ - GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, const Vector& sigmas); - - /** constructor with only one parent - * |Rx+Sy-d| - */ - GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const Vector& sigmas); - - /** constructor with two parents - * |Rx+Sy+Tz-d| - */ - GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas); - - /** - * constructor with number of arbitrary parents (only used in unit tests, - * std::list is not efficient) - * \f$ |Rx+sum(Ai*xi)-d| \f$ - */ - GaussianConditionalOrdered(Index key, const Vector& d, - const Matrix& R, const std::list >& parents, const Vector& sigmas); - - /** - * Constructor with arbitrary number of frontals and parents (only used in unit tests, - * std::list is not efficient) - */ - GaussianConditionalOrdered(const std::list >& terms, - size_t nrFrontals, const Vector& d, const Vector& sigmas); - - /** - * Constructor when matrices are already stored in a combined matrix, allows - * for multiple frontal variables. - */ - template - GaussianConditionalOrdered(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, - const VerticalBlockView& matrices, const Vector& sigmas); - - /** Copy constructor */ - GaussianConditionalOrdered(const GaussianConditionalOrdered& rhs); - - /** Combine several GaussianConditional into a single dense GC. The - * conditionals enumerated by \c first and \c last must be in increasing - * order, meaning that the parents of any conditional may not include a - * conditional coming before it. - * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. - * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. */ - template - static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); - - /** Assignment operator */ - GaussianConditionalOrdered& operator=(const GaussianConditionalOrdered& rhs); - - /** print */ - void print(const std::string& = "GaussianConditional", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** equals function */ - bool equals(const GaussianConditionalOrdered &cg, double tol = 1e-9) const; - - /** dimension of multivariate variable (same as rows()) */ - size_t dim() const { return rsd_.rows(); } - - /** dimension of multivariate variable (same as dim()) */ - size_t rows() const { return dim(); } - - /** Compute the augmented information matrix as - * \f$ [ R S d ]^T [ R S d ] \f$ - */ - Matrix augmentedInformation() const { - return rsd_.full().transpose() * rsd_.full().transpose(); - } - - /** Compute the information matrix */ - Matrix information() const { - return get_R().transpose() * get_R(); - } - - /** Return a view of the upper-triangular R block of the conditional */ - rsd_type::constBlock get_R() const { return rsd_.range(0, nrFrontals()); } - - /** Return a view of the r.h.s. d vector */ - const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); } - - /** get the dimension of a variable */ - size_t dim(const_iterator variable) const { return rsd_(variable - this->begin()).cols(); } - - /** Get a view of the parent block corresponding to the variable pointed to by the given key iterator */ - rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); } - /** Get a view of the parent block corresponding to the variable pointed to by the given key iterator (non-const version) */ - rsd_type::constBlock get_S() const { return rsd_.range(nrFrontals(), size()); } - /** Get the Vector of sigmas */ - const Vector& get_sigmas() const {return sigmas_;} - -protected: - - const RdMatrix& matrix() const { return matrix_; } - const rsd_type& rsd() const { return rsd_; } - -public: - /** - * Copy to a Factor (this creates a JacobianFactor and returns it as its - * base class GaussianFactor. - */ - boost::shared_ptr toFactor() const; - - /** - * Solves a conditional Gaussian and writes the solution into the entries of - * \c x for each frontal variable of the conditional. The parents are - * assumed to have already been solved in and their values are read from \c x. - * This function works for multiple frontal variables. - * - * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, - * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator - * variables of this conditional, this solve function computes - * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. - * - * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the - * solution \f$ x_f \f$ will be written. - */ - void solveInPlace(VectorValuesOrdered& x) const; - - // functions for transpose backsubstitution - - /** - * Performs backsubstition in place on values - */ - void solveTransposeInPlace(VectorValuesOrdered& gy) const; - void scaleFrontalsBySigma(VectorValuesOrdered& gy) const; - -protected: - rsd_type::Column get_d_() { return rsd_.column(nrFrontals()+nrParents(), 0); } - rsd_type::Block get_R_() { return rsd_.range(0, nrFrontals()); } - rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); } - -private: - - // Friends - friend class JacobianFactorOrdered; - friend class ::JacobianFactorOrderedeliminate2Test; - friend class ::GaussianConditionalOrderedconstructorTest; - friend class ::GaussianFactorGraphOrderedeliminationTest; - friend class ::GaussianJunctionTreeOrderedcomplicatedMarginalTest; - friend class ::GaussianConditionalOrderedinformationTest; - friend class ::GaussianConditionalOrderedisGaussianFactorTest; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditionalOrdered); - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(rsd_); - ar & BOOST_SERIALIZATION_NVP(sigmas_); - } -}; // GaussianConditional - -/* ************************************************************************* */ -template -GaussianConditionalOrdered::GaussianConditionalOrdered(ITERATOR firstKey, ITERATOR lastKey, - size_t nrFrontals, const VerticalBlockView& matrices, - const Vector& sigmas) : - IndexConditionalOrdered(std::vector(firstKey, lastKey), nrFrontals), rsd_( - matrix_), sigmas_(sigmas) { - rsd_.assignNoalias(matrices); -} - -/* ************************************************************************* */ -template -GaussianConditionalOrdered::shared_ptr GaussianConditionalOrdered::Combine(ITERATOR firstConditional, ITERATOR lastConditional) { - - // TODO: check for being a clique - - // Get dimensions from first conditional - std::vector dims; dims.reserve((*firstConditional)->size() + 1); - for(const_iterator j = (*firstConditional)->begin(); j != (*firstConditional)->end(); ++j) - dims.push_back((*firstConditional)->dim(j)); - dims.push_back(1); - - // We assume the conditionals form clique, so the first n variables will be - // frontal variables in the new conditional. - size_t nFrontals = 0; - size_t nRows = 0; - for(ITERATOR c = firstConditional; c != lastConditional; ++c) { - nRows += dims[nFrontals]; - ++ nFrontals; - } - - // Allocate combined conditional, has same keys as firstConditional - Matrix tempCombined; - VerticalBlockView tempBlockView(tempCombined, dims.begin(), dims.end(), 0); - GaussianConditionalOrdered::shared_ptr combinedConditional(new GaussianConditionalOrdered((*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, zero(nRows))); - - // Resize to correct number of rows - combinedConditional->matrix_.resize(nRows, combinedConditional->matrix_.cols()); - combinedConditional->rsd_.rowEnd() = combinedConditional->matrix_.rows(); - - // Copy matrix and sigmas - const size_t totalDims = combinedConditional->matrix_.cols(); - size_t currentSlot = 0; - for(ITERATOR c = firstConditional; c != lastConditional; ++c) { - const size_t startRow = combinedConditional->rsd_.offset(currentSlot); // Start row is same as start column - combinedConditional->rsd_.range(0, currentSlot).block(startRow, 0, dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)).operator=( - Matrix::Zero(dims[currentSlot], combinedConditional->rsd_.offset(currentSlot))); - combinedConditional->rsd_.range(currentSlot, dims.size()).block(startRow, 0, dims[currentSlot], totalDims - startRow).operator=( - (*c)->matrix_); - combinedConditional->sigmas_.segment(startRow, dims[currentSlot]) = (*c)->sigmas_; - ++ currentSlot; - } - - return combinedConditional; -} - -} // gtsam diff --git a/gtsam/linear/GaussianFactorGraphOrdered.cpp b/gtsam/linear/GaussianFactorGraphOrdered.cpp deleted file mode 100644 index c64994fca..000000000 --- a/gtsam/linear/GaussianFactorGraphOrdered.cpp +++ /dev/null @@ -1,590 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianFactorGraph.cpp - * @brief Linear Factor Graph where all factors are Gaussians - * @author Kai Ni - * @author Christian Potthast - * @author Richard Roberts - * @author Frank Dellaert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -namespace gtsam { - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::GaussianFactorGraphOrdered(const GaussianBayesNetOrdered& CBN) : Base(CBN) {} - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::Keys GaussianFactorGraphOrdered::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); - return keys; - } - - /* ************************************************************************* */ - void GaussianFactorGraphOrdered::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void GaussianFactorGraphOrdered::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); - } - } - - /* ************************************************************************* */ - void GaussianFactorGraphOrdered::combine(const GaussianFactorGraphOrdered &lfg) { - for (const_iterator factor = lfg.factors_.begin(); factor - != lfg.factors_.end(); factor++) { - push_back(*factor); - } - } - - /* ************************************************************************* */ - GaussianFactorGraphOrdered GaussianFactorGraphOrdered::combine2( - const GaussianFactorGraphOrdered& lfg1, const GaussianFactorGraphOrdered& lfg2) { - - // create new linear factor graph equal to the first one - GaussianFactorGraphOrdered fg = lfg1; - - // add the second factors_ in the graph - for (const_iterator factor = lfg2.factors_.begin(); factor - != lfg2.factors_.end(); factor++) { - fg.push_back(*factor); - } - return fg; - } - - /* ************************************************************************* */ - std::vector > GaussianFactorGraphOrdered::sparseJacobian() const { - // First find dimensions of each variable - FastVector dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { - for(GaussianFactorOrdered::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { - if(dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); - } - } - - // Compute first scalar column of each variable - vector columnIndices(dims.size()+1, 0); - for(size_t j=1; j triplet; - FastVector entries; - size_t row = 0; - BOOST_FOREACH(const sharedFactor& factor, *this) { - // Convert to JacobianFactor if necessary - JacobianFactorOrdered::shared_ptr jacobianFactor( - boost::dynamic_pointer_cast(factor)); - if (!jacobianFactor) { - HessianFactorOrdered::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (hessian) - jacobianFactor.reset(new JacobianFactorOrdered(*hessian)); - else - throw invalid_argument( - "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - - // Whiten the factor and add entries for it - // iterate over all variables in the factor - const JacobianFactorOrdered whitened(jacobianFactor->whiten()); - for(JacobianFactorOrdered::const_iterator pos=whitened.begin(); pos 1e-12) entries.push_back( - boost::make_tuple(row+i, column_start+j, s)); - } - } - - JacobianFactorOrdered::constBVector whitenedb(whitened.getb()); - size_t bcolumn = columnIndices.back(); - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); - - // Increment row index - row += jacobianFactor->rows(); - } - return vector(entries.begin(), entries.end()); - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraphOrdered::sparseJacobian_() const { - - // call sparseJacobian - typedef boost::tuple triplet; - std::vector result = sparseJacobian(); - - // translate to base 1 matrix - size_t nzmax = result.size(); - Matrix IJS(3,nzmax); - for (size_t k = 0; k < result.size(); k++) { - const triplet& entry = result[k]; - IJS(0,k) = double(entry.get<0>() + 1); - IJS(1,k) = double(entry.get<1>() + 1); - IJS(2,k) = entry.get<2>(); - } - return IJS; - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraphOrdered::augmentedJacobian() const { - // Convert to Jacobians - FactorGraphOrdered jfg; - jfg.reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(boost::shared_ptr jf = - boost::dynamic_pointer_cast(factor)) - jfg.push_back(jf); - else - jfg.push_back(boost::make_shared(*factor)); - } - // combine all factors - JacobianFactorOrdered combined(*CombineJacobians(jfg, VariableSlots(*this))); - return combined.matrix_augmented(); - } - - /* ************************************************************************* */ - std::pair GaussianFactorGraphOrdered::jacobian() const { - Matrix augmented = augmentedJacobian(); - return make_pair( - augmented.leftCols(augmented.cols()-1), - augmented.col(augmented.cols()-1)); - } - - // Helper functions for Combine - static boost::tuple, size_t, size_t> countDims(const FactorGraphOrdered& factors, const VariableSlots& variableSlots) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - vector varDims(variableSlots.size(), numeric_limits::max()); -#else - vector varDims(variableSlots.size()); -#endif - size_t m = 0; - size_t n = 0; - { - Index jointVarpos = 0; - BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { - - assert(slots.second.size() == factors.size()); - - Index sourceFactorI = 0; - BOOST_FOREACH(const Index sourceVarpos, slots.second) { - if(sourceVarpos < numeric_limits::max()) { - const JacobianFactorOrdered& sourceFactor = *factors[sourceFactorI]; - size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - if(varDims[jointVarpos] == numeric_limits::max()) { - varDims[jointVarpos] = vardim; - n += vardim; - } else - assert(varDims[jointVarpos] == vardim); -#else - varDims[jointVarpos] = vardim; - n += vardim; - break; -#endif - } - ++ sourceFactorI; - } - ++ jointVarpos; - } - BOOST_FOREACH(const JacobianFactorOrdered::shared_ptr& factor, factors) { - m += factor->rows(); - } - } - return boost::make_tuple(varDims, m, n); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::shared_ptr CombineJacobians( - const FactorGraphOrdered& factors, - const VariableSlots& variableSlots) { - - const bool debug = ISDEBUG("CombineJacobians"); - if (debug) factors.print("Combining factors: "); - if (debug) variableSlots.print(); - - if (debug) cout << "Determine dimensions" << endl; - gttic(countDims); - vector varDims; - size_t m, n; - boost::tie(varDims, m, n) = countDims(factors, variableSlots); - if (debug) { - cout << "Dims: " << m << " x " << n << "\n"; - BOOST_FOREACH(const size_t dim, varDims) cout << dim << " "; - cout << endl; - } - gttoc(countDims); - - if (debug) cout << "Allocate new factor" << endl; - gttic(allocate); - JacobianFactorOrdered::shared_ptr combined(new JacobianFactorOrdered()); - combined->allocate(variableSlots, varDims, m); - Vector sigmas(m); - gttoc(allocate); - - if (debug) cout << "Copy blocks" << endl; - gttic(copy_blocks); - // Loop over slots in combined factor - Index combinedSlot = 0; - BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { - JacobianFactorOrdered::ABlock destSlot(combined->getA(combined->begin()+combinedSlot)); - // Loop over source factors - size_t nextRow = 0; - for(size_t factorI = 0; factorI < factors.size(); ++factorI) { - // Slot in source factor - const Index sourceSlot = varslot.second[factorI]; - const size_t sourceRows = factors[factorI]->rows(); - JacobianFactorOrdered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); - // Copy if exists in source factor, otherwise set zero - if(sourceSlot != numeric_limits::max()) - destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot); - else - destBlock.setZero(); - nextRow += sourceRows; - } - ++combinedSlot; - } - gttoc(copy_blocks); - - if (debug) cout << "Copy rhs (b) and sigma" << endl; - gttic(copy_vectors); - bool anyConstrained = false; - // Loop over source factors - size_t nextRow = 0; - for(size_t factorI = 0; factorI < factors.size(); ++factorI) { - const size_t sourceRows = factors[factorI]->rows(); - combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb(); - sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas(); - if (factors[factorI]->isConstrained()) anyConstrained = true; - nextRow += sourceRows; - } - gttoc(copy_vectors); - - if (debug) cout << "Create noise model from sigmas" << endl; - gttic(noise_model); - combined->setModel(anyConstrained, sigmas); - gttoc(noise_model); - - if (debug) cout << "Assert Invariants" << endl; - combined->assertInvariants(); - - return combined; - } - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::EliminationResult EliminateJacobians(const FactorGraphOrdered< - JacobianFactorOrdered>& factors, size_t nrFrontals) { - gttic(Combine); - JacobianFactorOrdered::shared_ptr jointFactor = - CombineJacobians(factors, VariableSlots(factors)); - gttoc(Combine); - gttic(eliminate); - GaussianConditionalOrdered::shared_ptr gbn = jointFactor->eliminate(nrFrontals); - gttoc(eliminate); - return make_pair(gbn, jointFactor); - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraphOrdered::augmentedHessian() const { - // combine all factors and get upper-triangular part of Hessian - HessianFactorOrdered combined(*this); - Matrix result = combined.info(); - // Fill in lower-triangular part of Hessian - result.triangularView() = result.transpose(); - return result; - } - - /* ************************************************************************* */ - std::pair GaussianFactorGraphOrdered::hessian() const { - Matrix augmented = augmentedHessian(); - return make_pair( - augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), - augmented.col(augmented.rows()-1).head(augmented.rows()-1)); - } - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::EliminationResult EliminateCholeskyOrdered(const FactorGraphOrdered< - GaussianFactorOrdered>& factors, size_t nrFrontals) { - - const bool debug = ISDEBUG("EliminateCholesky"); - - // Form Ab' * Ab - gttic(combine); - HessianFactorOrdered::shared_ptr combinedFactor(new HessianFactorOrdered(factors)); - gttoc(combine); - - // Do Cholesky, note that after this, the lower triangle still contains - // some untouched non-zeros that should be zero. We zero them while - // extracting submatrices next. - gttic(partial_Cholesky); - combinedFactor->partialCholesky(nrFrontals); - - gttoc(partial_Cholesky); - - // Extract conditional and fill in details of the remaining factor - gttic(split); - GaussianConditionalOrdered::shared_ptr conditional = - combinedFactor->splitEliminatedFactor(nrFrontals); - if (debug) { - conditional->print("Extracted conditional: "); - combinedFactor->print("Eliminated factor (L piece): "); - } - gttoc(split); - - combinedFactor->assertInvariants(); - return make_pair(conditional, combinedFactor); - } - - /* ************************************************************************* */ - static FactorGraphOrdered convertToJacobians(const FactorGraphOrdered< - GaussianFactorOrdered>& factors) { - - typedef JacobianFactorOrdered J; - typedef HessianFactorOrdered H; - - const bool debug = ISDEBUG("convertToJacobians"); - - FactorGraphOrdered jacobians; - jacobians.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) - if (factor) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian) { - jacobians.push_back(jacobian); - if (debug) jacobian->print("Existing JacobianFactor: "); - } else { - H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (!hessian) throw std::invalid_argument( - "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); - J::shared_ptr converted(new J(*hessian)); - if (debug) { - cout << "Converted HessianFactor to JacobianFactor:\n"; - hessian->print("HessianFactor: "); - converted->print("JacobianFactor: "); - if (!assert_equal(*hessian, HessianFactorOrdered(*converted), 1e-3)) throw runtime_error( - "convertToJacobians: Conversion between Jacobian and Hessian incorrect"); - } - jacobians.push_back(converted); - } - } - return jacobians; - } - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::EliminationResult EliminateQROrdered(const FactorGraphOrdered< - GaussianFactorOrdered>& factors, size_t nrFrontals) { - - const bool debug = ISDEBUG("EliminateQR"); - - // Convert all factors to the appropriate type and call the type-specific EliminateGaussian. - if (debug) cout << "Using QR" << endl; - - gttic(convert_to_Jacobian); - FactorGraphOrdered jacobians = convertToJacobians(factors); - gttoc(convert_to_Jacobian); - - gttic(Jacobian_EliminateGaussian); - GaussianConditionalOrdered::shared_ptr conditional; - GaussianFactorOrdered::shared_ptr factor; - boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals); - gttoc(Jacobian_EliminateGaussian); - - return make_pair(conditional, factor); - } // \EliminateQR - - /* ************************************************************************* */ - bool hasConstraints(const FactorGraphOrdered& factors) { - typedef JacobianFactorOrdered J; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian && jacobian->get_model()->isConstrained()) { - return true; - } - } - return false; - } - - /* ************************************************************************* */ - GaussianFactorGraphOrdered::EliminationResult EliminatePreferCholeskyOrdered( - const FactorGraphOrdered& factors, size_t nrFrontals) { - - // If any JacobianFactors have constrained noise models, we have to convert - // all factors to JacobianFactors. Otherwise, we can convert all factors - // to HessianFactors. This is because QR can handle constrained noise - // models but Cholesky cannot. - if (hasConstraints(factors)) - return EliminateQROrdered(factors, nrFrontals); - else { - GaussianFactorGraphOrdered::EliminationResult ret; - gttic(EliminateCholeskyOrdered); - ret = EliminateCholeskyOrdered(factors, nrFrontals); - gttoc(EliminateCholeskyOrdered); - return ret; - } - - } // \EliminatePreferCholesky - - - - /* ************************************************************************* */ - static JacobianFactorOrdered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorOrdered::shared_ptr &gf) { - JacobianFactorOrdered::shared_ptr result = boost::dynamic_pointer_cast(gf); - if( !result ) { - result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - return result; - } - - /* ************************************************************************* */ - Errors operator*(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) { - Errors e; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back((*Ai)*x); - } - return e; - } - - /* ************************************************************************* */ - void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, Errors& e) { - multiplyInPlace(fg,x,e.begin()); - } - - /* ************************************************************************* */ - void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, const Errors::iterator& e) { - Errors::iterator ei = e; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - *ei = (*Ai)*x; - ei++; - } - } - - /* ************************************************************************* */ - // x += alpha*A'*e - void transposeMultiplyAdd(const GaussianFactorGraphOrdered& fg, double alpha, const Errors& e, VectorValuesOrdered& x) { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha,*(ei++),x); - } - } - - /* ************************************************************************* */ - VectorValuesOrdered gradient(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x0) { - // It is crucial for performance to make a zero-valued clone of x - VectorValuesOrdered g = VectorValuesOrdered::Zero(x0); - Errors e; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back(Ai->error_vector(x0)); - } - transposeMultiplyAdd(fg, 1.0, e, g); - return g; - } - - /* ************************************************************************* */ - void gradientAtZero(const GaussianFactorGraphOrdered& fg, VectorValuesOrdered& g) { - // Zero-out the gradient - g.setZero(); - Errors e; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back(-Ai->getb()); - } - transposeMultiplyAdd(fg, 1.0, e, g); - } - - /* ************************************************************************* */ - void residual(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r) { - Index i = 0 ; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - r[i] = Ai->getb(); - i++; - } - VectorValuesOrdered Ax = VectorValuesOrdered::SameStructure(r); - multiply(fg,x,Ax); - axpy(-1.0,Ax,r); - } - - /* ************************************************************************* */ - void multiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r) { - r.setZero(); - Index i = 0; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Vector &y = r[i]; - for(JacobianFactorOrdered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - y += Ai->getA(j) * x[*j]; - } - ++i; - } - } - - /* ************************************************************************* */ - void transposeMultiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &r, VectorValuesOrdered &x) { - x.setZero(); - Index i = 0; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - for(JacobianFactorOrdered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - x[*j] += Ai->getA(j).transpose() * r[i]; - } - ++i; - } - } - - /* ************************************************************************* */ - boost::shared_ptr gaussianErrors_(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) { - JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e->push_back(Ai->error_vector(x)); - } - return e; - } - -} // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraphOrdered.h b/gtsam/linear/GaussianFactorGraphOrdered.h deleted file mode 100644 index 0db7f9fae..000000000 --- a/gtsam/linear/GaussianFactorGraphOrdered.h +++ /dev/null @@ -1,388 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianFactorGraph.h - * @brief Linear Factor Graph where all factors are Gaussians - * @author Kai Ni - * @author Christian Potthast - * @author Alireza Fathi - * @author Richard Roberts - * @author Frank Dellaert - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -namespace gtsam { - - // Forward declaration to use as default argument, documented declaration below. - GTSAM_EXPORT FactorGraphOrdered::EliminationResult - EliminateQROrdered(const FactorGraphOrdered& factors, size_t nrFrontals); - - /** - * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. - * Factor == GaussianFactor - * VectorValues = A values structure of vectors - * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. - */ - class GaussianFactorGraphOrdered : public FactorGraphOrdered { - public: - - typedef boost::shared_ptr shared_ptr; - typedef FactorGraphOrdered Base; - - /** - * Default constructor - */ - GaussianFactorGraphOrdered() {} - - /** - * Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph - */ - GTSAM_EXPORT GaussianFactorGraphOrdered(const GaussianBayesNetOrdered& CBN); - - /** - * Constructor that receives a BayesTree and returns a GaussianFactorGraph - */ - template - GaussianFactorGraphOrdered(const BayesTreeOrdered& gbt) : Base(gbt) {} - - /** Constructor from a factor graph of GaussianFactor or a derived type */ - template - GaussianFactorGraphOrdered(const FactorGraphOrdered& fg) { - push_back(fg); - } - - /** Add a factor by value - makes a copy */ - void add(const GaussianFactorOrdered& factor) { - factors_.push_back(factor.clone()); - } - - /** Add a factor by pointer - stores pointer without copying the factor */ - void add(const sharedFactor& factor) { - factors_.push_back(factor); - } - - /** Add a null factor */ - void add(const Vector& b) { - add(JacobianFactorOrdered(b)); - } - - /** Add a unary factor */ - void add(Index key1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactorOrdered(key1,A1,b,model)); - } - - /** Add a binary factor */ - void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactorOrdered(key1,A1,key2,A2,b,model)); - } - - /** Add a ternary factor */ - void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - Index key3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model) { - add(JacobianFactorOrdered(key1,A1,key2,A2,key3,A3,b,model)); - } - - /** Add an n-ary factor */ - void add(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) { - add(JacobianFactorOrdered(terms,b,model)); - } - - /** - * Return the set of variables involved in the factors (computes a set - * union). - */ - typedef FastSet Keys; - GTSAM_EXPORT Keys keys() const; - - - /** Eliminate the first \c n frontal variables, returning the resulting - * conditional and remaining factor graph - this is very inefficient for - * eliminating all variables, to do that use EliminationTree or - * JunctionTree. Note that this version simply calls - * FactorGraph::eliminateFrontals with EliminateQR as the - * eliminate function argument. - */ - std::pair eliminateFrontals(size_t nFrontals, const Eliminate& function = EliminateQROrdered) const { - return Base::eliminateFrontals(nFrontals, function); } - - /** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - * Note that this version simply calls - * FactorGraph::eliminate with EliminateQR as the eliminate - * function argument. - */ - std::pair eliminate(const std::vector& variables, const Eliminate& function = EliminateQROrdered) const { - return Base::eliminate(variables, function); } - - /** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */ - std::pair eliminateOne(Index variable, const Eliminate& function = EliminateQROrdered) const { - return Base::eliminateOne(variable, function); } - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); - - /** unnormalized error */ - double error(const VectorValuesOrdered& x) const { - double total_error = 0.; - BOOST_FOREACH(const sharedFactor& factor, *this) - total_error += factor->error(x); - return total_error; - } - - /** Unnormalized probability. O(n) */ - double probPrime(const VectorValuesOrdered& c) const { - return exp(-0.5 * error(c)); - } - - /** - * Static function that combines two factor graphs. - * @param lfg1 Linear factor graph - * @param lfg2 Linear factor graph - * @return a new combined factor graph - */ - GTSAM_EXPORT static GaussianFactorGraphOrdered combine2(const GaussianFactorGraphOrdered& lfg1, - const GaussianFactorGraphOrdered& lfg2); - - /** - * combine two factor graphs - * @param *lfg Linear factor graph - */ - GTSAM_EXPORT void combine(const GaussianFactorGraphOrdered &lfg); - - /** - * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, - * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. - * The standard deviations are baked into A and b - */ - GTSAM_EXPORT std::vector > sparseJacobian() const; - - /** - * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * The standard deviations are baked into A and b - */ - GTSAM_EXPORT Matrix sparseJacobian_() const; - - /** - * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ - * Jacobian matrix, augmented with b with the noise models baked - * into A and b. The negative log-likelihood is - * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also - * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. - */ - GTSAM_EXPORT Matrix augmentedJacobian() const; - - /** - * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, - * with the noise models baked into A and b. The negative log-likelihood - * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also - * GaussianFactorGraph::augmentedJacobian and - * GaussianFactorGraph::sparseJacobian. - */ - GTSAM_EXPORT std::pair jacobian() const; - - /** - * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian - * matrix, augmented with the information vector \f$ \eta \f$. The - * augmented Hessian is - \f[ \left[ \begin{array}{ccc} - \Lambda & \eta \\ - \eta^T & c - \end{array} \right] \f] - and the negative log-likelihood is - \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. - */ - GTSAM_EXPORT Matrix augmentedHessian() const; - - /** - * Return the dense Hessian \f$ \Lambda \f$ and information vector - * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood - * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also - * GaussianFactorGraph::augmentedHessian. - */ - GTSAM_EXPORT std::pair hessian() const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - - }; - - /** - * Combine and eliminate several factors. - * \addtogroup LinearSolving - */ - GTSAM_EXPORT JacobianFactorOrdered::shared_ptr CombineJacobians( - const FactorGraphOrdered& factors, - const VariableSlots& variableSlots); - - /** - * Evaluates whether linear factors have any constrained noise models - * @return true if any factor is constrained. - */ - GTSAM_EXPORT bool hasConstraints(const FactorGraphOrdered& factors); - - /** - * Densely combine and partially eliminate JacobianFactors to produce a - * single conditional with nrFrontals frontal variables and a remaining - * factor. - * Variables are eliminated in the natural order of the variable indices of in the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateJacobians(const FactorGraphOrdered< - JacobianFactorOrdered>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with QR factorization. HessianFactors are - * first factored with Cholesky decomposition to produce JacobianFactors, - * by calling the conversion constructor JacobianFactor(const HessianFactor&). - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateQROrdered(const FactorGraphOrdered< - GaussianFactorOrdered>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models (any sigmas equal to - * zero), QR factorization will be performed instead, because our current - * implementation cannot handle constrained noise models in Cholesky - * factorization. EliminateCholesky(), on the other hand, will fail if any - * factors contain constrained noise models. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminatePreferCholeskyOrdered(const FactorGraphOrdered< - GaussianFactorOrdered>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models, this function will fail - * because our current implementation cannot handle constrained noise models - * in Cholesky factorization. The function EliminatePreferCholesky() - * automatically does QR instead when this is the case. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateCholeskyOrdered(const FactorGraphOrdered< - GaussianFactorOrdered>& factors, size_t nrFrontals = 1); - - /****** Linear Algebra Opeations ******/ - - /** return A*x */ - GTSAM_EXPORT Errors operator*(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x); - - /** In-place version e <- A*x that overwrites e. */ - GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, Errors& e); - - /** In-place version e <- A*x that takes an iterator. */ - GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, const Errors::iterator& e); - - /** x += alpha*A'*e */ - GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphOrdered& fg, double alpha, const Errors& e, VectorValuesOrdered& x); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x0); - - /** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, - * centered around zero. - * The gradient is \f$ A^T(Ax-b) \f$. - * @param fg The Jacobian factor graph $(A,b)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ - GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraphOrdered& fg, VectorValuesOrdered& g); - - /* matrix-vector operations */ - GTSAM_EXPORT void residual(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r); - GTSAM_EXPORT void multiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r); - GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &r, VectorValuesOrdered &x); - - /** shared pointer version - * \todo Make this a member function - affects SubgraphPreconditioner */ - GTSAM_EXPORT boost::shared_ptr gaussianErrors_(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x); - - /** return A*x-b - * \todo Make this a member function - affects SubgraphPreconditioner */ - inline Errors gaussianErrors(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) { - return *gaussianErrors_(fg, x); } - -} // namespace gtsam diff --git a/gtsam/linear/GaussianFactorOrdered.cpp b/gtsam/linear/GaussianFactorOrdered.cpp deleted file mode 100644 index d65410428..000000000 --- a/gtsam/linear/GaussianFactorOrdered.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianFactor.cpp - * @brief Linear Factor....A Gaussian - * @brief linearFactor - * @author Richard Roberts, Christian Potthast - */ - -#include -#include - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ - GaussianFactorOrdered::GaussianFactorOrdered(const GaussianConditionalOrdered& c) : - IndexFactorOrdered(c) {} - -} diff --git a/gtsam/linear/GaussianFactorOrdered.h b/gtsam/linear/GaussianFactorOrdered.h deleted file mode 100644 index 76f027281..000000000 --- a/gtsam/linear/GaussianFactorOrdered.h +++ /dev/null @@ -1,156 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianFactor.h - * @brief A factor with a quadratic error function - a Gaussian - * @brief GaussianFactor - * @author Richard Roberts, Christian Potthast - */ - -// \callgraph - -#pragma once - -#include -#include - -#include - -#include -#include - -namespace gtsam { - - // Forward declarations - class VectorValuesOrdered; - class Permutation; - class GaussianConditionalOrdered; - template class BayesNetOrdered; - - /** - * An abstract virtual base class for JacobianFactor and HessianFactor. - * A GaussianFactor has a quadratic error function. - * GaussianFactor is non-mutable (all methods const!). - * The factor value is exp(-0.5*||Ax-b||^2) - */ - class GTSAM_EXPORT GaussianFactorOrdered: public IndexFactorOrdered { - - protected: - - /** Copy constructor */ - GaussianFactorOrdered(const This& f) : IndexFactorOrdered(f) {} - - /** Construct from derived type */ - GaussianFactorOrdered(const GaussianConditionalOrdered& c); - - /** Constructor from a collection of keys */ - template GaussianFactorOrdered(KeyIterator beginKey, KeyIterator endKey) : - Base(beginKey, endKey) {} - - /** Default constructor for I/O */ - GaussianFactorOrdered() {} - - /** Construct unary factor */ - GaussianFactorOrdered(Index j) : IndexFactorOrdered(j) {} - - /** Construct binary factor */ - GaussianFactorOrdered(Index j1, Index j2) : IndexFactorOrdered(j1, j2) {} - - /** Construct ternary factor */ - GaussianFactorOrdered(Index j1, Index j2, Index j3) : IndexFactorOrdered(j1, j2, j3) {} - - /** Construct 4-way factor */ - GaussianFactorOrdered(Index j1, Index j2, Index j3, Index j4) : IndexFactorOrdered(j1, j2, j3, j4) {} - - /** Construct n-way factor */ - GaussianFactorOrdered(const std::set& js) : IndexFactorOrdered(js) {} - - /** Construct n-way factor */ - GaussianFactorOrdered(const std::vector& js) : IndexFactorOrdered(js) {} - - public: - - typedef GaussianConditionalOrdered ConditionalType; - typedef boost::shared_ptr shared_ptr; - - // Implementing Testable interface - virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const = 0; - - virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const = 0; - - virtual double error(const VectorValuesOrdered& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ - - /** Return the dimension of the variable pointed to by the given key iterator */ - virtual size_t getDim(const_iterator variable) const = 0; - - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - */ - virtual Matrix augmentedInformation() const = 0; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const = 0; - - /** Clone a factor (make a deep copy) */ - virtual GaussianFactorOrdered::shared_ptr clone() const = 0; - - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation) { - IndexFactorOrdered::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactorOrdered::reduceWithInverse(inverseReduction); - } - - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactorOrdered::shared_ptr negate() const = 0; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactorOrdered); - } - - }; // GaussianFactor - - /** make keys from list, vector, or map of matrices */ - template - static std::vector GetKeys(size_t n, ITERATOR begin, ITERATOR end) { - std::vector keys; - keys.reserve(n); - for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first); - return keys; - } - -} // namespace gtsam diff --git a/gtsam/linear/GaussianISAMOrdered.cpp b/gtsam/linear/GaussianISAMOrdered.cpp deleted file mode 100644 index 0b8f3e1ef..000000000 --- a/gtsam/linear/GaussianISAMOrdered.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianISAM.cpp - * @brief Linear ISAM only - * @author Michael Kaess - */ - -#include -#include - -#include - -using namespace std; -using namespace gtsam; - -namespace gtsam { - -// Explicitly instantiate so we don't have to include everywhere -template class ISAMOrdered; - -/* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr GaussianISAMOrdered::marginalFactor(Index j) const { - return Super::marginalFactor(j, &EliminateQROrdered); -} - -/* ************************************************************************* */ -BayesNetOrdered::shared_ptr GaussianISAMOrdered::marginalBayesNet(Index j) const { - return Super::marginalBayesNet(j, &EliminateQROrdered); -} - -/* ************************************************************************* */ -Matrix GaussianISAMOrdered::marginalCovariance(Index j) const { - GaussianConditionalOrdered::shared_ptr conditional = marginalBayesNet(j)->front(); - return conditional->information().inverse(); -} - -/* ************************************************************************* */ - BayesNetOrdered::shared_ptr GaussianISAMOrdered::jointBayesNet( - Index key1, Index key2) const { - return Super::jointBayesNet(key1, key2, &EliminateQROrdered); -} - -/* ************************************************************************* */ -VectorValuesOrdered optimize(const GaussianISAMOrdered& isam) { - VectorValuesOrdered result(isam.dims_); - // Call optimize for BayesTree - optimizeInPlace((const BayesTreeOrdered&)isam, result); - return result; -} - -/* ************************************************************************* */ -BayesNetOrdered GaussianISAMOrdered::shortcut(sharedClique clique, sharedClique root) { - return clique->shortcut(root,&EliminateQROrdered); -} -/* ************************************************************************* */ - -} // \namespace gtsam diff --git a/gtsam/linear/GaussianISAMOrdered.h b/gtsam/linear/GaussianISAMOrdered.h deleted file mode 100644 index 1434b6503..000000000 --- a/gtsam/linear/GaussianISAMOrdered.h +++ /dev/null @@ -1,102 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianISAM.h - * @brief Linear ISAM only - * @author Michael Kaess - */ - -// \callgraph - -#pragma once - -#include - -#include -#include -#include - -namespace gtsam { - -class GaussianISAMOrdered : public ISAMOrdered { - - typedef ISAMOrdered Super; - std::vector dims_; - -public: - - typedef std::vector Dims; - - /** Create an empty Bayes Tree */ - GaussianISAMOrdered() : Super() {} - - /** Create a Bayes Tree from a Bayes Net */ -// GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {} - GaussianISAMOrdered(const BayesTreeOrdered& bayesTree) : Super(bayesTree) { - GaussianJunctionTreeOrdered::countDims(bayesTree, dims_); - } - - /** Override update_internal to also keep track of variable dimensions. */ - template - void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { - Super::update_internal(newFactors, orphans, &EliminateQROrdered); // TODO: why does this force QR? - update_dimensions(newFactors); - } - - template - void update(const FACTORGRAPH& newFactors) { - Cliques orphans; - this->update_internal(newFactors, orphans); - } - - template - inline void update_dimensions(const FACTORGRAPH& newFactors) { - BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) { - for(typename FACTORGRAPH::FactorType::const_iterator key = factor->begin(); key != factor->end(); ++key) { - if(*key >= dims_.size()) - dims_.resize(*key + 1); - if(dims_[*key] == 0) - dims_[*key] = factor->getDim(key); - else - assert(dims_[*key] == factor->getDim(key)); - } - } - } - - void clear() { - Super::clear(); - dims_.clear(); - } - - // access - const Dims& dims() const { return dims_; } ///< Const access to dimensions structure - Dims& dims() { return dims_; } ///< non-const access to dimensions structure (advanced interface) - - GTSAM_EXPORT friend VectorValuesOrdered optimize(const GaussianISAMOrdered&); - - /** return marginal on any variable as a factor, Bayes net, or mean/cov */ - GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const; - GTSAM_EXPORT BayesNetOrdered::shared_ptr marginalBayesNet(Index key) const; - GTSAM_EXPORT Matrix marginalCovariance(Index key) const; - - /** return joint between two variables, as a Bayes net */ - GTSAM_EXPORT BayesNetOrdered::shared_ptr jointBayesNet(Index key1, Index key2) const; - - /** return the conditional P(S|Root) on the separator given the root */ - GTSAM_EXPORT static BayesNetOrdered shortcut(sharedClique clique, sharedClique root); - -}; // \class GaussianISAM - -// optimize the BayesTree, starting from the root -GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianISAMOrdered& isam); - -} // \namespace gtsam diff --git a/gtsam/linear/GaussianJunctionTreeOrdered.cpp b/gtsam/linear/GaussianJunctionTreeOrdered.cpp deleted file mode 100644 index ed928462c..000000000 --- a/gtsam/linear/GaussianJunctionTreeOrdered.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianJunctionTree.cpp - * @date Jul 12, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: the Gaussian junction tree - */ - -#include -#include -#include -#include - -#include - -#include - -namespace gtsam { - - // explicit template instantiation - template class JunctionTreeOrdered; - template class ClusterTreeOrdered; - - using namespace std; - - /* ************************************************************************* */ - VectorValuesOrdered GaussianJunctionTreeOrdered::optimize(Eliminate function) const { - gttic(GJT_eliminate); - // eliminate from leaves to the root - BTClique::shared_ptr rootClique(this->eliminate(function)); - gttoc(GJT_eliminate); - - // Allocate solution vector and copy RHS - gttic(allocate_VectorValues); - vector dims(rootClique->conditional()->back()+1, 0); - countDims(rootClique, dims); - VectorValuesOrdered result(dims); - gttoc(allocate_VectorValues); - - // back-substitution - gttic(backsubstitute); - internal::optimizeInPlace(rootClique, result); - gttoc(backsubstitute); - return result; - } - - /* ************************************************************************* */ -} //namespace gtsam diff --git a/gtsam/linear/GaussianJunctionTreeOrdered.h b/gtsam/linear/GaussianJunctionTreeOrdered.h deleted file mode 100644 index 0360513e3..000000000 --- a/gtsam/linear/GaussianJunctionTreeOrdered.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianJunctionTree.h - * @date Jul 12, 2010 - * @author Kai Ni - * @author Frank Dellaert - * @brief: the Gaussian junction tree - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * A JunctionTree where all the factors are of type GaussianFactor. - * - * In GTSAM, typically, a GaussianJunctionTree is created directly from a GaussianFactorGraph, - * after which you call optimize() to solve for the mean, or JunctionTree::eliminate() to - * create a BayesTree. In both cases, you need to provide a basic - * GaussianFactorGraph::Eliminate function that will be used to - * - * \addtogroup Multifrontal - */ - class GaussianJunctionTreeOrdered: public JunctionTreeOrdered { - - public: - typedef boost::shared_ptr shared_ptr; - typedef JunctionTreeOrdered Base; - typedef Base::sharedClique sharedClique; - typedef GaussianFactorGraphOrdered::Eliminate Eliminate; - - public : - - /** Default constructor */ - GaussianJunctionTreeOrdered() : Base() {} - - /** Constructor from a factor graph. Builds a VariableIndex. */ - GaussianJunctionTreeOrdered(const GaussianFactorGraphOrdered& fg) : Base(fg) {} - - /** Construct from a factor graph and a pre-computed variable index. */ - GaussianJunctionTreeOrdered(const GaussianFactorGraphOrdered& fg, const VariableIndexOrdered& variableIndex) - : Base(fg, variableIndex) {} - - /** - * optimize the linear graph - */ - GTSAM_EXPORT VectorValuesOrdered optimize(Eliminate function) const; - - // convenient function to return dimensions of all variables in the BayesTree - template - static void countDims(const BayesTreeOrdered& bayesTree, DIM_CONTAINER& dims) { - dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0); - countDims(bayesTree.root(), dims); - } - - private: - template - static void countDims(const boost::shared_ptr& clique, DIM_CONTAINER& dims) { - GaussianConditionalOrdered::const_iterator it = clique->conditional()->beginFrontals(); - for (; it != clique->conditional()->endFrontals(); ++it) { - assert(dims.at(*it) == 0); - dims.at(*it) = clique->conditional()->dim(it); - } - - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children()) { - countDims(child, dims); - } - } - - }; // GaussianJunctionTree - -} // namespace gtsam diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp deleted file mode 100644 index 26da08ecd..000000000 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#include - -namespace gtsam { - -/* ************************************************************************* */ -GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraphOrdered& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) {} - -/* ************************************************************************* */ -GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) {} - -/* ************************************************************************* */ -GaussianMultifrontalSolver::shared_ptr -GaussianMultifrontalSolver::Create(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) { - return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex, useQR)); -} - -/* ************************************************************************* */ -void GaussianMultifrontalSolver::replaceFactors(const FactorGraphOrdered::shared_ptr& factorGraph) { - Base::replaceFactors(factorGraph); -} - -/* ************************************************************************* */ -GaussianBayesTreeOrdered::shared_ptr GaussianMultifrontalSolver::eliminate() const { - if (useQR_) - return Base::eliminate(&EliminateQROrdered); - else - return Base::eliminate(&EliminatePreferCholeskyOrdered); -} - -/* ************************************************************************* */ -VectorValuesOrdered::shared_ptr GaussianMultifrontalSolver::optimize() const { - gttic(GaussianMultifrontalSolver_optimize); - VectorValuesOrdered::shared_ptr values; - if (useQR_) - values = VectorValuesOrdered::shared_ptr(new VectorValuesOrdered(junctionTree_->optimize(&EliminateQROrdered))); - else - values= VectorValuesOrdered::shared_ptr(new VectorValuesOrdered(junctionTree_->optimize(&EliminatePreferCholeskyOrdered))); - return values; -} - -/* ************************************************************************* */ -GaussianFactorGraphOrdered::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector& js) const { - if (useQR_) - return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(*Base::jointFactorGraph(js,&EliminateQROrdered))); - else - return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(*Base::jointFactorGraph(js,&EliminatePreferCholeskyOrdered))); -} - -/* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const { - if (useQR_) - return Base::marginalFactor(j,&EliminateQROrdered); - else - return Base::marginalFactor(j,&EliminatePreferCholeskyOrdered); -} - -/* ************************************************************************* */ -Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const { - FactorGraphOrdered fg; - GaussianConditionalOrdered::shared_ptr conditional; - if (useQR_) { - fg.push_back(Base::marginalFactor(j,&EliminateQROrdered)); - conditional = EliminateQROrdered(fg,1).first; - } else { - fg.push_back(Base::marginalFactor(j,&EliminatePreferCholeskyOrdered)); - conditional = EliminatePreferCholeskyOrdered(fg,1).first; - } - return conditional->information().inverse(); -} - -} diff --git a/gtsam/linear/GaussianMultifrontalSolver.h b/gtsam/linear/GaussianMultifrontalSolver.h deleted file mode 100644 index 4b7460773..000000000 --- a/gtsam/linear/GaussianMultifrontalSolver.h +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Oct 21, 2010 - */ - -#pragma once - -#include -#include -#include -#include - -#include - -namespace gtsam { - -/** - * This solver uses multifrontal elimination to solve a GaussianFactorGraph, - * i.e. a sparse linear system. Underlying this is a junction tree, which is - * eliminated into a Bayes tree. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. A fill-reducing - * ordering is computed symbolically from the NonlinearFactorGraph, on the - * nonlinear side of gtsam. (It is actually also possible to permute an - * existing GaussianFactorGraph into a COLAMD ordering instead, this is done - * when computing marginals). - * - * The JunctionTree recursively produces a BayesTree, - * on which this class calls optimize(...) to perform back-substitution. - */ -class GaussianMultifrontalSolver : GenericMultifrontalSolver { - -protected: - - typedef GenericMultifrontalSolver Base; - typedef boost::shared_ptr shared_ptr; - - /** flag to determine whether to use Cholesky or QR */ - bool useQR_; - -public: - - /** - * Construct the solver for a factor graph. This builds the junction - * tree, which already does some of the work of elimination. - */ - GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraphOrdered& factorGraph, bool useQR = false); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false); - - /** - * Named constructor to return a shared_ptr. This builds the elimination - * tree, which already does some of the symbolic work of elimination. - */ - GTSAM_EXPORT static shared_ptr Create(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false); - - /** - * Return a new solver that solves the given factor graph, which must have - * the *same structure* as the one this solver solves. For some solvers this - * is more efficient than constructing the solver from scratch. This can be - * used in cases where the numerical values of the linear problem change, - * e.g. during iterative nonlinear optimization. - */ - GTSAM_EXPORT void replaceFactors(const FactorGraphOrdered::shared_ptr& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - GTSAM_EXPORT GaussianBayesTreeOrdered::shared_ptr eliminate() const; - - /** - * Compute the least-squares solution of the GaussianFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - GTSAM_EXPORT VectorValuesOrdered::shared_ptr optimize() const; - - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - * - * NOTE: This function is limited to computing a joint on 2 variables - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector& js) const; - - /** - * Compute the marginal Gaussian density over a variable, 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 GaussianConditional with - * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) - */ - GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a mean - * vector and covariance matrix. Compared to marginalCanonical, which - * returns a GaussianConditional, this function back-substitutes the R factor - * to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$. - */ - GTSAM_EXPORT Matrix marginalCovariance(Index j) const; - - /** @return true if using QR */ - inline bool usingQR() const { return useQR_; } - -}; - -} - - diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp deleted file mode 100644 index 85012d62f..000000000 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianSequentialSolver.cpp - * @author Richard Roberts - * @date Oct 19, 2010 - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -GaussianSequentialSolver::GaussianSequentialSolver( - const FactorGraphOrdered& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) { -} - -/* ************************************************************************* */ -GaussianSequentialSolver::GaussianSequentialSolver( - const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) { -} - -/* ************************************************************************* */ -GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create( - const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) { - return shared_ptr( - new GaussianSequentialSolver(factorGraph, variableIndex, useQR)); -} - -/* ************************************************************************* */ -void GaussianSequentialSolver::replaceFactors( - const FactorGraphOrdered::shared_ptr& factorGraph) { - Base::replaceFactors(factorGraph); -} - -/* ************************************************************************* */ -GaussianBayesNetOrdered::shared_ptr GaussianSequentialSolver::eliminate() const { - if (useQR_) - return Base::eliminate(&EliminateQROrdered); - else - return Base::eliminate(&EliminatePreferCholeskyOrdered); -} - -/* ************************************************************************* */ -VectorValuesOrdered::shared_ptr GaussianSequentialSolver::optimize() const { - - static const bool debug = false; - - if(debug) this->factors_->print("GaussianSequentialSolver, eliminating "); - if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree "); - - gttic(eliminate); - // Eliminate using the elimination tree - GaussianBayesNetOrdered::shared_ptr bayesNet(this->eliminate()); - gttoc(eliminate); - - if(debug) bayesNet->print("GaussianSequentialSolver, Bayes net "); - - // Allocate the solution vector if it is not already allocated -// VectorValuesOrdered::shared_ptr solution = allocateVectorValues(*bayesNet); - - gttic(optimize); - // Back-substitute - VectorValuesOrdered::shared_ptr solution( - new VectorValuesOrdered(gtsam::optimize(*bayesNet))); - gttoc(optimize); - - if(debug) solution->print("GaussianSequentialSolver, solution "); - - return solution; -} - -/* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const { - if (useQR_) - return Base::marginalFactor(j,&EliminateQROrdered); - else - return Base::marginalFactor(j,&EliminatePreferCholeskyOrdered); -} - -/* ************************************************************************* */ -Matrix GaussianSequentialSolver::marginalCovariance(Index j) const { - FactorGraphOrdered fg; - GaussianConditionalOrdered::shared_ptr conditional; - if (useQR_) { - fg.push_back(Base::marginalFactor(j, &EliminateQROrdered)); - conditional = EliminateQROrdered(fg, 1).first; - } else { - fg.push_back(Base::marginalFactor(j, &EliminatePreferCholeskyOrdered)); - conditional = EliminatePreferCholeskyOrdered(fg, 1).first; - } - return conditional->information().inverse(); -} - -/* ************************************************************************* */ -GaussianFactorGraphOrdered::shared_ptr -GaussianSequentialSolver::jointFactorGraph(const std::vector& js) const { - if (useQR_) - return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered( - *Base::jointFactorGraph(js, &EliminateQROrdered))); - else - return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered( - *Base::jointFactorGraph(js, &EliminatePreferCholeskyOrdered))); -} - -} /// namespace gtsam diff --git a/gtsam/linear/GaussianSequentialSolver.h b/gtsam/linear/GaussianSequentialSolver.h deleted file mode 100644 index ec236c49c..000000000 --- a/gtsam/linear/GaussianSequentialSolver.h +++ /dev/null @@ -1,133 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 GaussianSequentialSolver.h - * @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination. - * @author Richard Roberts - * @date Oct 19, 2010 - */ - -#pragma once - -#include -#include -#include - -#include - -namespace gtsam { - -/** This solver uses sequential variable elimination to solve a - * GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a - * column elimination tree (inference/EliminationTree), see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. A fill-reducing - * ordering is computed symbolically from the NonlinearFactorGraph, on the - * nonlinear side of gtsam. (To be precise, it is possible to permute an - * existing GaussianFactorGraph into a COLAMD ordering instead, this is done - * when computing marginals). - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which performs Multi-frontal QR factorization. However, - * sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * - * The EliminationTree recursively produces a BayesNet, - * typedef'ed in linear/GaussianBayesNet, on which this class calls - * optimize(...) to perform back-substitution. - */ -class GaussianSequentialSolver : GenericSequentialSolver { - -protected: - - typedef GenericSequentialSolver Base; - typedef boost::shared_ptr shared_ptr; - - /** flag to determine whether to use Cholesky or QR */ - bool useQR_; - -public: - - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GTSAM_EXPORT GaussianSequentialSolver(const FactorGraphOrdered& factorGraph, bool useQR = false); - - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GTSAM_EXPORT GaussianSequentialSolver(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false); - - /** - * Named constructor to return a shared_ptr. This builds the elimination - * tree, which already does some of the symbolic work of elimination. - */ - GTSAM_EXPORT static shared_ptr Create(const FactorGraphOrdered::shared_ptr& factorGraph, - const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false); - - /** - * Return a new solver that solves the given factor graph, which must have - * the *same structure* as the one this solver solves. For some solvers this - * is more efficient than constructing the solver from scratch. This can be - * used in cases where the numerical values of the linear problem change, - * e.g. during iterative nonlinear optimization. - */ - GTSAM_EXPORT void replaceFactors(const FactorGraphOrdered::shared_ptr& factorGraph); - - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - GTSAM_EXPORT GaussianBayesNetOrdered::shared_ptr eliminate() const; - - /** - * Compute the least-squares solution of the GaussianFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - GTSAM_EXPORT VectorValuesOrdered::shared_ptr optimize() const; - - /** - * Compute the marginal Gaussian density over a variable, 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 GaussianConditional with - * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) - */ - GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const; - - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a mean - * vector and covariance matrix. Compared to marginalCanonical, which - * returns a GaussianConditional, this function back-substitutes the R factor - * to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$. - */ - GTSAM_EXPORT Matrix marginalCovariance(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(...) - */ - GTSAM_EXPORT GaussianFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector& js) const; - -}; - -} - diff --git a/gtsam/linear/HessianFactorOrdered.cpp b/gtsam/linear/HessianFactorOrdered.cpp deleted file mode 100644 index a619568c4..000000000 --- a/gtsam/linear/HessianFactorOrdered.cpp +++ /dev/null @@ -1,560 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HessianFactor.cpp - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#include - -#include -#include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -string SlotEntryOrdered::toString() const { - ostringstream oss; - oss << "SlotEntryOrdered: slot=" << slot << ", dim=" << dimension; - return oss.str(); -} - -/* ************************************************************************* */ -ScatterOrdered::ScatterOrdered(const FactorGraphOrdered& gfg) { - // First do the set union. - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) { - if(factor) { - for(GaussianFactorOrdered::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - this->insert(make_pair(*variable, SlotEntryOrdered(0, factor->getDim(variable)))); - } - } - } - - // Next fill in the slot indices (we can only get these after doing the set - // union. - size_t slot = 0; - BOOST_FOREACH(value_type& var_slot, *this) { - var_slot.second.slot = (slot ++); - } -} - -/* ************************************************************************* */ -void HessianFactorOrdered::assertInvariants() const { - GaussianFactorOrdered::assertInvariants(); // The base class checks for unique keys -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(const HessianFactorOrdered& gf) : - GaussianFactorOrdered(gf), info_(matrix_) { - info_.assignNoalias(gf.info_); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered() : info_(matrix_) { - // The empty HessianFactor has only a constant error term of zero - FastVector dims; - dims.push_back(1); - info_.resize(dims.begin(), dims.end(), false); - info_(0,0)(0,0) = 0.0; - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(Index j, const Matrix& G, const Vector& g, double f) : - GaussianFactorOrdered(j), info_(matrix_) { - if(G.rows() != G.cols() || G.rows() != g.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+2); - infoMatrix(0,0) = G; - infoMatrix.column(0,1,0) = g; - infoMatrix(1,1)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); -} - -/* ************************************************************************* */ -// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) -// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g -HessianFactorOrdered::HessianFactorOrdered(Index j, const Vector& mu, const Matrix& Sigma) : - GaussianFactorOrdered(j), info_(matrix_) { - if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - Matrix G = inverse(Sigma); - Vector g = G * mu; - double f = dot(mu, g); - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims + 2); - infoMatrix(0, 0) = G; - infoMatrix.column(0, 1, 0) = g; - infoMatrix(1, 1)(0, 0) = f; - infoMatrix.swap(info_); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(Index j1, Index j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : - GaussianFactorOrdered(j1, j2), info_(matrix_) { - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G22.cols() != g2.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G11.rows(), G22.rows(), 1 }; - InfoMatrix fullMatrix(G11.rows() + G22.rows() + 1, G11.rows() + G22.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+3); - infoMatrix(0,0) = G11; - infoMatrix(0,1) = G12; - infoMatrix.column(0,2,0) = g1; - infoMatrix(1,1) = G22; - infoMatrix.column(1,2,0) = g2; - infoMatrix(2,2)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(Index j1, Index j2, Index j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : - GaussianFactorOrdered(j1, j2, j3), info_(matrix_) { - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - size_t dims[] = { G11.rows(), G22.rows(), G33.rows(), 1 }; - InfoMatrix fullMatrix(G11.rows() + G22.rows() + G33.rows() + 1, G11.rows() + G22.rows() + G33.rows() + 1); - BlockInfo infoMatrix(fullMatrix, dims, dims+4); - infoMatrix(0,0) = G11; - infoMatrix(0,1) = G12; - infoMatrix(0,2) = G13; - infoMatrix.column(0,3,0) = g1; - infoMatrix(1,1) = G22; - infoMatrix(1,2) = G23; - infoMatrix.column(1,3,0) = g2; - infoMatrix(2,2) = G33; - infoMatrix.column(2,3,0) = g3; - infoMatrix(3,3)(0,0) = f; - infoMatrix.swap(info_); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : GaussianFactorOrdered(js), info_(matrix_) { - - // Get the number of variables - size_t variable_count = js.size(); - - // Verify the provided number of entries in the vectors are consistent - if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) - throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ - in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); - - // Verify the dimensions of each provided matrix are consistent - // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula - for(size_t i = 0; i < variable_count; ++i){ - int block_size = gs[i].size(); - // Check rows - for(size_t j = 0; j < variable_count-i; ++j){ - size_t index = i*(2*variable_count - i + 1)/2 + j; - if(Gs[index].rows() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - } - } - // Check cols - for(size_t j = 0; j <= i; ++j){ - size_t index = j*(2*variable_count - j + 1)/2 + (i-j); - if(Gs[index].cols() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - } - } - } - - // Create the dims vector - size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google. - size_t total_size = 0; - for(unsigned int i = 0; i < variable_count; ++i){ - dims[i] = gs[i].size(); - total_size += gs[i].size(); - } - dims[variable_count] = 1; - total_size += 1; - - // Fill in the internal matrix with the supplied blocks - InfoMatrix fullMatrix(total_size, total_size); - BlockInfo infoMatrix(fullMatrix, dims, dims+variable_count+1); - size_t index = 0; - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ - infoMatrix(i,j) = Gs[index++]; - } - infoMatrix.column(i,variable_count,0) = gs[i]; - } - infoMatrix(variable_count,variable_count)(0,0) = f; - - // update the BlockView variable - infoMatrix.swap(info_); - - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(const GaussianConditionalOrdered& cg) : GaussianFactorOrdered(cg), info_(matrix_) { - JacobianFactorOrdered jf(cg); - info_.copyStructureFrom(jf.Ab_); - matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_; - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(const GaussianFactorOrdered& gf) : info_(matrix_) { - // Copy the variable indices - (GaussianFactorOrdered&)(*this) = gf; - // Copy the matrix data depending on what type of factor we're copying from - if(dynamic_cast(&gf)) { - const JacobianFactorOrdered& jf(static_cast(gf)); - if(jf.model_->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - else { - Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); - info_.copyStructureFrom(jf.Ab_); - BlockInfo::constBlock A = jf.Ab_.full(); - matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; - } - } else if(dynamic_cast(&gf)) { - const HessianFactorOrdered& hf(static_cast(gf)); - info_.assignNoalias(hf.info_); - } else - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered& factors) : info_(matrix_) -{ - ScatterOrdered scatter(factors); - - // Pull out keys and dimensions - gttic(keys); - vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const ScatterOrdered::value_type& var_slot, scatter) { - dimensions[var_slot.second.slot] = var_slot.second.dimension; - } - // This is for the r.h.s. vector - dimensions.back() = 1; - gttoc(keys); - - const bool debug = ISDEBUG("EliminateCholesky"); - // Form Ab' * Ab - gttic(allocate); - info_.resize(dimensions.begin(), dimensions.end(), false); - // Fill in keys - keys_.resize(scatter.size()); - std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&ScatterOrdered::value_type::first, ::_1)); - gttoc(allocate); - gttic(zero); - matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); - gttoc(zero); - gttic(update); - if (debug) cout << "Combining " << factors.size() << " factors" << endl; - BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) - { - if(factor) { - if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) - updateATA(*hessian, scatter); - else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) - updateATA(*jacobianFactor, scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } - gttoc(update); - - if (debug) gtsam::print(matrix_, "Ab' * Ab: "); - - assertInvariants(); -} - -/* ************************************************************************* */ -HessianFactorOrdered& HessianFactorOrdered::operator=(const HessianFactorOrdered& rhs) { - this->Base::operator=(rhs); // Copy keys - info_.assignNoalias(rhs.info_); // Copy matrix and block structure - return *this; -} - -/* ************************************************************************* */ -void HessianFactorOrdered::print(const std::string& s, const IndexFormatter& formatter) const { - cout << s << "\n"; - cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; - cout << "\n"; - gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); -} - -/* ************************************************************************* */ -bool HessianFactorOrdered::equals(const GaussianFactorOrdered& lf, double tol) const { - if(!dynamic_cast(&lf)) - return false; - else { - Matrix thisMatrix = this->info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } -} - -/* ************************************************************************* */ -Matrix HessianFactorOrdered::augmentedInformation() const { - return info_.full().selfadjointView(); -} - -/* ************************************************************************* */ -Matrix HessianFactorOrdered::information() const { - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); -} - -/* ************************************************************************* */ -double HessianFactorOrdered::error(const VectorValuesOrdered& c) const { - // error 0.5*(f - 2*x'*g + x'*G*x) - const double f = constantTerm(); - double xtg = 0, xGx = 0; - // extract the relevant subset of the VectorValues - // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); - xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - return 0.5 * (f - 2.0 * xtg + xGx); -} - -/* ************************************************************************* */ -void HessianFactorOrdered::updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter) { - - // This function updates 'combined' with the information in 'update'. - // 'scatter' maps variables in the update factor to slots in the combined - // factor. - - const bool debug = ISDEBUG("updateATA"); - - // First build an array of slots - gttic(slots); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. - size_t slot = 0; - BOOST_FOREACH(Index j, update) { - slots[slot] = scatter.find(j)->second.slot; - ++ slot; - } - gttoc(slots); - - if(debug) { - this->print("Updating this: "); - update.print("with (Hessian): "); - } - - // Apply updates to the upper triangle - gttic(update); - for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; - for(size_t j1=0; j1<=j2; ++j1) { - size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; - if(slot2 > slot1) { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - } else if(slot1 > slot2) { - if(debug) - cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose(); - } else { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); - } - if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - if(debug) this->print(); - } - } - gttoc(update); -} - -/* ************************************************************************* */ -void HessianFactorOrdered::updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter) { - - // This function updates 'combined' with the information in 'update'. - // 'scatter' maps variables in the update factor to slots in the combined - // factor. - - const bool debug = ISDEBUG("updateATA"); - - // First build an array of slots - gttic(slots); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. - size_t slot = 0; - BOOST_FOREACH(Index j, update) { - slots[slot] = scatter.find(j)->second.slot; - ++ slot; - } - gttoc(slots); - - gttic(form_ATA); - if(update.model_->isConstrained()) - throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); - - if(debug) { - this->print("Updating this: "); - update.print("with (Jacobian): "); - } - - typedef Eigen::Block BlockUpdateMatrix; - BlockUpdateMatrix updateA(update.matrix_.block( - update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); - if (debug) cout << "updateA: \n" << updateA << endl; - - Matrix updateInform; - if(boost::dynamic_pointer_cast(update.model_)) { - updateInform.noalias() = updateA.transpose() * updateA; - } else { - noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); - if(diagonal) { - Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); - updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; - } else - throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); - } - if (debug) cout << "updateInform: \n" << updateInform << endl; - gttoc(form_ATA); - - // Apply updates to the upper triangle - gttic(update); - for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; - for(size_t j1=0; j1<=j2; ++j1) { - size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; - size_t off0 = update.Ab_.offset(0); - if(slot2 > slot1) { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); - } else if(slot1 > slot2) { - if(debug) - cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose(); - } else { - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += - updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); - } - if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; - if(debug) this->print(); - } - } - gttoc(update); -} - -/* ************************************************************************* */ -void HessianFactorOrdered::partialCholesky(size_t nrFrontals) { - if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) - throw IndeterminantLinearSystemException(this->keys().front()); -} - -/* ************************************************************************* */ -GaussianConditionalOrdered::shared_ptr HessianFactorOrdered::splitEliminatedFactor(size_t nrFrontals) { - - static const bool debug = false; - - // Extract conditionals - gttic(extract_conditionals); - GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered()); - typedef VerticalBlockView BlockAb; - BlockAb Ab(matrix_, info_); - - size_t varDim = info_.offset(nrFrontals); - Ab.rowEnd() = Ab.rowStart() + varDim; - - // Create one big conditionals with many frontal variables. - gttic(construct_cond); - Vector sigmas = Vector::Ones(varDim); - conditional = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); - gttoc(construct_cond); - if(debug) conditional->print("Extracted conditional: "); - - gttoc(extract_conditionals); - - // Take lower-right block of Ab_ to get the new factor - gttic(remaining_factor); - info_.blockStart() = nrFrontals; - // Assign the keys - vector remainingKeys(keys_.size() - nrFrontals); - remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); - keys_.swap(remainingKeys); - gttoc(remaining_factor); - - return conditional; -} - -/* ************************************************************************* */ -GaussianFactorOrdered::shared_ptr HessianFactorOrdered::negate() const { - // Copy Hessian Blocks from Hessian factor and invert - std::vector js; - std::vector Gs; - std::vector gs; - double f; - js.insert(js.end(), begin(), end()); - for(size_t i = 0; i < js.size(); ++i){ - for(size_t j = i; j < js.size(); ++j){ - Gs.push_back( -info(begin()+i, begin()+j) ); - } - gs.push_back( -linearTerm(begin()+i) ); - } - f = -constantTerm(); - - // Create the Anti-Hessian Factor from the negated blocks - return HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f)); -} - -} // gtsam diff --git a/gtsam/linear/HessianFactorOrdered.h b/gtsam/linear/HessianFactorOrdered.h deleted file mode 100644 index 9ed884345..000000000 --- a/gtsam/linear/HessianFactorOrdered.h +++ /dev/null @@ -1,377 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 HessianFactor.h - * @brief Contains the HessianFactor class, a general quadratic factor - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#pragma once - -#include -#include -#include - -// Forward declarations for friend unit tests -class HessianFactorOrderedConversionConstructorTest; -class HessianFactorOrderedConstructor1Test; -class HessianFactorOrderedConstructor1bTest; -class HessianFactorOrderedcombineTest; - - -namespace gtsam { - - // Forward declarations - class JacobianFactorOrdered; - class GaussianConditionalOrdered; - template class BayesNetOrdered; - - // Definition of ScatterOrdered, which is an intermediate data structure used when - // building a HessianFactor incrementally, to get the keys in the right - // order. - // The "scatter" is a map from global variable indices to slot indices in the - // union of involved variables. We also include the dimensionality of the - // variable. - struct GTSAM_EXPORT SlotEntryOrdered { - size_t slot; - size_t dimension; - SlotEntryOrdered(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - class ScatterOrdered : public FastMap { - public: - ScatterOrdered() {} - ScatterOrdered(const FactorGraphOrdered& gfg); - }; - - /** - * @brief A Gaussian factor using the canonical parameters (information form) - * - * HessianFactor implements a general quadratic factor of the form - * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] - * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. - * - * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, - * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, - * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual - * sum-square-error at the mean, when \f$ x = \mu \f$. - * - * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) - * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ - * with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get - * @f[ - * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu - * @f] - * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ - * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ - * to arrive at the canonical form of the Gaussian: - * @f[ - * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu - * @f] - * - * This factor is one of the factors that can be in a GaussianFactorGraph. - * It may be returned from NonlinearFactor::linearize(), but is also - * used internally to store the Hessian during Cholesky elimination. - * - * This can represent a quadratic factor with characteristics that cannot be - * represented using a JacobianFactor (which has the form - * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ - * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, - * a HessianFactor need not be positive semidefinite, it can be indefinite or - * even negative semidefinite. - * - * If a HessianFactor is indefinite or negative semi-definite, then in order - * for solving the linear system to be possible, - * the Hessian of the full system must be positive definite (i.e. when all - * small Hessians are combined, the result must be positive definite). If - * this is not the case, an error will occur during elimination. - * - * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. - * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right - * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ - * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. - * \code - HessianFactor::matrix_ = [ G11 G12 G13 ... g1 - 0 G22 G23 ... g2 - 0 0 G33 ... g3 - : : : : - 0 0 0 ... f ] - \endcode - Blocks can be accessed as follows: - \code - G11 = info(begin(), begin()); - G12 = info(begin(), begin()+1); - G23 = info(begin()+1, begin()+2); - g2 = linearTerm(begin()+1); - f = constantTerm(); - ....... - \endcode - */ - class GTSAM_EXPORT HessianFactorOrdered : public GaussianFactorOrdered { - protected: - typedef Matrix InfoMatrix; ///< The full augmented Hessian - typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian - typedef GaussianFactorOrdered Base; ///< Typedef to base class - - InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] - BlockInfo info_; ///< The block view of the full information matrix. - - public: - - typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this - typedef BlockInfo::Block Block; ///< A block from the Hessian matrix - typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) - typedef BlockInfo::Column Column; ///< A column containing the linear term h - typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) - - /** Copy constructor */ - HessianFactorOrdered(const HessianFactorOrdered& gf); - - /** default constructor for I/O */ - HessianFactorOrdered(); - - /** Construct a unary factor. G is the quadratic term (Hessian matrix), g - * the linear term (a vector), and f the constant term. The quadratic - * error is: - * 0.5*(f - 2*x'*g + x'*G*x) - */ - HessianFactorOrdered(Index j, const Matrix& G, const Vector& g, double f); - - /** Construct a unary factor, given a mean and covariance matrix. - * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) - */ - HessianFactorOrdered(Index j, const Vector& mu, const Matrix& Sigma); - - /** Construct a binary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] - * HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f] - * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have - \code - n1*n1 G11 = A1'*M*A1 - n1*n2 G12 = A1'*M*A2 - n2*n2 G22 = A2'*M*A2 - n1*1 g1 = A1'*M*b - n2*1 g2 = A2'*M*b - 1*1 f = b'*M*b - \endcode - */ - HessianFactorOrdered(Index j1, Index j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f); - - /** Construct a ternary factor. Gxx are the upper-triangle blocks of the - * quadratic term (the Hessian matrix), gx the pieces of the linear vector - * term, and f the constant term. - */ - HessianFactorOrdered(Index j1, Index j2, Index j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f); - - /** Construct an n-way factor. Gs contains the upper-triangle blocks of the - * quadratic term (the Hessian matrix) provided in row-order, gs the pieces - * of the linear vector term, and f the constant term. - */ - HessianFactorOrdered(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f); - - /** Construct from Conditional Gaussian */ - explicit HessianFactorOrdered(const GaussianConditionalOrdered& cg); - - /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ - explicit HessianFactorOrdered(const GaussianFactorOrdered& factor); - - /** Combine a set of factors into a single dense HessianFactor */ - HessianFactorOrdered(const FactorGraphOrdered& factors); - - /** Destructor */ - virtual ~HessianFactorOrdered() {} - - /** Aassignment operator */ - HessianFactorOrdered& operator=(const HessianFactorOrdered& rhs); - - /** Clone this JacobianFactor */ - virtual GaussianFactorOrdered::shared_ptr clone() const { - return boost::static_pointer_cast( - shared_ptr(new HessianFactorOrdered(*this))); - } - - /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** Compare to another factor for testing (implementing Testable) */ - virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const; - - /** Evaluate the factor error f(x), see above. */ - virtual double error(const VectorValuesOrdered& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ - - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - * @param variable An iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - */ - virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } - - /** Return the number of columns and rows of the Hessian matrix */ - size_t rows() const { return info_.rows(); } - - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactorOrdered::shared_ptr negate() const; - - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. - */ - constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } - - /** Return a view of the block at (j1,j2) of the upper-triangular part of the - * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation - * above to explain that only the upper-triangular part of the information matrix is stored - * and returned by this function. - * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return A view of the requested block, not a copy. - */ - Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - constBlock info() const { return info_.full(); } - - /** Return the upper-triangular part of the full *augmented* information matrix, - * as described above. See HessianFactor class documentation above to explain that only the - * upper-triangular part of the information matrix is stored and returned by this function. - */ - Block info() { return info_.full(); } - - /** Return the constant term \f$ f \f$ as described above - * @return The constant term \f$ f \f$ - */ - double constantTerm() const { return info_(this->size(), this->size())(0,0); } - - /** Return the constant term \f$ f \f$ as described above - * @return The constant term \f$ f \f$ - */ - double& constantTerm() { return info_(this->size(), this->size())(0,0); } - - /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. - * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return The linear term \f$ g \f$ */ - constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } - - /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. - * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can - * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return The linear term \f$ g \f$ */ - Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); } - - /** Return the complete linear term \f$ g \f$ as described above. - * @return The linear term \f$ g \f$ */ - constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } - - /** Return the complete linear term \f$ g \f$ as described above. - * @return The linear term \f$ g \f$ */ - Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); } - - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - * - * For HessianFactor, this is the same as info() except that this function - * returns a complete symmetric matrix whereas info() returns a matrix where - * only the upper triangle is valid, but should be interpreted as symmetric. - * This is because info() returns only a reference to the internal - * representation of the augmented information matrix, which stores only the - * upper triangle. - */ - virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; - - // Friend unit test classes - friend class ::HessianFactorOrderedConversionConstructorTest; - friend class ::HessianFactorOrderedConstructor1Test; - friend class ::HessianFactorOrderedConstructor1bTest; - friend class ::HessianFactorOrderedcombineTest; - - // Friend JacobianFactor for conversion - friend class JacobianFactorOrdered; - - // used in eliminateCholesky: - - /** - * Do Cholesky. Note that after this, the lower triangle still contains - * some untouched non-zeros that should be zero. We zero them while - * extracting submatrices in splitEliminatedFactor. Frank says :-( - */ - void partialCholesky(size_t nrFrontals); - - /** split partially eliminated factor */ - boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); - - /** Update the factor by adding the information from the JacobianFactor - * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter); - - /** assert invariants */ - void assertInvariants() const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactorOrdered); - ar & BOOST_SERIALIZATION_NVP(info_); - ar & BOOST_SERIALIZATION_NVP(matrix_); - } - }; - -} - diff --git a/gtsam/linear/JacobianFactorOrdered.cpp b/gtsam/linear/JacobianFactorOrdered.cpp deleted file mode 100644 index ced8881ba..000000000 --- a/gtsam/linear/JacobianFactorOrdered.cpp +++ /dev/null @@ -1,533 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JacobianFactor.cpp - * @author Richard Roberts - * @date Dec 8, 2010 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif - -#include -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - void JacobianFactorOrdered::assertInvariants() const { -#ifndef NDEBUG - GaussianFactorOrdered::assertInvariants(); // The base class checks for unique keys - assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks()); -#endif - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const JacobianFactorOrdered& gf) : - GaussianFactorOrdered(gf), model_(gf.model_), Ab_(matrix_) { - Ab_.assignNoalias(gf.Ab_); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const GaussianFactorOrdered& gf) : Ab_(matrix_) { - // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactorOrdered* rhs = dynamic_cast(&gf)) - *this = JacobianFactorOrdered(*rhs); - else if(const HessianFactorOrdered* rhs = dynamic_cast(&gf)) - *this = JacobianFactorOrdered(*rhs); - else - throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered() : Ab_(matrix_) { assertInvariants(); } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const Vector& b_in) : Ab_(matrix_) { - size_t dims[] = { 1 }; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size())); - getb() = b_in; - model_ = noiseModel::Unit::Create(this->rows()); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) : - GaussianFactorOrdered(i1), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size())); - Ab_(0) = A1; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) : - GaussianFactorOrdered(i1,i2), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), A2.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size())); - Ab_(0) = A1; - Ab_(1) = A2; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : - GaussianFactorOrdered(i1,i2,i3), model_(model), Ab_(matrix_) { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t dims[] = { A1.cols(), A2.cols(), A3.cols(), 1}; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size())); - Ab_(0) = A1; - Ab_(1) = A2; - Ab_(2) = A3; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) : - GaussianFactorOrdered(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), Ab_(matrix_) - { - // get number of measurements and variables involved in this factor - size_t m = b.size(), n = terms.size(); - - if(model->dim() != (size_t) m) - throw InvalidNoiseModel(b.size(), model->dim()); - - // Get the dimensions of each variable and copy to "dims" array, add 1 for RHS - size_t* dims = (size_t*)alloca(sizeof(size_t)*(n+1)); // FIXME: alloca is bad, just ask Google. - for(size_t j=0; j > &terms, - const Vector &b, const SharedDiagonal& model) : - GaussianFactorOrdered(GetKeys(terms.size(), terms.begin(), terms.end())), - model_(model), Ab_(matrix_) - { - - if(model->dim() != (size_t) b.size()) - throw InvalidNoiseModel(b.size(), model->dim()); - - size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. - size_t j=0; - std::list >::const_iterator term=terms.begin(); - for(; term!=terms.end(); ++term,++j) - dims[j] = term->second.cols(); - dims[j] = 1; - Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size())); - j = 0; - for(term=terms.begin(); term!=terms.end(); ++term,++j) - Ab_(j) = term->second; - getb() = b; - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const GaussianConditionalOrdered& cg) : - GaussianFactorOrdered(cg), - model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), - Ab_(matrix_) { - Ab_.assignNoalias(cg.rsd_); - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const HessianFactorOrdered& factor) : Ab_(matrix_) { - keys_ = factor.keys_; - Ab_.assignNoalias(factor.info_); - - // Do Cholesky to get a Jacobian - size_t maxrank; - bool success; - boost::tie(maxrank, success) = choleskyCareful(matrix_); - - // Check for indefinite system - if(!success) - throw IndeterminantLinearSystemException(factor.keys().front()); - - // Zero out lower triangle - matrix_.topRows(maxrank).triangularView() = - Matrix::Zero(maxrank, matrix_.cols()); - // FIXME: replace with triangular system - Ab_.rowEnd() = maxrank; - model_ = noiseModel::Unit::Create(maxrank); - - assertInvariants(); - } - - /* ************************************************************************* */ - JacobianFactorOrdered::JacobianFactorOrdered(const GaussianFactorGraphOrdered& gfg) : Ab_(matrix_) { - // Cast or convert to Jacobians - FactorGraphOrdered jacobians; - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { - if(factor) { - if(JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - jacobians.push_back(jf); - else - jacobians.push_back(boost::make_shared(*factor)); - } - } - - *this = *CombineJacobians(jacobians, VariableSlots(jacobians)); - } - - /* ************************************************************************* */ - JacobianFactorOrdered& JacobianFactorOrdered::operator=(const JacobianFactorOrdered& rhs) { - this->Base::operator=(rhs); // Copy keys - model_ = rhs.model_; // Copy noise model - Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure - assertInvariants(); - return *this; - } - - /* ************************************************************************* */ - void JacobianFactorOrdered::print(const string& s, const IndexFormatter& formatter) const { - cout << s << "\n"; - if (empty()) { - cout << " empty, keys: "; - BOOST_FOREACH(const Index& key, keys()) { cout << formatter(key) << " "; } - cout << endl; - } else { - for(const_iterator key=begin(); key!=end(); ++key) - cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl; - cout << "b=" << getb() << endl; - model_->print("model"); - } - } - - /* ************************************************************************* */ - // Check if two linear factors are equal - bool JacobianFactorOrdered::equals(const GaussianFactorOrdered& f_, double tol) const { - if(!dynamic_cast(&f_)) - return false; - else { - const JacobianFactorOrdered& f(static_cast(f_)); - if (empty()) return (f.empty()); - if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/) - return false; - - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) - return false; - - constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); - constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); - for(size_t row=0; row< (size_t) Ab1.rows(); ++row) - if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) && - !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) - return false; - - return true; - } - } - - /* ************************************************************************* */ - Vector JacobianFactorOrdered::unweighted_error(const VectorValuesOrdered& c) const { - Vector e = -getb(); - if (empty()) return e; - for(size_t pos=0; poswhiten(-getb()); - return model_->whiten(unweighted_error(c)); - } - - /* ************************************************************************* */ - double JacobianFactorOrdered::error(const VectorValuesOrdered& c) const { - if (empty()) return 0; - Vector weighted = error_vector(c); - return 0.5 * weighted.dot(weighted); - } - - /* ************************************************************************* */ - Matrix JacobianFactorOrdered::augmentedInformation() const { - Matrix AbWhitened = Ab_.full(); - model_->WhitenInPlace(AbWhitened); - return AbWhitened.transpose() * AbWhitened; - } - - /* ************************************************************************* */ - Matrix JacobianFactorOrdered::information() const { - Matrix AWhitened = this->getA(); - model_->WhitenInPlace(AWhitened); - return AWhitened.transpose() * AWhitened; - } - - /* ************************************************************************* */ - Vector JacobianFactorOrdered::operator*(const VectorValuesOrdered& x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax); - } - - /* ************************************************************************* */ - void JacobianFactorOrdered::transposeMultiplyAdd(double alpha, const Vector& e, - VectorValuesOrdered& x) const { - Vector E = alpha * model_->whiten(e); - // Just iterate over all A matrices and insert Ai^e into VectorValues - for(size_t pos=0; pos JacobianFactorOrdered::matrix(bool weight) const { - Matrix A(Ab_.range(0, size())); - Vector b(getb()); - // divide in sigma so error is indeed 0.5*|Ax-b| - if (weight) model_->WhitenSystem(A,b); - return make_pair(A, b); - } - - /* ************************************************************************* */ - Matrix JacobianFactorOrdered::matrix_augmented(bool weight) const { - if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; } - else return Ab_.range(0, Ab_.nBlocks()); - } - - /* ************************************************************************* */ - std::vector > - JacobianFactorOrdered::sparse(const std::vector& columnIndices) const { - - std::vector > entries; - - // iterate over all variables in the factor - for(const_iterator var=begin(); varWhiten(getA(var))); - // find first column index for this key - size_t column_start = columnIndices[*var]; - for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) - for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { - double s = whitenedA(i,j); - if (std::abs(s) > 1e-12) entries.push_back( - boost::make_tuple(i, column_start + j, s)); - } - } - - Vector whitenedb(model_->whiten(getb())); - size_t bcolumn = columnIndices.back(); - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i))); - - // return the result - return entries; - } - - /* ************************************************************************* */ - JacobianFactorOrdered JacobianFactorOrdered::whiten() const { - JacobianFactorOrdered result(*this); - result.model_->WhitenInPlace(result.matrix_); - result.model_ = noiseModel::Unit::Create(result.model_->dim()); - return result; - } - - /* ************************************************************************* */ - GaussianFactorOrdered::shared_ptr JacobianFactorOrdered::negate() const { - HessianFactorOrdered hessian(*this); - return hessian.negate(); - } - - /* ************************************************************************* */ - GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::eliminateFirst() { - return this->eliminate(1); - } - - /* ************************************************************************* */ - GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::splitConditional(size_t nrFrontals) { - assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); - assert(size() >= nrFrontals); - assertInvariants(); - - const bool debug = ISDEBUG("JacobianFactor::splitConditional"); - - if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; - if(debug) this->print("Splitting JacobianFactor: "); - - size_t frontalDim = Ab_.range(0,nrFrontals).cols(); - - // Check for singular factor - if(model_->dim() < frontalDim) - throw IndeterminantLinearSystemException(this->keys().front()); - - // Extract conditional - gttic(cond_Rd); - - // Restrict the matrix to be in the first nrFrontals variables - Ab_.rowEnd() = Ab_.rowStart() + frontalDim; - const Eigen::VectorBlock sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); - GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered(begin(), end(), nrFrontals, Ab_, sigmas)); - if(debug) conditional->print("Extracted conditional: "); - Ab_.rowStart() += frontalDim; - Ab_.firstBlock() += nrFrontals; - gttoc(cond_Rd); - - if(debug) conditional->print("Extracted conditional: "); - - gttic(remaining_factor); - // Take lower-right block of Ab to get the new factor - Ab_.rowEnd() = model_->dim(); - keys_.erase(begin(), begin() + nrFrontals); - // Set sigmas with the right model - if (model_->isConstrained()) - model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim())); - else - model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim())); - if(debug) this->print("Eliminated factor: "); - assert(Ab_.rows() <= Ab_.cols()-1); - gttoc(remaining_factor); - - if(debug) print("Eliminated factor: "); - - assertInvariants(); - - return conditional; - } - - /* ************************************************************************* */ - GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::eliminate(size_t nrFrontals) { - - assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); - assert(size() >= nrFrontals); - assertInvariants(); - - const bool debug = ISDEBUG("JacobianFactor::eliminate"); - - if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; - if(debug) this->print("Eliminating JacobianFactor: "); - if(debug) gtsam::print(matrix_, "Augmented Ab: "); - - size_t frontalDim = Ab_.range(0,nrFrontals).cols(); - - if(debug) cout << "frontalDim = " << frontalDim << endl; - - // Use in-place QR dense Ab appropriate to NoiseModel - gttic(QR); - SharedDiagonal noiseModel = model_->QR(matrix_); - // In the new unordered code, empty noise model indicates unit noise model, and I already - // modified QR to return an empty noise model. This just creates a unit noise model in that - // case because this old code does not handle empty noise models. - if(!noiseModel) - noiseModel = noiseModel::Unit::Create(std::min(matrix_.rows(), matrix_.cols() - 1)); - gttoc(QR); - - // Zero the lower-left triangle. todo: not all of these entries actually - // need to be zeroed if we are careful to start copying rows after the last - // structural zero. - if(matrix_.rows() > 0) - for(size_t j=0; j<(size_t) matrix_.cols(); ++j) - for(size_t i=j+1; idim(); ++i) - matrix_(i,j) = 0.0; - - if(debug) gtsam::print(matrix_, "QR result: "); - if(debug) noiseModel->print("QR result noise model: "); - - // Start of next part - model_ = noiseModel; - return splitConditional(nrFrontals); - } - - /* ************************************************************************* */ - void JacobianFactorOrdered::allocate(const VariableSlots& variableSlots, vector< - size_t>& varDims, size_t m) { - keys_.resize(variableSlots.size()); - std::transform(variableSlots.begin(), variableSlots.end(), begin(), - boost::bind(&VariableSlots::const_iterator::value_type::first, _1)); - varDims.push_back(1); - Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); - } - - /* ************************************************************************* */ - void JacobianFactorOrdered::setModel(bool anyConstrained, const Vector& sigmas) { - if((size_t) sigmas.size() != this->rows()) - throw InvalidNoiseModel(this->rows(), sigmas.size()); - if (anyConstrained) - model_ = noiseModel::Constrained::MixedSigmas(sigmas); - else - model_ = noiseModel::Diagonal::Sigmas(sigmas); - } - - /* ************************************************************************* */ - const char* JacobianFactorOrdered::InvalidNoiseModel::what() const throw() { - if(description_.empty()) - description_ = (boost::format( - "A JacobianFactor was attempted to be constructed or modified to use a\n" - "noise model of incompatible dimension. The JacobianFactor has\n" - "dimensionality (i.e. length of error vector) %d but the provided noise\n" - "model has dimensionality %d.") % factorDims % noiseModelDims).str(); - return description_.c_str(); - } - -} diff --git a/gtsam/linear/JacobianFactorOrdered.h b/gtsam/linear/JacobianFactorOrdered.h deleted file mode 100644 index cab39f7fa..000000000 --- a/gtsam/linear/JacobianFactorOrdered.h +++ /dev/null @@ -1,335 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JacobianFactor.h - * @author Richard Roberts - * @date Dec 8, 2010 - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include - -// Forward declarations of friend unit tests -class JacobianFactorOrderedCombine2Test; -class JacobianFactorOrderedeliminateFrontalsTest; -class JacobianFactorOrderedconstructor2Test; - -namespace gtsam { - - // Forward declarations - class HessianFactorOrdered; - class VariableSlots; - template class BayesNetOrdered; - class GaussianFactorGraphOrdered; - - /** - * A Gaussian factor in the squared-error form. - * - * JacobianFactor implements a - * Gaussian, which has quadratic negative log-likelihood - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The - * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model - * \f$ \Sigma \f$ are stored in this class. - * - * This factor represents the sum-of-squares error of a \a linear - * measurement function, and is created upon linearization of a NoiseModelFactor, - * which in turn is a sum-of-squares factor with a nonlinear measurement function. - * - * Here is an example of how this factor represents a sum-of-squares error: - * - * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be - * the actual observed measurement, the residual is - * \f[ f(x) = h(x) - z . \f] - * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this - * measurement, then the negative log-likelihood of the Gaussian induced by this - * measurement model is - * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] - * Because \f$ h(x) \f$ is linear, we can write it as - * \f[ h(x) = Ax + e \f] - * and thus we have - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ b = z - e \f$. - * - * This factor can involve an arbitrary number of variables, and in the - * above example \f$ x \f$ would almost always be only be a subset of the variables - * in the entire factor graph. There are special constructors for 1-, 2-, and 3- - * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. - * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, - * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, - * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ - * and the negative log-likelihood represented by this factor would be - * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] - */ - class GTSAM_EXPORT JacobianFactorOrdered : public GaussianFactorOrdered { - protected: - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; - - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - AbMatrix matrix_; // the full matrix corresponding to the factor - BlockAb Ab_; // the block view of the full matrix - typedef GaussianFactorOrdered Base; // typedef to base - - public: - typedef boost::shared_ptr shared_ptr; - typedef BlockAb::Block ABlock; - typedef BlockAb::constBlock constABlock; - typedef BlockAb::Column BVector; - typedef BlockAb::constColumn constBVector; - - /** Copy constructor */ - JacobianFactorOrdered(const JacobianFactorOrdered& gf); - - /** Convert from other GaussianFactor */ - JacobianFactorOrdered(const GaussianFactorOrdered& gf); - - /** default constructor for I/O */ - JacobianFactorOrdered(); - - /** Construct Null factor */ - JacobianFactorOrdered(const Vector& b_in); - - /** Construct unary factor */ - JacobianFactorOrdered(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model); - - /** Construct binary factor */ - JacobianFactorOrdered(Index i1, const Matrix& A1, - Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model); - - /** Construct ternary factor */ - JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2, - const Matrix& A2, Index i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model); - - /** Construct an n-ary factor */ - JacobianFactorOrdered(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model); - - JacobianFactorOrdered(const std::list > &terms, - const Vector &b, const SharedDiagonal& model); - - /** Construct from Conditional Gaussian */ - JacobianFactorOrdered(const GaussianConditionalOrdered& cg); - - /** Convert from a HessianFactor (does Cholesky) */ - JacobianFactorOrdered(const HessianFactorOrdered& factor); - - /** Build a dense joint factor from all the factors in a factor graph. */ - JacobianFactorOrdered(const GaussianFactorGraphOrdered& gfg); - - /** Virtual destructor */ - virtual ~JacobianFactorOrdered() {} - - /** Aassignment operator */ - JacobianFactorOrdered& operator=(const JacobianFactorOrdered& rhs); - - /** Clone this JacobianFactor */ - virtual GaussianFactorOrdered::shared_ptr clone() const { - return boost::static_pointer_cast( - shared_ptr(new JacobianFactorOrdered(*this))); - } - - // Implementing Testable interface - virtual void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const; - - Vector unweighted_error(const VectorValuesOrdered& c) const; /** (A*x-b) */ - Vector error_vector(const VectorValuesOrdered& c) const; /** (A*x-b)/sigma */ - virtual double error(const VectorValuesOrdered& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ - - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - */ - virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; - - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactorOrdered::shared_ptr negate() const; - - /** Check if the factor contains no information, i.e. zero rows. This does - * not necessarily mean that the factor involves no variables (to check for - * involving no variables use keys().empty()). - */ - bool empty() const { return Ab_.rows() == 0;} - - /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained();} - - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - */ - virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } - - /** - * return the number of rows in the corresponding linear system - */ - size_t rows() const { return Ab_.rows(); } - - /** - * return the number of rows in the corresponding linear system - */ - size_t numberOfRows() const { return rows(); } - - /** - * return the number of columns in the corresponding linear system - */ - size_t cols() const { return Ab_.cols(); } - - /** get a copy of model */ - const SharedDiagonal& get_model() const { return model_; } - - /** get a copy of model (non-const version) */ - SharedDiagonal& get_model() { return model_; } - - /** Get a view of the r.h.s. vector b */ - const constBVector getb() const { return Ab_.column(size(), 0); } - - /** Get a view of the A matrix for the variable pointed to by the given key iterator */ - constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } - - /** Get a view of the A matrix */ - constABlock getA() const { return Ab_.range(0, size()); } - - /** Get a view of the r.h.s. vector b (non-const version) */ - BVector getb() { return Ab_.column(size(), 0); } - - /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ - ABlock getA(iterator variable) { return Ab_(variable - begin()); } - - /** Get a view of the A matrix */ - ABlock getA() { return Ab_.range(0, size()); } - - /** Return A*x */ - Vector operator*(const VectorValuesOrdered& x) const; - - /** x += A'*e */ - void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesOrdered& x) const; - - /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights - */ - std::pair matrix(bool weight = true) const; - - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param set weight to use whitening to bake in weights - */ - Matrix matrix_augmented(bool weight = true) const; - - /** - * Return vector of i, j, and s to generate an m-by-n sparse matrix - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * As above, the standard deviations are baked into A and b - * @param columnIndices First column index for each variable. - */ - std::vector > - sparse(const std::vector& columnIndices) const; - - /** - * Return a whitened version of the factor, i.e. with unit diagonal noise - * model. */ - JacobianFactorOrdered whiten() const; - - /** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */ - boost::shared_ptr eliminateFirst(); - - /** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */ - boost::shared_ptr eliminate(size_t nrFrontals = 1); - - /** - * splits a pre-factorized factor into a conditional, and changes the current - * factor to be the remaining component. Performs same operation as eliminate(), - * but without running QR. - */ - boost::shared_ptr splitConditional(size_t nrFrontals = 1); - - // following methods all used in CombineJacobians: - // Many imperative, perhaps all need to be combined in constructor - - /** allocate space */ - void allocate(const VariableSlots& variableSlots, - std::vector& varDims, size_t m); - - /** set noiseModel correctly */ - void setModel(bool anyConstrained, const Vector& sigmas); - - /** Assert invariants after elimination */ - void assertInvariants() const; - - /** An exception indicating that the noise model dimension passed into the - * JacobianFactor has a different dimensionality than the factor. */ - class InvalidNoiseModel : public std::exception { - public: - const size_t factorDims; ///< The dimensionality of the factor - const size_t noiseModelDims; ///< The dimensionality of the noise model - - InvalidNoiseModel(size_t factorDims, size_t noiseModelDims) : - factorDims(factorDims), noiseModelDims(noiseModelDims) {} - virtual ~InvalidNoiseModel() throw() {} - - virtual const char* what() const throw(); - - private: - mutable std::string description_; - }; - - private: - - // Friend HessianFactor to facilitate conversion constructors - friend class HessianFactorOrdered; - - // Friend unit tests (see also forward declarations above) - friend class ::JacobianFactorOrderedCombine2Test; - friend class ::JacobianFactorOrderedeliminateFrontalsTest; - friend class ::JacobianFactorOrderedconstructor2Test; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactorOrdered); - ar & BOOST_SERIALIZATION_NVP(Ab_); - ar & BOOST_SERIALIZATION_NVP(model_); - ar & BOOST_SERIALIZATION_NVP(matrix_); - } - }; // JacobianFactor - -} // gtsam - diff --git a/gtsam/linear/VectorValuesOrdered.cpp b/gtsam/linear/VectorValuesOrdered.cpp deleted file mode 100644 index 24367d82d..000000000 --- a/gtsam/linear/VectorValuesOrdered.cpp +++ /dev/null @@ -1,237 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 VectorValues.cpp - * @brief Implementations for VectorValues - * @author Richard Roberts - * @author Alex Cunningham - */ - -#include -#include -#include - -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -VectorValuesOrdered VectorValuesOrdered::Zero(const VectorValuesOrdered& x) { - VectorValuesOrdered result; - result.values_.resize(x.size()); - for(size_t j=0; j VectorValuesOrdered::dims() const { - std::vector result(this->size()); - for(Index j = 0; j < this->size(); ++j) - result[j] = this->dim(j); - return result; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::insert(Index j, const Vector& value) { - // Make sure j does not already exist - if(exists(j)) - throw invalid_argument("VectorValues: requested variable index to insert already exists."); - - // If this adds variables at the end, insert zero-length entries up to j - if(j >= size()) - values_.resize(j+1); - - // Assign value - values_[j] = value; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::print(const std::string& str, const IndexFormatter& formatter) const { - std::cout << str << ": " << size() << " elements\n"; - for (Index var = 0; var < size(); ++var) - std::cout << " " << formatter(var) << ": " << (*this)[var].transpose() << "\n"; - std::cout.flush(); -} - -/* ************************************************************************* */ -bool VectorValuesOrdered::equals(const VectorValuesOrdered& x, double tol) const { - if(this->size() != x.size()) - return false; - for(Index j=0; j < size(); ++j) - if(!equal_with_abs_tol(values_[j], x.values_[j], tol)) - return false; - return true; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::resize(Index nVars, size_t varDim) { - values_.resize(nVars); - for(Index j = 0; j < nVars; ++j) - values_[j] = Vector(varDim); -} - -/* ************************************************************************* */ -void VectorValuesOrdered::resizeLike(const VectorValuesOrdered& other) { - values_.resize(other.size()); - for(Index j = 0; j < other.size(); ++j) - values_[j].resize(other.values_[j].size()); -} - -/* ************************************************************************* */ -VectorValuesOrdered VectorValuesOrdered::SameStructure(const VectorValuesOrdered& other) { - VectorValuesOrdered ret; - ret.resizeLike(other); - return ret; -} - -/* ************************************************************************* */ -VectorValuesOrdered VectorValuesOrdered::Zero(Index nVars, size_t varDim) { - VectorValuesOrdered ret(nVars, varDim); - ret.setZero(); - return ret; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::setZero() { - BOOST_FOREACH(Vector& v, *this) { - v.setZero(); - } -} - -/* ************************************************************************* */ -const Vector VectorValuesOrdered::asVector() const { - return internal::extractVectorValuesSlices(*this, - boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true); -} - -/* ************************************************************************* */ -const Vector VectorValuesOrdered::vector(const std::vector& indices) const { - return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end()); -} - -/* ************************************************************************* */ -bool VectorValuesOrdered::hasSameStructure(const VectorValuesOrdered& other) const { - if(this->size() != other.size()) - return false; - for(size_t j = 0; j < size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].rows() != other.values_[j].rows()) - return false; - return true; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::permuteInPlace(const Permutation& permutation) { - // Allocate new values - Values newValues(this->size()); - - // Swap values from this VectorValues to the permuted VectorValues - for(size_t i = 0; i < this->size(); ++i) - newValues[i].swap(this->at(permutation[i])); - - // Swap the values into this VectorValues - values_.swap(newValues); -} - -/* ************************************************************************* */ -void VectorValuesOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - Values reorderedEntries(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]); - // Put the affected entries back in the new order - for(size_t slot = 0; slot < selector.size(); ++slot) - values_[selector[slot]].swap(reorderedEntries[slot]); -} - -/* ************************************************************************* */ -void VectorValuesOrdered::swap(VectorValuesOrdered& other) { - this->values_.swap(other.values_); -} - -/* ************************************************************************* */ -double VectorValuesOrdered::dot(const VectorValuesOrdered& v) const { - double result = 0.0; - if(this->size() != v.size()) - throw invalid_argument("VectorValues::dot called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == v.values_[j].size()) - result += this->values_[j].dot(v.values_[j]); - else - throw invalid_argument("VectorValues::dot called with different vector sizes"); - return result; -} - -/* ************************************************************************* */ -double VectorValuesOrdered::norm() const { - return std::sqrt(this->squaredNorm()); -} - -/* ************************************************************************* */ -double VectorValuesOrdered::squaredNorm() const { - double sumSquares = 0.0; - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - sumSquares += this->values_[j].squaredNorm(); - return sumSquares; -} - -/* ************************************************************************* */ -VectorValuesOrdered VectorValuesOrdered::operator+(const VectorValuesOrdered& c) const { - VectorValuesOrdered result = SameStructure(*this); - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+ called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - result.values_[j] = this->values_[j] + c.values_[j]; - else - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - return result; -} - -/* ************************************************************************* */ -VectorValuesOrdered VectorValuesOrdered::operator-(const VectorValuesOrdered& c) const { - VectorValuesOrdered result = SameStructure(*this); - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - result.values_[j] = this->values_[j] - c.values_[j]; - else - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - return result; -} - -/* ************************************************************************* */ -void VectorValuesOrdered::operator+=(const VectorValuesOrdered& c) { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); - for(Index j = 0; j < this->size(); ++j) - // Directly accessing maps instead of using VV::dim in case some values are empty - if(this->values_[j].size() == c.values_[j].size()) - this->values_[j] += c.values_[j]; - else - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); -} - -/* ************************************************************************* */ - -} // \namespace gtsam diff --git a/gtsam/linear/VectorValuesOrdered.h b/gtsam/linear/VectorValuesOrdered.h deleted file mode 100644 index 081014284..000000000 --- a/gtsam/linear/VectorValuesOrdered.h +++ /dev/null @@ -1,468 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 VectorValues.h - * @brief Factor Graph Values - * @author Richard Roberts - */ - -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - // Forward declarations - class Permutation; - - /** - * This class represents a collection of vector-valued variables associated - * each with a unique integer index. It is typically used to store the variables - * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet - * returns this class. - * - * For basic usage, such as receiving a linear solution from gtsam solving functions, - * or creating this class in unit tests and examples where speed is not important, - * you can use a simple interface: - * - The default constructor VectorValues() to create this class - * - insert(Index, const Vector&) to add vector variables - * - operator[](Index) for read and write access to stored variables - * - \ref exists (Index) to check if a variable is present - * - Other facilities like iterators, size(), dim(), etc. - * - * Indices can be non-consecutive and inserted out-of-order, but you should not - * use indices that are larger than a reasonable array size because the indices - * correspond to positions in an internal array. - * - * Example: - * \code - VectorValues values; - values.insert(3, Vector_(3, 1.0, 2.0, 3.0)); - values.insert(4, Vector_(2, 4.0, 5.0)); - values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0)); - - // Prints [ 3.0 4.0 ] - gtsam::print(values[1]); - - // Prints [ 8.0 9.0 ] - values[1] = Vector_(2, 8.0, 9.0); - gtsam::print(values[1]); - \endcode - * - *

Advanced Interface and Performance Information

- * - * Internally, all vector values are stored as part of one large vector. In - * gtsam this vector is always pre-allocated for efficiency, using the - * advanced interface described below. Accessing and modifying already-allocated - * values is \f$ O(1) \f$. Using the insert() function of the standard interface - * is slow because it requires re-allocating the internal vector. - * - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Index, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, - * which is a view on the underlying data structure. - * - * This class is additionally used in gradient descent and dog leg to store the gradient. - * \nosubgrouping - */ - class GTSAM_EXPORT VectorValuesOrdered { - protected: - typedef std::vector Values; ///< Typedef for the collection of Vectors making up a VectorValues - Values values_; ///< Collection of Vectors making up this VectorValues - - public: - typedef Values::iterator iterator; ///< Iterator over vector values - typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - - /// @name Standard Constructors - /// @{ - - /** - * Default constructor creates an empty VectorValues. - */ - VectorValuesOrdered() {} - - /** Named constructor to create a VectorValues of the same structure of the - * specified one, but filled with zeros. - * @return - */ - static VectorValuesOrdered Zero(const VectorValuesOrdered& model); - - /// @} - /// @name Standard Interface - /// @{ - - /** Number of variables stored, always 1 more than the highest variable index, - * even if some variables with lower indices are not present. */ - Index size() const { return values_.size(); } - - /** Return the dimension of variable \c j. */ - size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); } - - /** Return the dimension of each vector in this container */ - std::vector dims() const; - - /** Check whether a variable with index \c j exists. */ - bool exists(Index j) const { return j < size() && values_[j].rows() > 0; } - - /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - Vector& at(Index j) { checkExists(j); return values_[j]; } - - /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */ - const Vector& at(Index j) const { checkExists(j); return values_[j]; } - - /** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */ - Vector& operator[](Index j) { return at(j); } - - /** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */ - const Vector& operator[](Index j) const { return at(j); } - - /** Insert a vector \c value with index \c j. - * Causes reallocation, but can insert values in any order. - * Throws an invalid_argument exception if the index \c j is already used. - * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ - void insert(Index j, const Vector& value); - - iterator begin() { return values_.begin(); } ///< Iterator over variables - const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables - const_iterator end() const { return values_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables - - /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; - - /** equals required by Testable for unit testing */ - bool equals(const VectorValuesOrdered& x, double tol = 1e-9) const; - - /// @{ - /// \anchor AdvancedConstructors - /// @name Advanced Constructors - /// @} - - /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ - template - explicit VectorValuesOrdered(const CONTAINER& dimensions) { this->append(dimensions); } - - /** Construct to hold nVars vectors of varDim dimension each. */ - VectorValuesOrdered(Index nVars, size_t varDim) { this->resize(nVars, varDim); } - - /** Named constructor to create a VectorValues that matches the structure of - * the specified VectorValues, but do not initialize the new values. */ - static VectorValuesOrdered SameStructure(const VectorValuesOrdered& other); - - /** Named constructor to create a VectorValues from a container of variable - * dimensions that is filled with zeros. - * @param dimensions A container of the dimension of each variable to create. - */ - template - static VectorValuesOrdered Zero(const CONTAINER& dimensions); - - /** Named constructor to create a VectorValues filled with zeros that has - * \c nVars variables, each of dimension \c varDim - * @param nVars The number of variables to create - * @param varDim The dimension of each variable - * @return The new VectorValues - */ - static VectorValuesOrdered Zero(Index nVars, size_t varDim); - - /// @} - /// @name Advanced Interface - /// @{ - - /** Resize this VectorValues to have identical structure to other, leaving - * this VectorValues with uninitialized values. - * @param other The VectorValues whose structure to copy - */ - void resizeLike(const VectorValuesOrdered& other); - - /** Resize the VectorValues to hold \c nVars variables, each of dimension - * \c varDim. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param nVars The number of variables to create - * @param varDim The dimension of each variable - */ - void resize(Index nVars, size_t varDim); - - /** Resize the VectorValues to contain variables of the dimensions stored - * in \c dimensions. Any individual vectors that do not change size will keep - * their values, but any new or resized vectors will be uninitialized. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void resize(const CONTAINER& dimensions); - - /** Append to the VectorValues to additionally contain variables of the - * dimensions stored in \c dimensions. The new variables are uninitialized, - * but this function is used to pre-allocate space for performance. This - * function preserves the original data, so all previously-existing variables - * are left unchanged. - * @param dimensions A container of the dimension of each variable to create. - */ - template - void append(const CONTAINER& dimensions); - - /** Removes the last subvector from the VectorValues */ - void pop_back() { values_.pop_back(); }; - - /** Set all entries to zero, does not modify the size. */ - void setZero(); - - /** Retrieve the entire solution as a single vector */ - const Vector asVector() const; - - /** Access a vector that is a subset of relevant indices */ - const Vector vector(const std::vector& indices) const; - - /** Check whether this VectorValues has the same structure, meaning has the - * same number of variables and that all variables are of the same dimension, - * as another VectorValues - * @param other The other VectorValues with which to compare structure - * @return \c true if the structure is the same, \c false if not. - */ - bool hasSameStructure(const VectorValuesOrdered& other) const; - - /** - * Permute the variables in the VariableIndex according to the given partial permutation - */ - void permuteInPlace(const Permutation& selector, const Permutation& permutation); - - /** - * Permute the entries of this VectorValues in place - */ - void permuteInPlace(const Permutation& permutation); - - /** - * Swap the data in this VectorValues with another. - */ - void swap(VectorValuesOrdered& other); - - /// @} - /// @name Linear algebra operations - /// @{ - - /** Dot product with another VectorValues, interpreting both as vectors of - * their concatenated values. */ - double dot(const VectorValuesOrdered& v) const; - - /** Vector L2 norm */ - double norm() const; - - /** Squared vector L2 norm */ - double squaredNorm() const; - - /** - * + operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ - VectorValuesOrdered operator+(const VectorValuesOrdered& c) const; - - /** - * + operator does element-wise subtraction. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ - VectorValuesOrdered operator-(const VectorValuesOrdered& c) const; - - /** - * += operator does element-wise addition. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). - */ - void operator+=(const VectorValuesOrdered& c); - - /// @} - - /// @} - /// @name Matlab syntactic sugar for linear algebra operations - /// @{ - - inline VectorValuesOrdered add(const VectorValuesOrdered& c) const { return *this + c; } - inline VectorValuesOrdered scale(const double a, const VectorValuesOrdered& c) const { return a * (*this); } - - /// @} - - private: - // Throw an exception if j does not exist - void checkExists(Index j) const { - if(!exists(j)) { - const std::string msg = - (boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str(); - throw std::out_of_range(msg); - } - } - - public: - - /** - * scale a vector by a scalar - */ - friend VectorValuesOrdered operator*(const double a, const VectorValuesOrdered &v) { - VectorValuesOrdered result = VectorValuesOrdered::SameStructure(v); - for(Index j = 0; j < v.size(); ++j) - result.values_[j] = a * v.values_[j]; - return result; - } - - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void scal(double alpha, VectorValuesOrdered& x) { - for(Index j = 0; j < x.size(); ++j) - x.values_[j] *= alpha; - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void axpy(double alpha, const VectorValuesOrdered& x, VectorValuesOrdered& y) { - if(x.size() != y.size()) - throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) - if(x.values_[j].size() == y.values_[j].size()) - y.values_[j] += alpha * x.values_[j]; - else - throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - } - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void sqrt(VectorValuesOrdered &x) { - for(Index j = 0; j < x.size(); ++j) - x.values_[j] = x.values_[j].cwiseSqrt(); - } - - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void ediv(const VectorValuesOrdered& numerator, const VectorValuesOrdered& denominator, VectorValuesOrdered &result) { - if(numerator.size() != denominator.size() || numerator.size() != result.size()) - throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - for(Index j = 0; j < numerator.size(); ++j) - if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - else - throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - } - - /// TODO: linear algebra interface seems to have been added for SPCG. - friend void edivInPlace(VectorValuesOrdered& x, const VectorValuesOrdered& y) { - if(x.size() != y.size()) - throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - for(Index j = 0; j < x.size(); ++j) - if(x.values_[j].size() == y.values_[j].size()) - x.values_[j].array() /= y.values_[j].array(); - else - throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(values_); - } - }; // VectorValues definition - - // Implementations of template and inline functions - - /* ************************************************************************* */ - template - void VectorValuesOrdered::resize(const CONTAINER& dimensions) { - values_.clear(); - append(dimensions); - } - - /* ************************************************************************* */ - template - void VectorValuesOrdered::append(const CONTAINER& dimensions) { - size_t i = size(); - values_.resize(size() + dimensions.size()); - BOOST_FOREACH(size_t dim, dimensions) { - values_[i] = Vector(dim); - ++ i; - } - } - - /* ************************************************************************* */ - template - VectorValuesOrdered VectorValuesOrdered::Zero(const CONTAINER& dimensions) { - VectorValuesOrdered ret; - ret.values_.resize(dimensions.size()); - size_t i = 0; - BOOST_FOREACH(size_t dim, dimensions) { - ret.values_[i] = Vector::Zero(dim); - ++ i; - } - return ret; - } - - namespace internal { - /* ************************************************************************* */ - // Helper function, extracts vectors with variable indices - // in the first and last iterators, and concatenates them in that order into the - // output. - template - const Vector extractVectorValuesSlices(const VectorValuesOrdered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) { - // Find total dimensionality - size_t dim = 0; - for(ITERATOR j = first; j != last; ++j) - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) - dim += values.dim(*j); - - // Copy vectors - Vector ret(dim); - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - // If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent) - if(!allowNonexistant || values.exists(*j)) { - ret.segment(varStart, values.dim(*j)) = values[*j]; - varStart += values.dim(*j); - } - } - return ret; - } - - /* ************************************************************************* */ - // Helper function, writes to the variables in values - // with indices iterated over by first and last, interpreting vector as the - // concatenated vectors to write. - template - void writeVectorValuesSlices(const VECTOR& vector, VectorValuesOrdered& values, ITERATOR first, ITERATOR last) { - // Copy vectors - size_t varStart = 0; - for(ITERATOR j = first; j != last; ++j) { - values[*j] = vector.segment(varStart, values[*j].rows()); - varStart += values[*j].rows(); - } - assert(varStart == vector.rows()); - } - } - -} // \namespace gtsam diff --git a/gtsam/linear/tests/testGaussianConditionalObsolete.cpp b/gtsam/linear/tests/testGaussianConditionalObsolete.cpp deleted file mode 100644 index 2622b71a5..000000000 --- a/gtsam/linear/tests/testGaussianConditionalObsolete.cpp +++ /dev/null @@ -1,354 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianConditional.cpp - * @brief Unit tests for Conditional gaussian - * @author Christian Potthast - **/ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; -using namespace boost::assign; - -static const double tol = 1e-5; -static const Index _x_=0, _x1_=1, _l1_=2; - -Matrix R = Matrix_(2,2, - -12.1244, -5.1962, - 0., 4.6904); - -/* ************************************************************************* */ -TEST(GaussianConditionalOrdered, constructor) -{ - Matrix S1 = Matrix_(2,2, - -5.2786, -8.6603, - 5.0254, 5.5432); - Matrix S2 = Matrix_(2,2, - -10.5573, -5.9385, - 5.5737, 3.0153); - Matrix S3 = Matrix_(2,2, - -11.3820, -7.2581, - -3.0153, -3.5635); - - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); - - list > terms; - terms += - make_pair(3, S1), - make_pair(5, S2), - make_pair(7, S3); - - GaussianConditionalOrdered actual(1, d, R, terms, s); - - GaussianConditionalOrdered::const_iterator it = actual.beginFrontals(); - EXPECT(assert_equal(Index(1), *it)); - EXPECT(assert_equal(R, actual.get_R())); - ++ it; - EXPECT(it == actual.endFrontals()); - - it = actual.beginParents(); - EXPECT(assert_equal(Index(3), *it)); - EXPECT(assert_equal(S1, actual.get_S(it))); - - ++ it; - EXPECT(assert_equal(Index(5), *it)); - EXPECT(assert_equal(S2, actual.get_S(it))); - - ++ it; - EXPECT(assert_equal(Index(7), *it)); - EXPECT(assert_equal(S3, actual.get_S(it))); - - ++it; - EXPECT(it == actual.endParents()); - - EXPECT(assert_equal(d, actual.get_d())); - EXPECT(assert_equal(s, actual.get_sigmas())); - - // test copy constructor - GaussianConditionalOrdered copied(actual); - EXPECT(assert_equal(d, copied.get_d())); - EXPECT(assert_equal(s, copied.get_sigmas())); - EXPECT(assert_equal(R, copied.get_R())); -} - -/* ************************************************************************* */ - -GaussianConditionalOrdered construct() { - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); - GaussianConditionalOrdered::shared_ptr shared(new GaussianConditionalOrdered(1, d, R, s)); - return *shared; -} - -TEST(GaussianConditionalOrdered, return_value) -{ - Vector d = Vector_(2, 1.0, 2.0); - Vector s = Vector_(2, 3.0, 4.0); - GaussianConditionalOrdered copied = construct(); - EXPECT(assert_equal(d, copied.get_d())); - EXPECT(assert_equal(s, copied.get_sigmas())); - EXPECT(assert_equal(R, copied.get_R())); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, equals ) -{ - // create a conditional gaussian node - Matrix A1(2,2); - A1(0,0) = 1 ; A1(1,0) = 2; - A1(0,1) = 3 ; A1(1,1) = 4; - - Matrix A2(2,2); - A2(0,0) = 6 ; A2(1,0) = 0.2; - A2(0,1) = 8 ; A2(1,1) = 0.4; - - Matrix R(2,2); - R(0,0) = 0.1 ; R(1,0) = 0.3; - R(0,1) = 0.0 ; R(1,1) = 0.34; - - Vector tau(2); - tau(0) = 1.0; - tau(1) = 0.34; - - Vector d(2); - d(0) = 0.2; d(1) = 0.5; - - GaussianConditionalOrdered - expected(_x_,d, R, _x1_, A1, _l1_, A2, tau), - actual(_x_,d, R, _x1_, A1, _l1_, A2, tau); - - EXPECT( expected.equals(actual) ); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, solve ) -{ - //expected solution - Vector expectedX(2); - expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15; - - // create a conditional Gaussian node - Matrix R = Matrix_(2,2, 1., 0., - 0., 1.); - - Matrix A1 = Matrix_(2,2, 1., 2., - 3., 4.); - - Matrix A2 = Matrix_(2,2, 5., 6., - 7., 8.); - - Vector d(2); - d(0) = 20.0; d(1) = 40.0; - - Vector tau = ones(2); - - GaussianConditionalOrdered cg(_x_, d, R, _x1_, A1, _l1_, A2, tau); - - Vector sx1(2); - sx1(0) = 1.0; sx1(1) = 1.0; - - Vector sl1(2); - sl1(0) = 1.0; sl1(1) = 1.0; - - VectorValuesOrdered solution(vector(3, 2)); - solution[_x_] = d; - solution[_x1_] = sx1; // parents - solution[_l1_] = sl1; - - VectorValuesOrdered expected(vector(3, 2)); - expected[_x_] = expectedX; - expected[_x1_] = sx1; - expected[_l1_] = sl1; - cg.solveInPlace(solution); - - EXPECT(assert_equal(expected, solution, 0.0001)); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, solve_simple ) -{ - Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); - - // solve system as a non-multifrontal version first - // 2 variables, frontal has dim=4 - vector dims; dims += 4, 2, 1; - GaussianConditionalOrdered::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_; - GaussianConditionalOrdered cg(cgdims.begin(), cgdims.end(), 1, matrices, sigmas); - - // partial solution - Vector sx1 = Vector_(2, 9.0, 10.0); - - // elimination order; _x_, _x1_ - vector vdim; vdim += 4, 2; - VectorValuesOrdered actual(vdim); - actual[_x1_] = sx1; // parent - - VectorValuesOrdered expected(vdim); - expected[_x1_] = sx1; - expected[_x_] = Vector_(4, -3.1,-3.4,-11.9,-13.2); - - // verify indices/size - EXPECT_LONGS_EQUAL(2, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); - - // solve and verify - cg.solveInPlace(actual); - EXPECT(assert_equal(expected, actual, tol)); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, solve_multifrontal ) -{ - // create full system, 3 variables, 2 frontals, all 2 dim - Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); - - // 3 variables, all dim=2 - vector dims; dims += 2, 2, 2, 1; - GaussianConditionalOrdered::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_, _l1_; - GaussianConditionalOrdered cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); - - EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); - - // partial solution - Vector sl1 = Vector_(2, 9.0, 10.0); - - // elimination order; _x_, _x1_, _l1_ - VectorValuesOrdered actual(vector(3, 2)); - actual[_l1_] = sl1; // parent - - VectorValuesOrdered expected(vector(3, 2)); - expected[_x_] = Vector_(2, -3.1,-3.4); - expected[_x1_] = Vector_(2, -11.9,-13.2); - expected[_l1_] = sl1; - - // verify indices/size - EXPECT_LONGS_EQUAL(3, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); - - // solve and verify - cg.solveInPlace(actual); - EXPECT(assert_equal(expected, actual, tol)); - -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, solveTranspose ) { - static const Index _y_=1; - /** create small Chordal Bayes Net x <- y - * x y d - * 1 1 9 - * 1 5 - */ - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); - Vector d1(1), d2(1); - d1(0) = 9; - d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; - - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau)); - GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau)); - GaussianBayesNetOrdered cbn; - cbn.push_back(Px_y); - cbn.push_back(Py); - - // x=R'*y, y=inv(R')*x - // 2 = 1 2 - // 5 1 1 3 - - VectorValuesOrdered y(vector(2,1)), x(vector(2,1)); - x[_x_] = Vector_(1,2.); - x[_y_] = Vector_(1,5.); - y[_x_] = Vector_(1,2.); - y[_y_] = Vector_(1,3.); - - // test functional version - VectorValuesOrdered actual = backSubstituteTranspose(cbn,x); - CHECK(assert_equal(y,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, information ) { - - // Create R matrix - Matrix R = (Matrix(4,4) << - 1, 2, 3, 4, - 0, 5, 6, 7, - 0, 0, 8, 9, - 0, 0, 0, 10).finished(); - - // Create conditional - GaussianConditionalOrdered conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); - - // Expected information matrix (using permuted R) - Matrix IExpected = R.transpose() * R; - - // Actual information matrix (conditional should permute R) - Matrix IActual = conditional.information(); - EXPECT(assert_equal(IExpected, IActual)); -} - -/* ************************************************************************* */ -TEST( GaussianConditionalOrdered, isGaussianFactor ) { - - // Create R matrix - Matrix R = (Matrix(4,4) << - 1, 2, 3, 4, - 0, 5, 6, 7, - 0, 0, 8, 9, - 0, 0, 0, 10).finished(); - - // Create a conditional - GaussianConditionalOrdered conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0)); - - // Expected information matrix computed by conditional - Matrix IExpected = conditional.information(); - - // Expected information matrix computed by a factor - JacobianFactorOrdered jf = *conditional.toFactor(); - Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin()); - - EXPECT(assert_equal(IExpected, IActual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianFactorGraphObsolete.cpp b/gtsam/linear/tests/testGaussianFactorGraphObsolete.cpp deleted file mode 100644 index 63e537d59..000000000 --- a/gtsam/linear/tests/testGaussianFactorGraphObsolete.cpp +++ /dev/null @@ -1,537 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianFactor.cpp - * @brief Unit tests for Linear Factor - * @author Christian Potthast - * @author Frank Dellaert - **/ - -#include // for operator += -using namespace boost::assign; - -#include -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost; - -static SharedDiagonal - sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), - constraintModel = noiseModel::Constrained::All(2); - -static GaussianFactorGraphOrdered createSimpleGaussianFactorGraph() { - GaussianFactorGraphOrdered fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.add(2, 10*eye(2), -1.0*ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.add(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.add(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.add(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); - return fg; -} - - -/* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, initialization) { - // Create empty graph - GaussianFactorGraphOrdered fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - - fg.add(0, 10*eye(2), -1.0*ones(2), unit2); - fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); - fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); - fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); - - EXPECT_LONGS_EQUAL(4, fg.size()); - JacobianFactorOrdered factor = *boost::dynamic_pointer_cast(fg[0]); - - // Test sparse, which takes a vector and returns a matrix, used in MATLAB - // Note that this the augmented vector and the RHS is in column 7 - Matrix expectedIJS = Matrix_(3,22, - 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8., - 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7., - 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5 - ); - Matrix actualIJS = fg.sparseJacobian_(); - EQUALITY(expectedIJS, actualIJS); -} - -/* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, CombineJacobians) -{ - Matrix A01 = 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, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraphOrdered gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - // Convert to Jacobians (inefficient copy of all factors instead of selectively converting only Hessians) - FactorGraphOrdered jacobians; - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { - jacobians.push_back(boost::make_shared(*factor)); - } - - // Combine Jacobians into a single dense factor - JacobianFactorOrdered actual = *CombineJacobians(jacobians, VariableSlots(gfg)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &zero3x3, &A10, &zero3x3); - Matrix A1 = gtsam::stack(3, &A01, &A11, &A21); - Vector b = gtsam::concatVectors(3, &b0, &b1, &b2); - Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2); - - JacobianFactorOrdered expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorOrdered, CombineAndEliminate) -{ - Matrix A01 = 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, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraphOrdered gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); - - JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1); - - GaussianConditionalOrdered::shared_ptr actualBN; - GaussianFactorOrdered::shared_ptr actualFactor; - boost::tie(actualBN, actualFactor) = EliminateQROrdered(gfg, 1); - JacobianFactorOrdered::shared_ptr actualJacobian = boost::dynamic_pointer_cast< - JacobianFactorOrdered>(actualFactor); - - EXPECT(assert_equal(*expectedBN, *actualBN)); - EXPECT(assert_equal(expectedFactor, *actualJacobian)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorOrdered, eliminateFrontals) -{ - // Augmented Ab test case for whole factor graph - Matrix Ab = Matrix_(14,11, - 4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1., - 9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4., - 5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0., - 5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5., - 0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4., - 0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6., - 0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6., - 0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6., - 0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0., - 0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0., - 0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3., - 0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5., - 0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6., - 0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.); - - // Create first factor (from pieces of Ab) - list > terms1; - - terms1 += - make_pair( 3, Matrix(Ab.block(0, 0, 4, 2))), - make_pair( 5, Matrix(Ab.block(0, 2, 4, 2))), - make_pair( 7, Matrix(Ab.block(0, 4, 4, 2))), - make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))), - make_pair(11, Matrix(Ab.block(0, 8, 4, 2))); - Vector b1 = Ab.col(10).segment(0, 4); - JacobianFactorOrdered::shared_ptr factor1(new JacobianFactorOrdered(terms1, b1, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create second factor - list > terms2; - terms2 += - make_pair(5, Matrix(Ab.block(4, 2, 4, 2))), - make_pair(7, Matrix(Ab.block(4, 4, 4, 2))), - make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), - make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); - Vector b2 = Ab.col(10).segment(4, 4); - JacobianFactorOrdered::shared_ptr factor2(new JacobianFactorOrdered(terms2, b2, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create third factor - list > terms3; - terms3 += - make_pair(7, Matrix(Ab.block(8, 4, 4, 2))), - make_pair(9, Matrix(Ab.block(8, 6, 4, 2))), - make_pair(11,Matrix(Ab.block(8, 8, 4, 2))); - Vector b3 = Ab.col(10).segment(8, 4); - JacobianFactorOrdered::shared_ptr factor3(new JacobianFactorOrdered(terms3, b3, noiseModel::Isotropic::Sigma(4, 0.5))); - - // Create fourth factor - list > terms4; - terms4 += - make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); - Vector b4 = Ab.col(10).segment(12, 2); - JacobianFactorOrdered::shared_ptr factor4(new JacobianFactorOrdered(terms4, b4, noiseModel::Isotropic::Sigma(2, 0.5))); - - // Create factor graph - GaussianFactorGraphOrdered factors; - factors.push_back(factor1); - factors.push_back(factor2); - factors.push_back(factor3); - factors.push_back(factor4); - - // extract the dense matrix for the graph - Matrix actualDense = factors.augmentedJacobian(); - EXPECT(assert_equal(2.0 * Ab, actualDense)); - - // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians - FactorGraphOrdered jacobians; - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, factors) { - jacobians.push_back(boost::make_shared(*factor)); - } - - // Create combined factor - JacobianFactorOrdered combined(*CombineJacobians(jacobians, VariableSlots(factors))); - - // Copies factors as they will be eliminated in place - JacobianFactorOrdered actualFactor_QR = combined; - JacobianFactorOrdered actualFactor_Chol = combined; - - // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0*Matrix_(11,11, - -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, - 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, - 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, - 0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676, - 0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277, - 0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769, - 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, - 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, - 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, - 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635); - - // Expected conditional on first variable from first 2 rows of R - Matrix R1 = sub(R, 0,2, 0,2); - list > cterms1; - cterms1 += - make_pair(5, sub(R, 0,2, 2,4 )), - make_pair(7, sub(R, 0,2, 4,6 )), - make_pair(9, sub(R, 0,2, 6,8 )), - make_pair(11,sub(R, 0,2, 8,10)); - Vector d1 = R.col(10).segment(0,2); - GaussianConditionalOrdered::shared_ptr cond1(new GaussianConditionalOrdered(3, d1, R1, cterms1, ones(2))); - - // Expected conditional on second variable from next 2 rows of R - Matrix R2 = sub(R, 2,4, 2,4); - list > cterms2; - cterms2 += - make_pair(7, sub(R, 2,4, 4,6)), - make_pair(9, sub(R, 2,4, 6,8)), - make_pair(11,sub(R, 2,4, 8,10)); - Vector d2 = R.col(10).segment(2,2); - GaussianConditionalOrdered::shared_ptr cond2(new GaussianConditionalOrdered(5, d2, R2, cterms2, ones(2))); - - // Expected conditional on third variable from next 2 rows of R - Matrix R3 = sub(R, 4,6, 4,6); - list > cterms3; - cterms3 += - make_pair(9, sub(R, 4,6, 6,8)), - make_pair(11, sub(R, 4,6, 8,10)); - Vector d3 = R.col(10).segment(4,2); - GaussianConditionalOrdered::shared_ptr cond3(new GaussianConditionalOrdered(7, d3, R3, cterms3, ones(2))); - - // Create expected Bayes net fragment from three conditionals above -// GaussianBayesNetOrdered expectedFragment; -// expectedFragment.push_back(cond1); -// expectedFragment.push_back(cond2); -// expectedFragment.push_back(cond3); - Index ikeys[] = {3,5,7,9,11}; - std::vector keys(ikeys, ikeys + sizeof(ikeys)/sizeof(Index)); - size_t dims[] = { 2,2,2,2,2,1 }; - size_t height = 11; - VerticalBlockView Rblock(R, dims, dims+6, height); - GaussianConditionalOrdered::shared_ptr expectedFragment( new GaussianConditionalOrdered(keys.begin(), keys.end(), 3, - Rblock, ones(6)) ); - - // Get expected matrices for remaining factor - Matrix Ae1 = sub(R, 6,10, 6,8); - Matrix Ae2 = sub(R, 6,10, 8,10); - Vector be = R.col(10).segment(6, 4); - - // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! - GaussianConditionalOrdered::shared_ptr actualFragment_QR = actualFactor_QR.eliminate(3); - - EXPECT(assert_equal(*expectedFragment, *actualFragment_QR, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001)); - EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001)); - EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001)); - - // Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!! -#ifdef BROKEN - GaussianBayesNetOrdered actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactorOrdered::SOLVE_CHOLESKY); - EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); - EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); - EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); - EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); - EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); //// - EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); - EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); //// - EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); -#endif -} - -/* ************************************************************************* */ -TEST(GaussianFactorOrdered, permuteWithInverse) -{ - Matrix A1 = Matrix_(2,2, - 1.0, 2.0, - 3.0, 4.0); - Matrix A2 = Matrix_(2,1, - 5.0, - 6.0); - Matrix A3 = Matrix_(2,3, - 7.0, 8.0, 9.0, - 10.0, 11.0, 12.0); - Vector b = Vector_(2, 13.0, 14.0); - - Permutation inversePermutation(6); - inversePermutation[0] = 5; - inversePermutation[1] = 4; - inversePermutation[2] = 3; - inversePermutation[3] = 2; - inversePermutation[4] = 1; - inversePermutation[5] = 0; - - JacobianFactorOrdered actual(1, A1, 3, A2, 5, A3, b, noiseModel::Isotropic::Sigma(2, 1.0)); - GaussianFactorGraphOrdered actualFG; actualFG.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(actual))); - VariableIndexOrdered actualIndex(actualFG); - actual.permuteWithInverse(inversePermutation); -// actualIndex.permute(*inversePermutation.inverse()); - - JacobianFactorOrdered expected(4, A1, 2, A2, 0, A3, b, noiseModel::Isotropic::Sigma(2, 1.0)); - GaussianFactorGraphOrdered expectedFG; expectedFG.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(expected))); -// GaussianVariableIndex expectedIndex(expectedFG); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, sparseJacobian) { - // Create factor graph: - // x1 x2 x3 x4 x5 b - // 1 2 3 0 0 4 - // 5 6 7 0 0 8 - // 9 10 0 11 12 13 - // 0 0 0 14 15 16 - - // Expected - NOTE that we transpose this! - Matrix expected = Matrix_(16,3, - 1., 1., 2., - 1., 2., 4., - 1., 3., 6., - 2., 1.,10., - 2., 2.,12., - 2., 3.,14., - 1., 6., 8., - 2., 6.,16., - 3., 1.,18., - 3., 2.,20., - 3., 4.,22., - 3., 5.,24., - 4., 4.,28., - 4., 5.,30., - 3., 6.,26., - 4., 6.,32.).transpose(); - - GaussianFactorGraphOrdered gfg; - SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); - gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); - - Matrix actual = gfg.sparseJacobian_(); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, matrices) { - // Create factor graph: - // x1 x2 x3 x4 x5 b - // 1 2 3 0 0 4 - // 5 6 7 0 0 8 - // 9 10 0 11 12 13 - // 0 0 0 14 15 16 - - GaussianFactorGraphOrdered gfg; - SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); - gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); - - Matrix jacobian(4,6); - jacobian << - 1, 2, 3, 0, 0, 4, - 5, 6, 7, 0, 0, 8, - 9,10, 0,11,12,13, - 0, 0, 0,14,15,16; - - Matrix expectedJacobian = jacobian; - Matrix expectedHessian = jacobian.transpose() * jacobian; - Matrix expectedA = jacobian.leftCols(jacobian.cols()-1); - Vector expectedb = jacobian.col(jacobian.cols()-1); - Matrix expectedL = expectedA.transpose() * expectedA; - Vector expectedeta = expectedA.transpose() * expectedb; - - Matrix actualJacobian = gfg.augmentedJacobian(); - Matrix actualHessian = gfg.augmentedHessian(); - Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); - Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); - - EXPECT(assert_equal(expectedJacobian, actualJacobian)); - EXPECT(assert_equal(expectedHessian, actualHessian)); - EXPECT(assert_equal(expectedA, actualA)); - EXPECT(assert_equal(expectedb, actualb)); - EXPECT(assert_equal(expectedL, actualL)); - EXPECT(assert_equal(expectedeta, actualeta)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, gradient ) -{ - GaussianFactorGraphOrdered fg = createSimpleGaussianFactorGraph(); - - // Construct expected gradient - VectorValuesOrdered expected; - - // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 - // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] - expected.insert(1,Vector_(2, 5.0,-12.5)); - expected.insert(2,Vector_(2, 30.0, 5.0)); - expected.insert(0,Vector_(2,-25.0, 17.5)); - - // Check the gradient at delta=0 - VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected); - VectorValuesOrdered actual = gradient(fg, zero); - EXPECT(assert_equal(expected,actual)); - - // Check the gradient at the solution (should be zero) - VectorValuesOrdered solution = *GaussianSequentialSolver(fg).optimize(); - VectorValuesOrdered actual2 = gradient(fg, solution); - EXPECT(assert_equal(VectorValuesOrdered::Zero(solution), actual2)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraphOrdered, transposeMultiplication ) -{ - GaussianFactorGraphOrdered A = createSimpleGaussianFactorGraph(); - - VectorValuesOrdered e; - e.insert(0, Vector_(2, 0.0, 0.0)); - e.insert(1, Vector_(2,15.0, 0.0)); - e.insert(2, Vector_(2, 0.0,-5.0)); - e.insert(3, Vector_(2,-7.5,-5.0)); - - VectorValuesOrdered expected; - expected.insert(1, Vector_(2, -37.5,-50.0)); - expected.insert(2, Vector_(2,-150.0, 25.0)); - expected.insert(0, Vector_(2, 187.5, 25.0)); - - VectorValuesOrdered actual = VectorValuesOrdered::SameStructure(expected); - transposeMultiply(A, e, actual); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianFactorGraphOrdered, eliminate_empty ) -{ - // eliminate an empty factor - GaussianFactorGraphOrdered gfg; - gfg.push_back(boost::make_shared()); - GaussianConditionalOrdered::shared_ptr actualCG; - GaussianFactorGraphOrdered remainingGFG; - boost::tie(actualCG, remainingGFG) = gfg.eliminateOne(0); - - // expected Conditional Gaussian is just a parent-less node with P(x)=1 - GaussianConditionalOrdered expectedCG(0, Vector(), Matrix(), Vector()); - - // expected remaining graph should be the same as the original, still empty :-) - GaussianFactorGraphOrdered expectedLF = gfg; - - // check if the result matches - EXPECT(actualCG->equals(expectedCG)); - EXPECT(remainingGFG.equals(expectedLF)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp b/gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp deleted file mode 100644 index 53630b717..000000000 --- a/gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianJunctionTree.cpp - * @date Jul 8, 2010 - * @author Kai Ni - */ - -#include -#include - -#include -#include // for operator += -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index x2=0, x1=1, x3=2, x4=3; - -static GaussianFactorGraphOrdered createChain() { - - typedef GaussianFactorGraphOrdered::sharedFactor Factor; - SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5); - Factor factor1(new JacobianFactorOrdered(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor2(new JacobianFactorOrdered(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor3(new JacobianFactorOrdered(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model)); - Factor factor4(new JacobianFactorOrdered(x4, Matrix_(1,1,1.), Vector_(1,1.), model)); - - GaussianFactorGraphOrdered fg; - fg.push_back(factor1); - fg.push_back(factor2); - fg.push_back(factor3); - fg.push_back(factor4); - - return fg; -} - -/* ************************************************************************* */ -/** - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - * - * x2 x1 x3 x4 b - * 1 1 1 - * 1 1 1 - * 1 1 1 - * 1 1 - * - * 1 0 0 1 - */ -TEST( GaussianJunctionTreeOrdered, eliminate ) -{ - GaussianFactorGraphOrdered fg = createChain(); - GaussianJunctionTreeOrdered junctionTree(fg); - BayesTreeOrdered::sharedClique rootClique = junctionTree.eliminate(&EliminateQROrdered); - - typedef BayesTreeOrdered::sharedConditional sharedConditional; - Matrix two = Matrix_(1,1,2.); - Matrix one = Matrix_(1,1,1.); - - BayesTreeOrdered bayesTree_expected; - Index keys_root[] = {x3,x4}; - Matrix rsd_root = Matrix_(2,3, 2., 2., 2., 0., 2., 2.); - size_t dim_root[] = {1, 1, 1}; - sharedConditional root_expected(new GaussianConditionalOrdered(keys_root, keys_root+2, 2, - VerticalBlockView(rsd_root, dim_root, dim_root+3, 2), ones(2))); - BayesTreeOrdered::sharedClique rootClique_expected(new BayesTreeOrdered::Clique(root_expected)); - - Index keys_child[] = {x2,x1,x3}; - Matrix rsd_child = Matrix_(2,4, 2., 1., 1., 2., 0., -1., 1., 0.); - size_t dim_child[] = {1, 1, 1, 1}; - sharedConditional child_expected(new GaussianConditionalOrdered(keys_child, keys_child+3, 2, - VerticalBlockView(rsd_child, dim_child, dim_child+4, 2), ones(2))); - BayesTreeOrdered::sharedClique childClique_expected(new BayesTreeOrdered::Clique(child_expected)); - - bayesTree_expected.insert(rootClique_expected); - bayesTree_expected.insert(childClique_expected); - -// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x4, Vector_(1,2.), two, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.)))); -// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.)))); - CHECK(assert_equal(*bayesTree_expected.root(), *rootClique)); - EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front()))); -} - -/* ************************************************************************* */ -TEST( GaussianJunctionTreeOrdered, GBNConstructor ) -{ - GaussianFactorGraphOrdered fg = createChain(); - GaussianJunctionTreeOrdered jt(fg); - BayesTreeOrdered::sharedClique root = jt.eliminate(&EliminateQROrdered); - BayesTreeOrdered expected; - expected.insert(root); - - GaussianBayesNetOrdered bn(*GaussianSequentialSolver(fg).eliminate()); - BayesTreeOrdered actual(bn); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( GaussianJunctionTreeOrdered, optimizeMultiFrontal ) -{ - GaussianFactorGraphOrdered fg = createChain(); - GaussianJunctionTreeOrdered tree(fg); - - VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered); - VectorValuesOrdered expected(vector(4,1)); - expected[x1] = Vector_(1, 0.); - expected[x2] = Vector_(1, 1.); - expected[x3] = Vector_(1, 0.); - expected[x4] = Vector_(1, 1.); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeOrdered, complicatedMarginal) { - - // Create the conditionals to go in the BayesTree - GaussianConditionalOrdered::shared_ptr R_1_2(new GaussianConditionalOrdered( - pair_list_of - (1, (Matrix(3,1) << - 0.2630, - 0, - 0).finished()) - (2, (Matrix(3,2) << - 0.7482, 0.2290, - 0.4505, 0.9133, - 0, 0.1524).finished()) - (5, (Matrix(3,1) << - 0.8258, - 0.5383, - 0.9961).finished()), - 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3))); - GaussianConditionalOrdered::shared_ptr R_3_4(new GaussianConditionalOrdered( - pair_list_of - (3, (Matrix(3,1) << - 0.0540, - 0, - 0).finished()) - (4, (Matrix(3,2) << - 0.9340, 0.4694, - 0.1299, 0.0119, - 0, 0.3371).finished()) - (6, (Matrix(3,2) << - 0.1622, 0.5285, - 0.7943, 0.1656, - 0.3112, 0.6020).finished()), - 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3))); -// GaussianConditionalOrdered::shared_ptr R_5_6(new GaussianConditionalOrdered( -// pair_list_of -// (5, (Matrix(3,1) << -// 0.2435, -// 0, -// 0).finished()) -// (6, (Matrix(3,2) << -// 0.4733, 0.1966, -// 0.9022, 0.0979, -// 0.0, 0.2312).finished()) // Attempted to recreate without permutation -// (7, (Matrix(3,1) << -// 0.5853, -// 1.0589, -// 0.1487).finished()) -// (8, (Matrix(3,2) << -// 0.2858, 0.3804, -// 0.9893, 0.2912, -// 0.4035, 0.4933).finished()), -// 2, (Vector(3) << 0.8173, 0.4164, 0.7671).finished(), ones(3))); - GaussianConditionalOrdered::shared_ptr R_5_6(new GaussianConditionalOrdered( - pair_list_of - (5, (Matrix(3,1) << - 0.2435, - 0, - 0).finished()) - (6, (Matrix(3,2) << - 0.4733, 0.1966, - 0.3517, 0.2511, - 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form - // here since this test was written when we had column permutations - // from LDL. The code still works currently (does not enfore - // upper-triangularity in this case) but this test will need to be - // redone if this stops working in the future - (7, (Matrix(3,1) << - 0.5853, - 0.5497, - 0.9172).finished()) - (8, (Matrix(3,2) << - 0.2858, 0.3804, - 0.7572, 0.5678, - 0.7537, 0.0759).finished()), - 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3))); - GaussianConditionalOrdered::shared_ptr R_7_8(new GaussianConditionalOrdered( - pair_list_of - (7, (Matrix(3,1) << - 0.2551, - 0, - 0).finished()) - (8, (Matrix(3,2) << - 0.8909, 0.1386, - 0.9593, 0.1493, - 0, 0.2575).finished()) - (11, (Matrix(3,1) << - 0.8407, - 0.2543, - 0.8143).finished()), - 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3))); - GaussianConditionalOrdered::shared_ptr R_9_10(new GaussianConditionalOrdered( - pair_list_of - (9, (Matrix(3,1) << - 0.7952, - 0, - 0).finished()) - (10, (Matrix(3,2) << - 0.4456, 0.7547, - 0.6463, 0.2760, - 0, 0.6797).finished()) - (11, (Matrix(3,1) << - 0.6551, - 0.1626, - 0.1190).finished()) - (12, (Matrix(3,2) << - 0.4984, 0.5853, - 0.9597, 0.2238, - 0.3404, 0.7513).finished()), - 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3))); - GaussianConditionalOrdered::shared_ptr R_11_12(new GaussianConditionalOrdered( - pair_list_of - (11, (Matrix(3,1) << - 0.0971, - 0, - 0).finished()) - (12, (Matrix(3,2) << - 0.3171, 0.4387, - 0.9502, 0.3816, - 0, 0.7655).finished()), - 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3))); - - // Gaussian Bayes Tree - typedef BayesTreeOrdered GaussianBayesTree; - typedef GaussianBayesTree::Clique Clique; - typedef GaussianBayesTree::sharedClique sharedClique; - - // Create Bayes Tree - GaussianBayesTree bt; - bt.insert(sharedClique(new Clique(R_11_12))); - bt.insert(sharedClique(new Clique(R_9_10))); - bt.insert(sharedClique(new Clique(R_7_8))); - bt.insert(sharedClique(new Clique(R_5_6))); - bt.insert(sharedClique(new Clique(R_3_4))); - bt.insert(sharedClique(new Clique(R_1_2))); - - // Marginal on 5 - Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); - JacobianFactorOrdered::shared_ptr actualJacobianChol= boost::dynamic_pointer_cast( - bt.marginalFactor(5, EliminateCholeskyOrdered)); - JacobianFactorOrdered::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast( - bt.marginalFactor(5, EliminateQROrdered)); - CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same - LONGS_EQUAL(1, actualJacobianChol->rows()); - LONGS_EQUAL(1, actualJacobianChol->size()); - LONGS_EQUAL(5, actualJacobianChol->keys()[0]); - Matrix actualA = actualJacobianChol->getA(actualJacobianChol->begin()); - Matrix actualCov = inverse(actualA.transpose() * actualA); - EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); - - // Marginal on 6 -// expectedCov = (Matrix(2,2) << -// 8471.2, 2886.2, -// 2886.2, 1015.8).finished(); - expectedCov = (Matrix(2,2) << - 1015.8, 2886.2, - 2886.2, 8471.2).finished(); - actualJacobianChol = boost::dynamic_pointer_cast( - bt.marginalFactor(6, EliminateCholeskyOrdered)); - actualJacobianQR = boost::dynamic_pointer_cast( - bt.marginalFactor(6, EliminateQROrdered)); - CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same - LONGS_EQUAL(2, actualJacobianChol->rows()); - LONGS_EQUAL(1, actualJacobianChol->size()); - LONGS_EQUAL(6, actualJacobianChol->keys()[0]); - actualA = actualJacobianChol->getA(actualJacobianChol->begin()); - actualCov = inverse(actualA.transpose() * actualA); - EXPECT(assert_equal(expectedCov, actualCov, 1e1)); - -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testHessianFactorObsolete.cpp b/gtsam/linear/tests/testHessianFactorObsolete.cpp deleted file mode 100644 index 09c3b6a64..000000000 --- a/gtsam/linear/tests/testHessianFactorObsolete.cpp +++ /dev/null @@ -1,635 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testCholeskyFactor.cpp - * @author Richard Roberts - * @date Dec 15, 2010 - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -const double tol = 1e-5; - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, emptyConstructor) { - HessianFactorOrdered f; - DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero - EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty - EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix - DOUBLES_EQUAL(0.0, f.error(VectorValuesOrdered()), 1e-9); // Should have zero error -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, ConversionConstructor) { - - HessianFactorOrdered expected; - expected.keys_.push_back(0); - expected.keys_.push_back(1); - size_t dims[] = { 2, 4, 1 }; - expected.info_.resize(dims, dims+3, false); - expected.matrix_ = Matrix_(7,7, - 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, - 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, - -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, - 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000, - -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000, - 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000, - 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500); - - // sigmas - double sigma1 = 0.2; - double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); - - // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - Matrix Al1x1 = Matrix_(4,4, - // l1 x1 - 1., 0., 0.00, 0., // f4 - 0., 1., 0.00, 0., // f4 - 0., 0., -1., 0., // f2 - 0., 0., 0.00,-1. // f2 - ); - - // the RHS - Vector b2(4); - b2(0) = -0.2; - b2(1) = 0.3; - b2(2) = 0.2; - b2(3) = -0.1; - - vector > meas; - meas.push_back(make_pair(0, Ax2)); - meas.push_back(make_pair(1, Al1x1)); - JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - - HessianFactorOrdered actual(combined); - - VectorValuesOrdered values(std::vector(dims, dims+2)); - values[0] = Vector_(2, 1.0, 2.0); - values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0); - - EXPECT_LONGS_EQUAL(2, actual.size()); - - EXPECT(assert_equal(expected, actual, 1e-9)); - - // error terms - EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor1) -{ - Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g = Vector_(2, -8.0, -9.0); - double f = 10.0; - - Vector dxv = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(2); - VectorValuesOrdered dx(dims); - - dx[0] = dxv; - - HessianFactorOrdered factor(0, G, g, f); - - // extract underlying parts - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); - EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); - - // error 0.5*(f - 2*x'*g + x'*G*x) - double expected = 80.375; - double actual = factor.error(dx); - double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView() * dxv); - EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10); - DOUBLES_EQUAL(expected, actual, 1e-10); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor1b) -{ - Vector mu = Vector_(2,1.0,2.0); - Matrix Sigma = eye(2,2); - - HessianFactorOrdered factor(0, mu, Sigma); - - Matrix G = eye(2,2); - Vector g = G*mu; - double f = dot(g,mu); - - // Check - Matrix info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(Matrix(G), info_matrix)); - EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); - EXPECT_LONGS_EQUAL(1, factor.size()); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor2) -{ - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValuesOrdered dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - - HessianFactorOrdered factor(0, 1, G11, G12, g1, G22, g2, f); - - double expected = 90.5; - double actual = factor.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); - - Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, factor.linearTerm())); - - EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); - EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); - - // Check case when vector values is larger than factor - dims.push_back(2); - VectorValuesOrdered dxLarge(dims); - dxLarge[0] = dx0; - dxLarge[1] = dx1; - dxLarge[2] = Vector_(2, 0.1, 0.2); - EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, Constructor3) -{ - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); - - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); - - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValuesOrdered dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; - - HessianFactorOrdered factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); - - double expected = 371.3750; - double actual = factor.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); - - Vector linearExpected(6); linearExpected << g1, g2, g3; - EXPECT(assert_equal(linearExpected, factor.linearTerm())); - - EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); - EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); - EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); - EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); - EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, ConstructorNWay) -{ - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); - - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); - - Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); - - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(3); - VectorValuesOrdered dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - dx[2] = dx2; - - - std::vector js; - js.push_back(0); js.push_back(1); js.push_back(2); - std::vector Gs; - Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); - HessianFactorOrdered factor(js, Gs, gs, f); - - double expected = 371.3750; - double actual = factor.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(7, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); - - Vector linearExpected(6); linearExpected << g1, g2, g3; - EXPECT(assert_equal(linearExpected, factor.linearTerm())); - - EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); - EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); - EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); - EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); - EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); - EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); -} - -/* ************************************************************************* */ -TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) -{ - Matrix G11 = Matrix_(1,1, 1.0); - Matrix G12 = Matrix_(1,2, 2.0, 4.0); - Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - double f = 10.0; - - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - - vector dims; - dims.push_back(1); - dims.push_back(2); - VectorValuesOrdered dx(dims); - - dx[0] = dx0; - dx[1] = dx1; - - HessianFactorOrdered originalFactor(0, 1, G11, G12, g1, G22, g2, f); - - // Make a copy - HessianFactorOrdered copy1(originalFactor); - - double expected = 90.5; - double actual = copy1.error(dx); - - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy1.rows()); - DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10); - - Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, copy1.linearTerm())); - - EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin()))); - EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1))); - EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); - - // Make a copy using the assignment operator - HessianFactorOrdered copy2; - copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references - - actual = copy2.error(dx); - DOUBLES_EQUAL(expected, actual, 1e-10); - LONGS_EQUAL(4, copy2.rows()); - DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10); - - EXPECT(assert_equal(linearExpected, copy2.linearTerm())); - - EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin()))); - EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1))); - EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate) -{ - Matrix A01 = 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, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); - - Matrix A10 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0); - Matrix A11 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); - - Matrix A21 = Matrix_(3,3, - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); - - GaussianFactorGraphOrdered gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); - - // create a full, uneliminated version of the factor - JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); - - // perform elimination - GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1); - - // create expected Hessian after elimination - HessianFactorOrdered expectedCholeskyFactor(expectedFactor); - - // Convert all factors to hessians - FactorGraphOrdered hessians; - BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { - if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(hf); - else if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) - hessians.push_back(boost::make_shared(*jf)); - else - CHECK(false); - } - - // Eliminate - GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1); - HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); - - EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); - EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, eliminate2 ) -{ - // sigmas - double sigma1 = 0.2; - double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); - - // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - Matrix Al1x1 = Matrix_(4,4, - // l1 x1 - 1., 0., 0.00, 0., // f4 - 0., 1., 0.00, 0., // f4 - 0., 0., -1., 0., // f2 - 0., 0., 0.00,-1. // f2 - ); - - // the RHS - Vector b2(4); - b2(0) = -0.2; - b2(1) = 0.3; - b2(2) = 0.2; - b2(3) = -0.1; - - vector > meas; - meas.push_back(make_pair(0, Ax2)); - meas.push_back(make_pair(1, Al1x1)); - JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - - // eliminate the combined factor - HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined)); - FactorGraphOrdered combinedLFG_Chol; - combinedLFG_Chol.push_back(combinedLF_Chol); - - GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered( - combinedLFG_Chol, 1); - HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast< - HessianFactorOrdered>(actual_Chol.second); - - // create expected Conditional Gaussian - double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = Matrix_(2,2, - 1.00, 0.00, - 0.00, 1.00 - )/oldSigma; - Matrix S12 = Matrix_(2,4, - -0.20, 0.00,-0.80, 0.00, - +0.00,-0.20,+0.00,-0.80 - )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditionalOrdered expectedCG(0,d,R11,1,S12,ones(2)); - EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4)); - - // the expected linear factor - double sigma = 0.2236; - Matrix Bl1x1 = Matrix_(2,4, - // l1 x1 - 1.00, 0.00, -1.00, 0.00, - 0.00, 1.00, +0.00, -1.00 - )/sigma; - Vector b1 = Vector_(2,0.0,0.894427); - JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3)); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, eliminateUnsorted) { - - JacobianFactorOrdered::shared_ptr factor1( - new JacobianFactorOrdered(0, - Matrix_(3,3, - 44.7214, 0.0, 0.0, - 0.0, 44.7214, 0.0, - 0.0, 0.0, 44.7214), - 1, - Matrix_(3,3, - -0.179168, -44.721, 0.717294, - 44.721, -0.179168, -44.9138, - 0.0, 0.0, -44.7214), - Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), - noiseModel::Unit::Create(3))); - HessianFactorOrdered::shared_ptr unsorted_factor2( - new HessianFactorOrdered(JacobianFactorOrdered(0, - Matrix_(6,3, - 25.8367, 0.1211, 0.0593, - 0.0, 23.4099, 30.8733, - 0.0, 0.0, 25.8729, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0), - 1, - Matrix_(6,3, - 25.7429, -1.6897, 0.4587, - 1.6400, 23.3095, -8.4816, - 0.0034, 0.0509, -25.7855, - 0.9997, -0.0002, 0.0824, - 0.0, 0.9973, 0.9517, - 0.0, 0.0, 0.9973), - Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6)))); - Permutation permutation(2); - permutation[0] = 1; - permutation[1] = 0; - unsorted_factor2->permuteWithInverse(permutation); - - HessianFactorOrdered::shared_ptr sorted_factor2( - new HessianFactorOrdered(JacobianFactorOrdered(0, - Matrix_(6,3, - 25.7429, -1.6897, 0.4587, - 1.6400, 23.3095, -8.4816, - 0.0034, 0.0509, -25.7855, - 0.9997, -0.0002, 0.0824, - 0.0, 0.9973, 0.9517, - 0.0, 0.0, 0.9973), - 1, - Matrix_(6,3, - 25.8367, 0.1211, 0.0593, - 0.0, 23.4099, 30.8733, - 0.0, 0.0, 25.8729, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0), - Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6)))); - - GaussianFactorGraphOrdered sortedGraph; -// sortedGraph.push_back(factor1); - sortedGraph.push_back(sorted_factor2); - - GaussianFactorGraphOrdered unsortedGraph; -// unsortedGraph.push_back(factor1); - unsortedGraph.push_back(unsorted_factor2); - - GaussianConditionalOrdered::shared_ptr expected_bn; - GaussianFactorOrdered::shared_ptr expected_factor; - boost::tie(expected_bn, expected_factor) = - EliminatePreferCholeskyOrdered(sortedGraph, 1); - - GaussianConditionalOrdered::shared_ptr actual_bn; - GaussianFactorOrdered::shared_ptr actual_factor; - boost::tie(actual_bn, actual_factor) = - EliminatePreferCholeskyOrdered(unsortedGraph, 1); - - EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10)); - EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10)); -} - -/* ************************************************************************* */ -TEST(HessianFactorOrdered, combine) { - - // update the information matrix with a single jacobian factor - Matrix A0 = Matrix_(2, 2, - 11.1803399, 0.0, - 0.0, 11.1803399); - Matrix A1 = Matrix_(2, 2, - -2.23606798, 0.0, - 0.0, -2.23606798); - Matrix A2 = Matrix_(2, 2, - -8.94427191, 0.0, - 0.0, -8.94427191); - Vector b = Vector_(2, 2.23606798,-1.56524758); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); - GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model)); - FactorGraphOrdered factors; - factors.push_back(f); - - // Form Ab' * Ab - HessianFactorOrdered actual(factors); - - Matrix expected = Matrix_(7, 7, - 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, - 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, - -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000, - 0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000, - -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, - 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, - 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500); - EXPECT(assert_equal(Matrix(expected.triangularView()), - Matrix(actual.matrix_.triangularView()), tol)); - -} -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactorObsolete.cpp b/gtsam/linear/tests/testJacobianFactorObsolete.cpp deleted file mode 100644 index 8e94d6f09..000000000 --- a/gtsam/linear/tests/testJacobianFactorObsolete.cpp +++ /dev/null @@ -1,468 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testJacobianFactor.cpp - * @brief Unit tests for Linear Factor - * @author Christian Potthast - * @author Frank Dellaert - **/ - -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index _x0_=0, _x1_=1, _x2_=2, _x_=5, _y_=6, _l11_=8; - -static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, constructor) -{ - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - JacobianFactorOrdered actual(terms, b, noise); - JacobianFactorOrdered expected(_x0_, eye(3), _x1_, 2.*eye(3), b, noise); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, constructor2) -{ - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - const JacobianFactorOrdered actual(terms, b, noise); - - JacobianFactorOrdered::const_iterator key0 = actual.begin(); - JacobianFactorOrdered::const_iterator key1 = key0 + 1; - EXPECT(assert_equal(*key0, _x0_)); - EXPECT(assert_equal(*key1, _x1_)); - EXPECT(!actual.empty()); - EXPECT_LONGS_EQUAL(3, actual.Ab_.nBlocks()); - - Matrix actualA0 = actual.getA(key0); - Matrix actualA1 = actual.getA(key1); - Vector actualb = actual.getb(); - - EXPECT(assert_equal(eye(3), actualA0)); - EXPECT(assert_equal(2.*eye(3), actualA1)); - EXPECT(assert_equal(b, actualb)); -} - -/* ************************************************************************* */ - -JacobianFactorOrdered construct() { - Matrix A = Matrix_(2,2, 1.,2.,3.,4.); - Vector b = Vector_(2, 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0)); - JacobianFactorOrdered::shared_ptr shared( - new JacobianFactorOrdered(0, A, b, s)); - return *shared; -} - -TEST(JacobianFactorOrdered, return_value) -{ - Matrix A = Matrix_(2,2, 1.,2.,3.,4.); - Vector b = Vector_(2, 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0)); - JacobianFactorOrdered copied = construct(); - EXPECT(assert_equal(b, copied.getb())); - EXPECT(assert_equal(*s, *copied.get_model())); - EXPECT(assert_equal(A, copied.getA(copied.begin()))); -} - -/* ************************************************************************* */ -TEST(JabobianFactor, Hessian_conversion) { - HessianFactorOrdered hessian(0, (Matrix(4,4) << - 1.57, 2.695, -1.1, -2.35, - 2.695, 11.3125, -0.65, -10.225, - -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), - (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), - 73.1725); - - JacobianFactorOrdered expected(0, (Matrix(2,4) << - 1.2530, 2.1508, -0.8779, -1.8755, - 0, 2.5858, 0.4789, -2.3943).finished(), - (Vector(2) << -6.2929, -5.7941).finished(), - noiseModel::Unit::Create(2)); - - JacobianFactorOrdered actual(hessian); - - EXPECT(assert_equal(expected, actual, 1e-3)); -} - -/* ************************************************************************* */ -TEST( JacobianFactorOrdered, constructor_combined){ - double sigma1 = 0.0957; - Matrix A11(2,2); - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 1; - Vector b(2); - b(0) = 2; b(1) = -1; - JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(0, A11, b*sigma1, noiseModel::Isotropic::Sigma(2,sigma1))); - - double sigma2 = 0.5; - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -1; - b(0) = 4 ; b(1) = -5; - JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(0, A11, b*sigma2, noiseModel::Isotropic::Sigma(2,sigma2))); - - double sigma3 = 0.25; - A11(0,0) = 1; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = -1; - b(0) = 3 ; b(1) = -88; - JacobianFactorOrdered::shared_ptr f3(new JacobianFactorOrdered(0, A11, b*sigma3, noiseModel::Isotropic::Sigma(2,sigma3))); - - // TODO: find a real sigma value for this example - double sigma4 = 0.1; - A11(0,0) = 6; A11(0,1) = 0; - A11(1,0) = 0; A11(1,1) = 7; - b(0) = 5 ; b(1) = -6; - JacobianFactorOrdered::shared_ptr f4(new JacobianFactorOrdered(0, A11*sigma4, b*sigma4, noiseModel::Isotropic::Sigma(2,sigma4))); - - GaussianFactorGraphOrdered lfg; - lfg.push_back(f1); - lfg.push_back(f2); - lfg.push_back(f3); - lfg.push_back(f4); - JacobianFactorOrdered combined(lfg); - - Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); - Matrix A22(8,2); - A22(0,0) = 1; A22(0,1) = 0; - A22(1,0) = 0; A22(1,1) = 1; - A22(2,0) = 1; A22(2,1) = 0; - A22(3,0) = 0; A22(3,1) = -1; - A22(4,0) = 1; A22(4,1) = 0; - A22(5,0) = 0; A22(5,1) = -1; - A22(6,0) = 0.6; A22(6,1) = 0; - A22(7,0) = 0; A22(7,1) = 0.7; - Vector exb(8); - exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; - exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; - - vector > meas; - meas.push_back(make_pair(0, A22)); - JacobianFactorOrdered expected(meas, exb, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expected,combined)); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, linearFactorN){ - Matrix I = eye(2); - GaussianFactorGraphOrdered f; - SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1.0); - f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(0, I, Vector_(2, 10.0, 5.0), model))); - f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(0, -10 * I, 1, 10 * I, Vector_(2, 1.0, -2.0), model))); - f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(1, -10 * I, 2, 10 * I, Vector_(2, 1.5, -1.5), model))); - f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(2, -10 * I, 3, 10 * I, Vector_(2, 2.0, -1.0), model))); - - JacobianFactorOrdered combinedFactor(f); - - vector > combinedMeasurement; - combinedMeasurement.push_back(make_pair(0, Matrix_(8,2, - 1.0, 0.0, - 0.0, 1.0, - -10.0, 0.0, - 0.0,-10.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0))); - combinedMeasurement.push_back(make_pair(1, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0, 10.0, - -10.0, 0.0, - 0.0,-10.0, - 0.0, 0.0, - 0.0, 0.0))); - combinedMeasurement.push_back(make_pair(2, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0, 10.0, - -10.0, 0.0, - 0.0,-10.0))); - combinedMeasurement.push_back(make_pair(3, Matrix_(8,2, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 10.0, 0.0, - 0.0,10.0))); - Vector b = Vector_(8, - 10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); - - Vector sigmas = repeat(8,1.0); - JacobianFactorOrdered expected(combinedMeasurement, b, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expected,combinedFactor)); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, error) { - Vector b = Vector_(3, 1., 2., 3.); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,2.,2.,2.)); - std::list > terms; - terms.push_back(make_pair(_x0_, eye(3))); - terms.push_back(make_pair(_x1_, 2.*eye(3))); - const JacobianFactorOrdered jf(terms, b, noise); - - VectorValuesOrdered values(2, 3); - values[0] = Vector_(3, 1.,2.,3.); - values[1] = Vector_(3, 4.,5.,6.); - - Vector expected_unwhitened = Vector_(3, 8., 10., 12.); - Vector actual_unwhitened = jf.unweighted_error(values); - EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); - - Vector expected_whitened = Vector_(3, 4., 5., 6.); - Vector actual_whitened = jf.error_vector(values); - EXPECT(assert_equal(expected_whitened, actual_whitened)); - - // check behavior when there are more values than in this factor - VectorValuesOrdered largeValues(3, 3); - largeValues[0] = Vector_(3, 1.,2.,3.); - largeValues[1] = Vector_(3, 4.,5.,6.); - largeValues[2] = Vector_(3, 7.,8.,9.); - - EXPECT(assert_equal(expected_unwhitened, jf.unweighted_error(largeValues))); - EXPECT(assert_equal(expected_whitened, jf.error_vector(largeValues))); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, operators ) -{ - SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - - Matrix I = eye(2); - Vector b = Vector_(2,0.2,-0.1); - JacobianFactorOrdered lf(_x1_, -I, _x2_, I, b, sigma0_1); - - VectorValuesOrdered c; - c.insert(_x1_, Vector_(2,10.,20.)); - c.insert(_x2_, Vector_(2,30.,60.)); - - // test A*x - Vector expectedE = Vector_(2,200.,400.), e = lf*c; - EXPECT(assert_equal(expectedE,e)); - - // test A^e - VectorValuesOrdered expectedX; - expectedX.insert(_x1_, Vector_(2,-2000.,-4000.)); - expectedX.insert(_x2_, Vector_(2, 2000., 4000.)); - VectorValuesOrdered actualX = VectorValuesOrdered::Zero(expectedX); - lf.transposeMultiplyAdd(1.0, e, actualX); - EXPECT(assert_equal(expectedX, actualX)); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, eliminate2 ) -{ - // sigmas - double sigma1 = 0.2; - double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); - - // the combined linear factor - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - Matrix Al1x1 = Matrix_(4,4, - // l1 x1 - 1., 0., 0.00, 0., // f4 - 0., 1., 0.00, 0., // f4 - 0., 0., -1., 0., // f2 - 0., 0., 0.00,-1. // f2 - ); - - // the RHS - Vector b2(4); - b2(0) = -0.2; - b2(1) = 0.3; - b2(2) = 0.2; - b2(3) = -0.1; - - vector > meas; - meas.push_back(make_pair(_x2_, Ax2)); - meas.push_back(make_pair(_l11_, Al1x1)); - JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); - - // eliminate the combined factor - GaussianConditionalOrdered::shared_ptr actualCG_QR; - JacobianFactorOrdered::shared_ptr actualLF_QR(new JacobianFactorOrdered(combined)); - actualCG_QR = actualLF_QR->eliminateFirst(); - - // create expected Conditional Gaussian - double oldSigma = 0.0894427; // from when R was made unit - Matrix R11 = Matrix_(2,2, - 1.00, 0.00, - 0.00, 1.00 - )/oldSigma; - Matrix S12 = Matrix_(2,4, - -0.20, 0.00,-0.80, 0.00, - +0.00,-0.20,+0.00,-0.80 - )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; - GaussianConditionalOrdered expectedCG(_x2_,d,R11,_l11_,S12,ones(2)); - - EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().firstBlock()); - EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().rowStart()); - EXPECT_LONGS_EQUAL(2, actualCG_QR->rsd().rowEnd()); - EXPECT_LONGS_EQUAL(3, actualCG_QR->rsd().nBlocks()); - EXPECT(assert_equal(expectedCG,*actualCG_QR,1e-4)); - - // the expected linear factor - double sigma = 0.2236; - Matrix Bl1x1 = Matrix_(2,4, - // l1 x1 - 1.00, 0.00, -1.00, 0.00, - 0.00, 1.00, +0.00, -1.00 - )/sigma; - Vector b1 = Vector_(2,0.0,0.894427); - JacobianFactorOrdered expectedLF(_l11_, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); - EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3)); -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, default_error ) -{ - JacobianFactorOrdered f; - vector dims; - VectorValuesOrdered c(dims); - double actual = f.error(c); - EXPECT(actual==0.0); -} - -//* ************************************************************************* */ -TEST(JacobianFactorOrdered, empty ) -{ - // create an empty factor - JacobianFactorOrdered f; - EXPECT(f.empty()==true); -} - -/* ************************************************************************* */ -// small aux. function to print out lists of anything -template -void print(const list& i) { - copy(i.begin(), i.end(), ostream_iterator (cout, ",")); - cout << endl; -} - -/* ************************************************************************* */ -TEST(JacobianFactorOrdered, CONSTRUCTOR_GaussianConditional ) -{ - Matrix R11 = eye(2); - Matrix S12 = Matrix_(2,2, - -0.200001, 0.00, - +0.00,-0.200001 - ); - Vector d(2); d(0) = 2.23607; d(1) = -1.56525; - Vector sigmas =repeat(2,0.29907); - GaussianConditionalOrdered::shared_ptr CG(new GaussianConditionalOrdered(_x2_,d,R11,_l11_,S12,sigmas)); - - // Call the constructor we are testing ! - JacobianFactorOrdered actualLF(*CG); - - JacobianFactorOrdered expectedLF(_x2_,R11,_l11_,S12,d, noiseModel::Diagonal::Sigmas(sigmas)); - EXPECT(assert_equal(expectedLF,actualLF,1e-5)); -} - -/* ************************************************************************* */ -TEST ( JacobianFactorOrdered, constraint_eliminate1 ) -{ - // construct a linear constraint - Vector v(2); v(0)=1.2; v(1)=3.4; - Index key = _x0_; - JacobianFactorOrdered lc(key, eye(2), v, constraintModel); - - // eliminate it - GaussianConditionalOrdered::shared_ptr actualCG; - JacobianFactorOrdered::shared_ptr actualLF(new JacobianFactorOrdered(lc)); - actualCG = actualLF->eliminateFirst(); - - // verify linear factor - EXPECT(actualLF->size() == 0); - - // verify conditional Gaussian - Vector sigmas = Vector_(2, 0.0, 0.0); - GaussianConditionalOrdered expCG(_x0_, v, eye(2), sigmas); - EXPECT(assert_equal(expCG, *actualCG)); -} - -/* ************************************************************************* */ -TEST ( JacobianFactorOrdered, constraint_eliminate2 ) -{ - // Construct a linear constraint - // RHS - Vector b(2); b(0)=3.0; b(1)=4.0; - - // A1 - invertible - Matrix A1(2,2); - A1(0,0) = 1.0 ; A1(0,1) = 2.0; - A1(1,0) = 2.0 ; A1(1,1) = 1.0; - - // A2 - not invertible - Matrix A2(2,2); - A2(0,0) = 1.0 ; A2(0,1) = 2.0; - A2(1,0) = 2.0 ; A2(1,1) = 4.0; - - JacobianFactorOrdered lc(_x_, A1, _y_, A2, b, constraintModel); - - // eliminate x and verify results - GaussianConditionalOrdered::shared_ptr actualCG; - JacobianFactorOrdered::shared_ptr actualLF(new JacobianFactorOrdered(lc)); - actualCG = actualLF->eliminateFirst(); - - // LF should be null - JacobianFactorOrdered expectedLF; - EXPECT(assert_equal(*actualLF, expectedLF)); - - // verify CG - Matrix R = Matrix_(2, 2, - 1.0, 2.0, - 0.0, 1.0); - Matrix S = Matrix_(2,2, - 1.0, 2.0, - 0.0, 0.0); - Vector d = Vector_(2, 3.0, 0.6666); - GaussianConditionalOrdered expectedCG(_x_, d, R, _y_, S, zero(2)); - EXPECT(assert_equal(expectedCG, *actualCG, 1e-4)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorValuesObsolete.cpp b/gtsam/linear/tests/testVectorValuesObsolete.cpp deleted file mode 100644 index 264c4543b..000000000 --- a/gtsam/linear/tests/testVectorValuesObsolete.cpp +++ /dev/null @@ -1,456 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testVectorValues.cpp - * @author Richard Roberts - * @date Sep 16, 2010 - */ - -#include - -#include -#include -#include - -#include - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, insert) { - - // insert, with out-of-order indices - VectorValuesOrdered actual; - actual.insert(0, Vector_(1, 1.0)); - actual.insert(1, Vector_(2, 2.0, 3.0)); - actual.insert(5, Vector_(2, 6.0, 7.0)); - actual.insert(2, Vector_(2, 4.0, 5.0)); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); - CHECK_EXCEPTION(actual.dim(3), out_of_range); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, dimsConstructor) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValuesOrdered actual(dims); - actual[0] = Vector_(1, 1.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, copyConstructor) { - - // insert, with out-of-order indices - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValuesOrdered actual(original); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, assignment) { - - VectorValuesOrdered actual; - - { - // insert, with out-of-order indices - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - actual = original; - } - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector())); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, SameStructure) { - // insert, with out-of-order indices - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValuesOrdered actual(VectorValuesOrdered::SameStructure(original)); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, Zero_fromModel) { - // insert, with out-of-order indices - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValuesOrdered actual(VectorValuesOrdered::Zero(original)); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Values - EXPECT(assert_equal(Vector::Zero(1), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[5])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, Zero_fromDims) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValuesOrdered actual(VectorValuesOrdered::Zero(dims)); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Values - EXPECT(assert_equal(Vector::Zero(1), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, Zero_fromUniform) { - VectorValuesOrdered actual(VectorValuesOrdered::Zero(3, 2)); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(2, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Values - EXPECT(assert_equal(Vector::Zero(2), actual[0])); - EXPECT(assert_equal(Vector::Zero(2), actual[1])); - EXPECT(assert_equal(Vector::Zero(2), actual[2])); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, resizeLike) { - // insert, with out-of-order indices - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(5, Vector_(2, 6.0, 7.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - - VectorValuesOrdered actual(10, 3); - actual.resizeLike(original); - - // Check dimensions - LONGS_EQUAL(6, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(2, actual.dim(5)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(!actual.exists(3)); - EXPECT(!actual.exists(4)); - EXPECT(actual.exists(5)); - EXPECT(!actual.exists(6)); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, resize_fromUniform) { - VectorValuesOrdered actual(4, 10); - actual.resize(3, 2); - - actual[0] = Vector_(2, 1.0, 2.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(2, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, resize_fromDims) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - - VectorValuesOrdered actual(4, 10); - actual.resize(dims); - actual[0] = Vector_(1, 1.0); - actual[1] = Vector_(2, 2.0, 3.0); - actual[2] = Vector_(2, 4.0, 5.0); - - // Check dimensions - LONGS_EQUAL(3, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector())); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, append) { - // insert - VectorValuesOrdered actual; - actual.insert(0, Vector_(1, 1.0)); - actual.insert(1, Vector_(2, 2.0, 3.0)); - actual.insert(2, Vector_(2, 4.0, 5.0)); - - // append - vector dims(2); - dims[0] = 3; - dims[1] = 5; - actual.append(dims); - - // Check dimensions - LONGS_EQUAL(5, actual.size()); - LONGS_EQUAL(1, actual.dim(0)); - LONGS_EQUAL(2, actual.dim(1)); - LONGS_EQUAL(2, actual.dim(2)); - LONGS_EQUAL(3, actual.dim(3)); - LONGS_EQUAL(5, actual.dim(4)); - - // Logic - EXPECT(actual.exists(0)); - EXPECT(actual.exists(1)); - EXPECT(actual.exists(2)); - EXPECT(actual.exists(3)); - EXPECT(actual.exists(4)); - EXPECT(!actual.exists(5)); - - // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - - // Check exceptions - CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, hasSameStructure) { - VectorValuesOrdered v1(2, 3); - VectorValuesOrdered v2(3, 2); - VectorValuesOrdered v3(4, 2); - VectorValuesOrdered v4(4, 2); - - EXPECT(!v1.hasSameStructure(v2)); - EXPECT(!v2.hasSameStructure(v3)); - EXPECT(v3.hasSameStructure(v4)); - EXPECT(VectorValuesOrdered().hasSameStructure(VectorValuesOrdered())); - EXPECT(!v1.hasSameStructure(VectorValuesOrdered())); -} - - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, permute) { - - VectorValuesOrdered original; - original.insert(0, Vector_(1, 1.0)); - original.insert(1, Vector_(2, 2.0, 3.0)); - original.insert(2, Vector_(2, 4.0, 5.0)); - original.insert(3, Vector_(2, 6.0, 7.0)); - - VectorValuesOrdered expected; - expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2 - expected.insert(1, Vector_(1, 1.0)); // from 0 - expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3 - expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1 - - Permutation permutation(4); - permutation[0] = 2; - permutation[1] = 0; - permutation[2] = 3; - permutation[3] = 1; - - VectorValuesOrdered actual = original; - actual.permuteInPlace(permutation); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(VectorValuesOrdered, subvector) { - VectorValuesOrdered init; - init.insert(0, Vector_(1, 1.0)); - init.insert(1, Vector_(2, 2.0, 3.0)); - init.insert(2, Vector_(2, 4.0, 5.0)); - init.insert(3, Vector_(2, 6.0, 7.0)); - - std::vector indices; - indices += 0, 2, 3; - Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0); - EXPECT(assert_equal(expSubVector, init.vector(indices))); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/testGaussianBayesNetObsolete.cpp b/tests/testGaussianBayesNetObsolete.cpp deleted file mode 100644 index 53483dd47..000000000 --- a/tests/testGaussianBayesNetObsolete.cpp +++ /dev/null @@ -1,189 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianBayesNet.cpp - * @brief Unit tests for GaussianBayesNet - * @author Frank Dellaert - */ - -// STL/C++ -#include -#include -#include -#include -#include - -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -static const Index _x_=0, _y_=1, _z_=2; - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, constructor ) -{ - // small Bayes Net x <- y - // x y d - // 1 1 9 - // 1 5 - Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0); - Matrix R22 = Matrix_(1,1,1.0); - Vector d1(1), d2(1); - d1(0) = 9; d2(0) = 5; - Vector sigmas(1); - sigmas(0) = 1.; - - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditionalOrdered x(_x_,d1,R11,_y_,S12, sigmas), y(_y_,d2,R22, sigmas); - - // check small example which uses constructor - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); - EXPECT( x.equals(*cbn[_x_]) ); - EXPECT( y.equals(*cbn[_y_]) ); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, matrix ) -{ - // Create a test graph - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); - - Matrix R; Vector d; - boost::tie(R,d) = matrix(cbn); // find matrix and RHS - - Matrix R1 = Matrix_(2,2, - 1.0, 1.0, - 0.0, 1.0 - ); - Vector d1 = Vector_(2, 9.0, 5.0); - - EXPECT(assert_equal(R,R1)); - EXPECT(assert_equal(d,d1)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, optimize ) -{ - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); - VectorValuesOrdered actual = optimize(cbn); - - VectorValuesOrdered expected(vector(2,1)); - expected[_x_] = Vector_(1,4.); - expected[_y_] = Vector_(1,5.); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, optimize2 ) -{ - - // Create empty graph - GaussianFactorGraphOrdered fg; - SharedDiagonal noise = noiseModel::Unit::Create(1); - - fg.add(_y_, eye(1), 2*ones(1), noise); - - fg.add(_x_, eye(1),_y_, -eye(1), -ones(1), noise); - - fg.add(_y_, eye(1),_z_, -eye(1), -ones(1), noise); - - fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise); - - VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize(); - - VectorValuesOrdered expected(vector(3,1)); - expected[_x_] = Vector_(1,1.); - expected[_y_] = Vector_(1,2.); - expected[_z_] = Vector_(1,3.); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, optimize3 ) -{ - // y=R*x, x=inv(R)*y - // 9 = 1 1 4 - // 5 1 5 - // NOTE: we are supplying a new RHS here - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); - - VectorValuesOrdered expected(vector(2,1)), x(vector(2,1)); - expected[_x_] = Vector_(1, 4.); - expected[_y_] = Vector_(1, 5.); - - // test functional version - VectorValuesOrdered actual = optimize(cbn); - EXPECT(assert_equal(expected,actual)); - - // test imperative version - optimizeInPlace(cbn,x); - EXPECT(assert_equal(expected,x)); -} - -/* ************************************************************************* */ -TEST( GaussianBayesNetOrdered, backSubstituteTranspose ) -{ - // x=R'*y, y=inv(R')*x - // 2 = 1 2 - // 5 1 1 3 - GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet(); - - VectorValuesOrdered y(vector(2,1)), x(vector(2,1)); - x[_x_] = Vector_(1,2.); - x[_y_] = Vector_(1,5.); - y[_x_] = Vector_(1,2.); - y[_y_] = Vector_(1,3.); - - // test functional version - VectorValuesOrdered actual = backSubstituteTranspose(cbn,x); - EXPECT(assert_equal(y,actual)); -} - -/* ************************************************************************* */ -// Tests computing Determinant -TEST( GaussianBayesNetOrdered, DeterminantTest ) -{ - GaussianBayesNetOrdered cbn; - cbn += boost::shared_ptr(new GaussianConditionalOrdered( - 0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ), - 1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0), - ones(2))); - - cbn += boost::shared_ptr(new GaussianConditionalOrdered( - 1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ), - 2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0), - ones(2))); - - cbn += boost::shared_ptr(new GaussianConditionalOrdered( - 3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), - ones(2))); - - double expectedDeterminant = 60; - double actualDeterminant = determinant(cbn); - - EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/tests/testGaussianFactorObsolete.cpp b/tests/testGaussianFactorObsolete.cpp deleted file mode 100644 index 9dd7f27ba..000000000 --- a/tests/testGaussianFactorObsolete.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testGaussianFactor.cpp - * @brief Unit tests for Linear Factor - * @author Christian Potthast - * @author Frank Dellaert - **/ - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include // for operator += -#include -#include // for insert -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static SharedDiagonal - sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), - constraintModel = noiseModel::Constrained::All(2); - -//const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // FIXME: throws exception - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, linearFactor ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - - Matrix I = eye(2); - Vector b = Vector_(2, 2.0, -1.0); - JacobianFactorOrdered expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); - - // create a small linear factor graph - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - JacobianFactorOrdered::shared_ptr lf = - boost::dynamic_pointer_cast(fg[1]); - - // check if the two factors are the same - EXPECT(assert_equal(expected,*lf)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, getDim ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // get a factor - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - GaussianFactorOrdered::shared_ptr factor = fg[0]; - - // get the size of a variable - size_t actual = factor->getDim(factor->find(ordering[kx1])); - - // verify - size_t expected = 2; - EXPECT_LONGS_EQUAL(expected, actual); -} - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, error ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - - // get the first factor from the factor graph - GaussianFactorOrdered::shared_ptr lf = fg[0]; - - // check the error of the first factor with noisy config - VectorValuesOrdered cfg = example::createZeroDelta(ordering); - - // calculate the error from the factor kf1 - // note the error is the same as in testNonlinearFactor - double actual = lf->error(cfg); - DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); -} - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, matrix ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - //GaussianFactorOrdered::shared_ptr lf = fg[1]; // NOTE: using the older version - Vector b2 = Vector_(2, 0.2, -0.1); - Matrix I = eye(2); - // render with a given ordering - OrderingOrdered ord; - ord += kx1,kx2; - JacobianFactorOrdered::shared_ptr lf(new JacobianFactorOrdered(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); - - // Test whitened version - Matrix A_act1; Vector b_act1; - boost::tie(A_act1,b_act1) = lf->matrix(true); - - Matrix A1 = Matrix_(2,4, - -10.0, 0.0, 10.0, 0.0, - 000.0,-10.0, 0.0, 10.0 ); - Vector b1 = Vector_(2, 2.0, -1.0); - - EQUALITY(A_act1,A1); - EQUALITY(b_act1,b1); - - // Test unwhitened version - Matrix A_act2; Vector b_act2; - boost::tie(A_act2,b_act2) = lf->matrix(false); - - - Matrix A2 = Matrix_(2,4, - -1.0, 0.0, 1.0, 0.0, - 000.0,-1.0, 0.0, 1.0 ); - //Vector b2 = Vector_(2, 2.0, -1.0); - - EQUALITY(A_act2,A2); - EQUALITY(b_act2,b2); - - // Ensure that whitening is consistent - boost::shared_ptr model = lf->get_model(); - model->WhitenSystem(A_act2, b_act2); - EQUALITY(A_act1, A_act2); - EQUALITY(b_act1, b_act2); -} - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, matrix_aug ) -{ - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - // create a small linear factor graph - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - - // get the factor kf2 from the factor graph - //GaussianFactorOrdered::shared_ptr lf = fg[1]; - Vector b2 = Vector_(2, 0.2, -0.1); - Matrix I = eye(2); - // render with a given ordering - OrderingOrdered ord; - ord += kx1,kx2; - JacobianFactorOrdered::shared_ptr lf(new JacobianFactorOrdered(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); - - - // Test unwhitened version - Matrix Ab_act1; - Ab_act1 = lf->matrix_augmented(false); - - Matrix Ab1 = Matrix_(2,5, - -1.0, 0.0, 1.0, 0.0, 0.2, - 00.0,- 1.0, 0.0, 1.0, -0.1 ); - - EQUALITY(Ab_act1,Ab1); - - // Test whitened version - Matrix Ab_act2; - Ab_act2 = lf->matrix_augmented(true); - - Matrix Ab2 = Matrix_(2,5, - -10.0, 0.0, 10.0, 0.0, 2.0, - 00.0, -10.0, 0.0, 10.0, -1.0 ); - - EQUALITY(Ab_act2,Ab2); - - // Ensure that whitening is consistent - boost::shared_ptr model = lf->get_model(); - model->WhitenInPlace(Ab_act1); - EQUALITY(Ab_act1, Ab_act2); -} - -/* ************************************************************************* */ -// small aux. function to print out lists of anything -template -void print(const list& i) { - copy(i.begin(), i.end(), ostream_iterator (cout, ",")); - cout << endl; -} - -/* ************************************************************************* */ -TEST( GaussianFactorOrdered, size ) -{ - // create a linear factor graph - const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); - OrderingOrdered ordering; ordering += kx1,kx2,kl1; - GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering); - - // get some factors from the graph - boost::shared_ptr factor1 = fg[0]; - boost::shared_ptr factor2 = fg[1]; - boost::shared_ptr factor3 = fg[2]; - - EXPECT_LONGS_EQUAL(1, factor1->size()); - EXPECT_LONGS_EQUAL(2, factor2->size()); - EXPECT_LONGS_EQUAL(2, factor3->size()); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/tests/testSymbolicBayesNetBObsolete.cpp b/tests/testSymbolicBayesNetBObsolete.cpp deleted file mode 100644 index 152661a40..000000000 --- a/tests/testSymbolicBayesNetBObsolete.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicBayesNetB.cpp - * @brief Unit tests for a symbolic Bayes chain - * @author Frank Dellaert - */ - -#include // for 'insert()' -#include // for operator += -using namespace boost::assign; - -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -/* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, constructor ) -{ - OrderingOrdered o; o += X(2),L(1),X(1); - // Create manually - IndexConditionalOrdered::shared_ptr - x2(new IndexConditionalOrdered(o[X(2)],o[L(1)], o[X(1)])), - l1(new IndexConditionalOrdered(o[L(1)],o[X(1)])), - x1(new IndexConditionalOrdered(o[X(1)])); - BayesNetOrdered expected; - expected.push_back(x2); - expected.push_back(l1); - expected.push_back(x1); - - // Create from a factor graph - GaussianFactorGraphOrdered factorGraph = createGaussianFactorGraph(o); - SymbolicFactorGraphOrdered fg(factorGraph); - - // eliminate it - SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( SymbolicBayesNetOrdered, FromGaussian) { - SymbolicBayesNetOrdered expected; - expected.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(0, 1))); - expected.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(1))); - - GaussianBayesNetOrdered gbn = createSmallGaussianBayesNet(); - SymbolicBayesNetOrdered actual(gbn); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/tests/testSymbolicFactorGraphBObsolete.cpp b/tests/testSymbolicFactorGraphBObsolete.cpp deleted file mode 100644 index e4e82ab9a..000000000 --- a/tests/testSymbolicFactorGraphBObsolete.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testSymbolicFactorGraphB.cpp - * @brief Unit tests for a symbolic Factor Graph - * @author Frank Dellaert - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include // for operator += -using namespace boost::assign; - -using namespace std; -using namespace gtsam; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -/* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, symbolicFactorGraph ) -{ - OrderingOrdered o; o += X(1),L(1),X(2); - // construct expected symbolic graph - SymbolicFactorGraphOrdered expected; - expected.push_factor(o[X(1)]); - expected.push_factor(o[X(1)],o[X(2)]); - expected.push_factor(o[X(1)],o[L(1)]); - expected.push_factor(o[X(2)],o[L(1)]); - - // construct it from the factor graph - GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o); - SymbolicFactorGraphOrdered actual(factorGraph); - - CHECK(assert_equal(expected, actual)); -} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraphOrdered, findAndRemoveFactors ) -//{ -// // construct it from the factor graph graph -// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraphOrdered actual(factorGraph); -// SymbolicFactor::shared_ptr f1 = actual[0]; -// SymbolicFactor::shared_ptr f3 = actual[2]; -// actual.findAndRemoveFactors(X(2)); -// -// // construct expected graph after find_factors_and_remove -// SymbolicFactorGraphOrdered expected; -// SymbolicFactor::shared_ptr null; -// expected.push_back(f1); -// expected.push_back(null); -// expected.push_back(f3); -// expected.push_back(null); -// -// CHECK(assert_equal(expected, actual)); -//} -///* ************************************************************************* */ -//TEST( SymbolicFactorGraphOrdered, factors) -//{ -// // create a test graph -// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraphOrdered fg(factorGraph); -// -// // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(X(1)); -// int x1_indices[] = { 0, 1, 2 }; -// list x1_expected(x1_indices, x1_indices + 3); -// CHECK(x1_factors==x1_expected); -// -// // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(X(2)); -// int x2_indices[] = { 1, 3 }; -// list x2_expected(x2_indices, x2_indices + 2); -// CHECK(x2_factors==x2_expected); -//} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraphOrdered, removeAndCombineFactors ) -//{ -// // create a test graph -// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(); -// SymbolicFactorGraphOrdered fg(factorGraph); -// -// // combine all factors connected to x1 -// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); -// -// // check result -// SymbolicFactor expected(L(1),X(1),X(2)); -// CHECK(assert_equal(expected,*actual)); -//} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraphOrdered, eliminateOne ) -//{ -// OrderingOrdered o; o += X(1),L(1),X(2); -// // create a test graph -// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o); -// SymbolicFactorGraphOrdered fg(factorGraph); -// -// // eliminate -// IndexConditionalOrdered::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[X(1)]+1); -// -// // create expected symbolic IndexConditional -// IndexConditionalOrdered expected(o[X(1)],o[L(1)],o[X(2)]); -// -// CHECK(assert_equal(expected,*actual)); -//} - -/* ************************************************************************* */ -TEST( SymbolicFactorGraphOrdered, eliminate ) -{ - OrderingOrdered o; o += X(2),L(1),X(1); - - // create expected Chordal bayes Net - IndexConditionalOrdered::shared_ptr x2(new IndexConditionalOrdered(o[X(2)], o[L(1)], o[X(1)])); - IndexConditionalOrdered::shared_ptr l1(new IndexConditionalOrdered(o[L(1)], o[X(1)])); - IndexConditionalOrdered::shared_ptr x1(new IndexConditionalOrdered(o[X(1)])); - - SymbolicBayesNetOrdered expected; - expected.push_back(x2); - expected.push_back(l1); - expected.push_back(x1); - - // create a test graph - GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o); - SymbolicFactorGraphOrdered fg(factorGraph); - - // eliminate it - SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate(); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */