diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index cddd642f0..c003102e4 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -42,79 +42,6 @@ namespace gtsam { using namespace std; - /* ************************************************************************* */ - template - void BayesTreeClique::assertInvariants() const { -#ifndef NDEBUG - // We rely on the keys being sorted -// FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); -// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// assert(sortedUniqueKeys.size() == conditional_->size() && -// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin())); -#endif - } - - /* ************************************************************************* */ - template - BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTreeClique::print(const string& s) const { - conditional_->print(s); - } - - /* ************************************************************************* */ - template - size_t BayesTreeClique::treeSize() const { - size_t size = 1; - BOOST_FOREACH(const shared_ptr& child, children_) - size += child->treeSize(); - return size; - } - - /* ************************************************************************* */ - template - void BayesTreeClique::printTree(const string& indent) const { - print(indent); - BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent+" "); - } - - /* ************************************************************************* */ - template - void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { - conditional_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const shared_ptr& child, children_) { - child->permuteWithInverse(inversePermutation); - } - assertInvariants(); - } - - /* ************************************************************************* */ - template - bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); -#ifndef NDEBUG - if(!changed) { - BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } - BOOST_FOREACH(const shared_ptr& child, children_) { - assert(child->permuteSeparatorWithInverse(inversePermutation) == false); - } - } -#endif - if(changed) { - BOOST_FOREACH(const shared_ptr& child, children_) { - (void)child->permuteSeparatorWithInverse(inversePermutation); - } - } - assertInvariants(); - return changed; - } - /* ************************************************************************* */ template typename BayesTree::CliqueData @@ -208,162 +135,6 @@ namespace gtsam { return stats; } - /* ************************************************************************* */ - // 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 - BayesNet BayesTreeClique::shortcut(shared_ptr R, - Eliminate function) { - - static const bool debug = false; - - // A first base case is when this clique or its parent is the root, - // in which case we return an empty Bayes net. - - shared_ptr parent(parent_.lock()); - - if (R.get()==this || parent==R) { - BayesNet empty; - return empty; - } - - // The root conditional - FactorGraph p_R(BayesNet(R->conditional())); - - // The parent clique has a CONDITIONAL for each frontal node in Fp - // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); - - // If not the base case, obtain the parent shortcut P(Sp|R) as factors - FactorGraph p_Sp_R(parent->shortcut(R, function)); - - // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FactorGraph p_Cp_R; - p_Cp_R.push_back(p_R); - p_Cp_R.push_back(p_Fp_Sp); - p_Cp_R.push_back(p_Sp_R); - - // Eliminate into a Bayes net with ordering designed to integrate out - // any variables not in *our* separator. Variables to integrate out must be - // eliminated first hence the desired ordering is [Cp\S S]. - // However, an added wrinkle is that Cp might overlap with the root. - // Keys corresponding to the root should not be added to the ordering at all. - - if(debug) { - p_R.print("p_R: "); - p_Fp_Sp.print("p_Fp_Sp: "); - p_Sp_R.print("p_Sp_R: "); - } - - // We want to factor into a conditional of the clique variables given the - // root and the marginal on the root, integrating out all other variables. - // The integrands include any parents of this clique and the variables of - // the parent clique. - FastSet variablesAtBack; - FastSet separator; - size_t uniqueRootVariables = 0; - BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { - variablesAtBack.insert(separatorIndex); - separator.insert(separatorIndex); - if(debug) cout << "At back (this): " << separatorIndex << endl; - } - BOOST_FOREACH(const Index key, R->conditional()->keys()) { - if(variablesAtBack.insert(key).second) - ++ uniqueRootVariables; - if(debug) cout << "At back (root): " << key << endl; - } - - Permutation toBack = Permutation::PushToBack( - vector(variablesAtBack.begin(), variablesAtBack.end()), - R->conditional()->lastFrontalKey() + 1); - Permutation::shared_ptr toBackInverse(toBack.inverse()); - BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { - factor->permuteWithInverse(*toBackInverse); } - typename BayesNet::shared_ptr eliminated(EliminationTree< - FactorType>::Create(p_Cp_R)->eliminate(function)); - - // Take only the conditionals for p(S|R). We check for each variable being - // in the separator set because if some separator variables overlap with - // root variables, we cannot rely on the number of root variables, and also - // want to include those variables in the conditional. - BayesNet p_S_R; - BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) { - assert(conditional->nrFrontals() == 1); - if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { - if(debug) - conditional->print("Taking C|R conditional: "); - p_S_R.push_front(conditional); - } - if(p_S_R.size() == separator.size()) - break; - } - - // Undo the permutation - if(debug) toBack.print("toBack: "); - p_S_R.permuteWithInverse(toBack); - - // return the parent shortcut P(Sp|R) - assertInvariants(); - return p_S_R; - } - - /* ************************************************************************* */ - // P(C) = \int_R P(F|S) P(S|R) P(R) - // TODO: Maybe we should integrate given parent marginal P(Cp), - // \int(Cp\S) P(F|S)P(S|Cp)P(Cp) - // Because the root clique could be very big. - /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::marginal( - shared_ptr R, Eliminate function) { - // If we are the root, just return this root - // NOTE: immediately cast to a factor graph - BayesNet bn(R->conditional()); - if (R.get()==this) return bn; - - // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R, function); - p_FSR.push_front(this->conditional()); - p_FSR.push_back(R->conditional()); - - assertInvariants(); - GenericSequentialSolver solver(p_FSR); - return *solver.jointFactorGraph(conditional_->keys(), function); - } - - /* ************************************************************************* */ - // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) - /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::joint( - shared_ptr C2, shared_ptr R, Eliminate function) { - // For now, assume neither is the root - - // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) - FactorGraph joint; - if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1) - if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) - if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) - if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) - joint.push_back(R->conditional()->toFactor()); // P(R) - - // Find the keys of both C1 and C2 - vector keys1(conditional_->keys()); - vector keys2(C2->conditional_->keys()); - FastSet keys12; - keys12.insert(keys1.begin(), keys1.end()); - keys12.insert(keys2.begin(), keys2.end()); - - // Calculate the marginal - vector keys12vector; keys12vector.reserve(keys12.size()); - keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); - assertInvariants(); - GenericSequentialSolver solver(joint); - return *solver.jointFactorGraph(keys12vector, function); - } - /* ************************************************************************* */ template void BayesTree::Cliques::print(const std::string& s) const { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 45dea6e49..2f6b9735e 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -24,11 +24,13 @@ #include #include #include +#include #include #include #include #include +#include #include namespace gtsam { @@ -196,6 +198,8 @@ namespace gtsam { /** return root clique */ const sharedClique& root() const { return root_; } + + /** Access the root clique (non-const version) */ sharedClique& root() { return root_; } /** find the clique to which key belongs */ @@ -207,24 +211,20 @@ namespace gtsam { CliqueData getCliqueData() const; /** return marginal on any variable */ - typename FactorType::shared_ptr marginalFactor(Index key, - Eliminate function) const; + typename FactorType::shared_ptr marginalFactor(Index key, 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 BayesNet::shared_ptr marginalBayesNet(Index key, - Eliminate function) const; + typename BayesNet::shared_ptr marginalBayesNet(Index key, Eliminate function) const; /** return joint on two variables */ - typename FactorGraph::shared_ptr joint(Index key1, Index key2, - Eliminate function) const; + typename FactorGraph::shared_ptr joint(Index key1, Index key2, Eliminate function) const; /** return joint on two variables as a BayesNet */ - typename BayesNet::shared_ptr jointBayesNet(Index key1, - Index key2, Eliminate function) const; + typename BayesNet::shared_ptr jointBayesNet(Index key1, Index key2, Eliminate function) const; /** * Read only with side effects @@ -310,120 +310,24 @@ namespace gtsam { * extra data along with the clique. */ template - struct BayesTreeClique { - - }; - - - /* ************************************************************************* */ - /** - * 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. - */ - template - struct BayesTreeCliqueBase { - - protected: - void assertInvariants() const; - + struct BayesTreeClique : public BayesTreeCliqueBase > { public: - typedef BayesTreeClique This; - typedef DERIVED DerivedType; - typedef typename DERIVED::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; - typedef typename ConditionalType::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; - - sharedConditional conditional_; - weak_ptr parent_; - std::list children_; - - friend class BayesTree; - - /** Default constructor */ - BayesTreeCliqueBase() {} - - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional); - - virtual ~BayesTreeCliqueBase() {} - - /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ - static shared_ptr Create(const sharedConditional& conditional) { return shared_ptr(new BayesTreeClique(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 - */ - template - static shared_ptr Create(const RESULT& result) { return Create(result.first); } - - /** print this node */ - void print(const std::string& s = "") const; - - /** The arrow operator accesses the conditional */ - const ConditionalType* operator->() const { return conditional_.get(); } - - /** The arrow operator accesses the conditional */ - ConditionalType* operator->() { return conditional_.get(); } - - /** Access the conditional */ - const sharedConditional& conditional() const { return conditional_; } - - /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { return parent_.expired(); } - - /** return the const reference of children */ - std::list& children() { return children_; } - const std::list& children() const { return children_; } - - /** The size of subtree rooted at this clique, i.e., nr of Cliques */ - size_t treeSize() const; - - /** print this node and entire subtree below it */ - void printTree(const std::string& indent="") const; - - /** 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 permuteSeparatorWithInverse(const Permutation& inversePermutation); - - /** return the conditional P(S|Root) on the separator given the root */ - // TODO: create a cached version - BayesNet shortcut(shared_ptr root, Eliminate function); - - /** return the marginal P(C) of the clique */ - FactorGraph marginal(shared_ptr root, Eliminate function); - - /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); - - bool equals(const This& other, double tol=1e-9) const { - return (!conditional_ && !other.conditional()) || - conditional_->equals(*(other.conditional()), tol); - } + typedef CONDITIONAL ConditionalType; + typedef BayesTreeClique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + BayesTreeClique() {} + BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {} private: /** 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_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } + }; - }; // \struct Clique - - +#include } /// namespace gtsam diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h new file mode 100644 index 000000000..2956a6cc2 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -0,0 +1,251 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesTreeClique::assertInvariants() const { +#ifndef NDEBUG + // We rely on the keys being sorted +// FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); +// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); +// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); +// assert(sortedUniqueKeys.size() == conditional_->size() && +// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin())); +#endif + } + + /* ************************************************************************* */ + template + BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { + assertInvariants(); + } + + /* ************************************************************************* */ + template + void BayesTreeClique::print(const string& s) const { + conditional_->print(s); + } + + /* ************************************************************************* */ + template + size_t BayesTreeClique::treeSize() const { + size_t size = 1; + BOOST_FOREACH(const shared_ptr& child, children_) + size += child->treeSize(); + return size; + } + + /* ************************************************************************* */ + template + void BayesTreeClique::printTree(const string& indent) const { + print(indent); + BOOST_FOREACH(const shared_ptr& child, children_) + child->printTree(indent+" "); + } + + /* ************************************************************************* */ + template + void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { + conditional_->permuteWithInverse(inversePermutation); + BOOST_FOREACH(const shared_ptr& child, children_) { + child->permuteWithInverse(inversePermutation); + } + assertInvariants(); + } + + /* ************************************************************************* */ + template + bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); +#ifndef NDEBUG + if(!changed) { + BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } + BOOST_FOREACH(const shared_ptr& child, children_) { + assert(child->permuteSeparatorWithInverse(inversePermutation) == false); + } + } +#endif + if(changed) { + BOOST_FOREACH(const shared_ptr& child, children_) { + (void)child->permuteSeparatorWithInverse(inversePermutation); + } + } + 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 + BayesNet BayesTreeClique::shortcut(shared_ptr R, + Eliminate function) { + + static const bool debug = false; + + // A first base case is when this clique or its parent is the root, + // in which case we return an empty Bayes net. + + shared_ptr parent(parent_.lock()); + + if (R.get()==this || parent==R) { + BayesNet empty; + return empty; + } + + // The root conditional + FactorGraph p_R(BayesNet(R->conditional())); + + // The parent clique has a CONDITIONAL for each frontal node in Fp + // so we can obtain P(Fp|Sp) in factor graph form + FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); + + // If not the base case, obtain the parent shortcut P(Sp|R) as factors + FactorGraph p_Sp_R(parent->shortcut(R, function)); + + // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) + FactorGraph p_Cp_R; + p_Cp_R.push_back(p_R); + p_Cp_R.push_back(p_Fp_Sp); + p_Cp_R.push_back(p_Sp_R); + + // Eliminate into a Bayes net with ordering designed to integrate out + // any variables not in *our* separator. Variables to integrate out must be + // eliminated first hence the desired ordering is [Cp\S S]. + // However, an added wrinkle is that Cp might overlap with the root. + // Keys corresponding to the root should not be added to the ordering at all. + + if(debug) { + p_R.print("p_R: "); + p_Fp_Sp.print("p_Fp_Sp: "); + p_Sp_R.print("p_Sp_R: "); + } + + // We want to factor into a conditional of the clique variables given the + // root and the marginal on the root, integrating out all other variables. + // The integrands include any parents of this clique and the variables of + // the parent clique. + FastSet variablesAtBack; + FastSet separator; + size_t uniqueRootVariables = 0; + BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { + variablesAtBack.insert(separatorIndex); + separator.insert(separatorIndex); + if(debug) cout << "At back (this): " << separatorIndex << endl; + } + BOOST_FOREACH(const Index key, R->conditional()->keys()) { + if(variablesAtBack.insert(key).second) + ++ uniqueRootVariables; + if(debug) cout << "At back (root): " << key << endl; + } + + Permutation toBack = Permutation::PushToBack( + vector(variablesAtBack.begin(), variablesAtBack.end()), + R->conditional()->lastFrontalKey() + 1); + Permutation::shared_ptr toBackInverse(toBack.inverse()); + BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { + factor->permuteWithInverse(*toBackInverse); } + typename BayesNet::shared_ptr eliminated(EliminationTree< + FactorType>::Create(p_Cp_R)->eliminate(function)); + + // Take only the conditionals for p(S|R). We check for each variable being + // in the separator set because if some separator variables overlap with + // root variables, we cannot rely on the number of root variables, and also + // want to include those variables in the conditional. + BayesNet p_S_R; + BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) { + assert(conditional->nrFrontals() == 1); + if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { + if(debug) + conditional->print("Taking C|R conditional: "); + p_S_R.push_front(conditional); + } + if(p_S_R.size() == separator.size()) + break; + } + + // Undo the permutation + if(debug) toBack.print("toBack: "); + p_S_R.permuteWithInverse(toBack); + + // return the parent shortcut P(Sp|R) + assertInvariants(); + return p_S_R; + } + + /* ************************************************************************* */ + // P(C) = \int_R P(F|S) P(S|R) P(R) + // TODO: Maybe we should integrate given parent marginal P(Cp), + // \int(Cp\S) P(F|S)P(S|Cp)P(Cp) + // Because the root clique could be very big. + /* ************************************************************************* */ + template + FactorGraph BayesTreeClique::marginal( + shared_ptr R, Eliminate function) { + // If we are the root, just return this root + // NOTE: immediately cast to a factor graph + BayesNet bn(R->conditional()); + if (R.get()==this) return bn; + + // Combine P(F|S), P(S|R), and P(R) + BayesNet p_FSR = this->shortcut(R, function); + p_FSR.push_front(this->conditional()); + p_FSR.push_back(R->conditional()); + + assertInvariants(); + GenericSequentialSolver solver(p_FSR); + return *solver.jointFactorGraph(conditional_->keys(), function); + } + + /* ************************************************************************* */ + // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) + /* ************************************************************************* */ + template + FactorGraph BayesTreeClique::joint( + shared_ptr C2, shared_ptr R, Eliminate function) { + // For now, assume neither is the root + + // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) + FactorGraph joint; + if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1) + if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) + if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) + if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) + joint.push_back(R->conditional()->toFactor()); // P(R) + + // Find the keys of both C1 and C2 + vector keys1(conditional_->keys()); + vector keys2(C2->conditional_->keys()); + FastSet keys12; + keys12.insert(keys1.begin(), keys1.end()); + keys12.insert(keys2.begin(), keys2.end()); + + // Calculate the marginal + vector keys12vector; keys12vector.reserve(keys12.size()); + keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); + assertInvariants(); + GenericSequentialSolver solver(joint); + return *solver.jointFactorGraph(keys12vector, function); + } + +} diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h new file mode 100644 index 000000000..549814312 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -0,0 +1,153 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include + +#include +#include +#include + +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 The derived clique type. + */ + template + struct BayesTreeCliqueBase { + + protected: + void assertInvariants() const; + + /** Default constructor */ + BayesTreeCliqueBase() {} + + /** Construct from a conditional, leaving parent and child pointers uninitialized */ + BayesTreeCliqueBase(const sharedConditional& conditional); + + public: + typedef BayesTreeClique This; + typedef DERIVED DerivedType; + typedef typename DERIVED::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::weak_ptr weak_ptr; + typedef typename boost::shared_ptr derived_ptr; + typedef typename boost::weak_ptr derived_weak_ptr; + typedef typename ConditionalType::FactorType FactorType; + typedef typename FactorGraph::Eliminate Eliminate; + + sharedConditional conditional_; + derived_weak_ptr parent_; + std::list 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 An elimination result, which is a pair + */ + static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } + + /** print this node */ + void print(const std::string& s = "") const; + + /** The arrow operator accesses the conditional */ + const ConditionalType* operator->() const { return conditional_.get(); } + + /** The arrow operator accesses the conditional */ + ConditionalType* operator->() { return conditional_.get(); } + + /** Access the conditional */ + const sharedConditional& conditional() const { return conditional_; } + + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_.expired(); } + + /** return the const reference of children */ + std::list& children() { return children_; } + const std::list& children() const { return children_; } + + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ + size_t treeSize() const; + + /** print this node and entire subtree below it */ + void printTree(const std::string& indent="") const; + + /** 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 permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** return the conditional P(S|Root) on the separator given the root */ + // TODO: create a cached version + BayesNet shortcut(shared_ptr root, Eliminate function); + + /** return the marginal P(C) of the clique */ + FactorGraph marginal(shared_ptr root, Eliminate function); + + /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ + FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); + + bool equals(const This& other, double tol=1e-9) const { + return (!conditional_ && !other.conditional()) || + conditional_->equals(*(other.conditional()), tol); + } + + private: + /** 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 + typename BayesTreeCliqueBase::derived_ptr asDerived(const BayesTreeCliqueBase& base) { +#ifndef NDEBUG + return boost::dynamic_pointer_cast(base); +#else + return boost::static_pointer_cast(base); +#endif + } + +#include + +} diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 54f96efe2..2bbfc2f22 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -52,8 +52,8 @@ public: typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ - typedef Matrix AbMatrix; - typedef VerticalBlockView rsd_type; + typedef Matrix RdMatrix; + typedef VerticalBlockView rsd_type; typedef rsd_type::Block r_type; typedef rsd_type::constBlock const_r_type; @@ -72,7 +72,7 @@ protected: * Use R*permutation_ to get back the correct non-permuted order, * for example when converting to the Jacobian * */ - AbMatrix matrix_; + RdMatrix matrix_; rsd_type rsd_; /** vector of standard deviations */ @@ -169,7 +169,7 @@ public: protected: - const AbMatrix& matrix() const { return matrix_; } + const RdMatrix& matrix() const { return matrix_; } const rsd_type& rsd() const { return rsd_; } public: diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 32ef1b0ac..9316e0839 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -9,28 +9,28 @@ namespace gtsam { /* ************************************************************************* */ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( - double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) { + double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) { // Get magnitude of each update and find out which segment Delta falls in assert(Delta >= 0.0); double DeltaSq = Delta*Delta; - double x_u_norm_sq = x_u.vector().squaredNorm(); - double x_n_norm_sq = x_n.vector().squaredNorm(); + double x_u_norm_sq = dx_u.vector().squaredNorm(); + double x_n_norm_sq = dx_n.vector().squaredNorm(); if(verbose) cout << "Steepest descent magnitude " << sqrt(x_u_norm_sq) << ", Newton's method magnitude " << sqrt(x_n_norm_sq) << endl; if(DeltaSq < x_u_norm_sq) { // Trust region is smaller than steepest descent update - VectorValues x_d = VectorValues::SameStructure(x_u); - x_d.vector() = x_u.vector() * sqrt(DeltaSq / x_u_norm_sq); + VectorValues x_d = VectorValues::SameStructure(dx_u); + x_d.vector() = dx_u.vector() * sqrt(DeltaSq / x_u_norm_sq); if(verbose) cout << "In steepest descent region with fraction " << sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; return x_d; } else if(DeltaSq < x_n_norm_sq) { // Trust region boundary is between steepest descent point and Newton's method point - return ComputeBlend(Delta, x_u, x_n); + return ComputeBlend(Delta, dx_u, dx_n); } else { assert(DeltaSq >= x_n_norm_sq); if(verbose) cout << "In pure Newton's method region" << endl; // Trust region is larger than Newton's method point - return x_n; + return dx_n; } } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index bfb1c1878..5f5801918 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -95,10 +95,11 @@ struct DoglegOptimizerImpl { * GaussianBayesNet, containing GaussianConditional s. * * @param Delta The trust region radius - * @param bayesNet The Bayes' net \f$ (R,d) \f$ as described above. + * @param dx_u The steepest descent point, i.e. the Cauchy point + * @param dx_n The Gauss-Newton point * @return The dogleg point \f$ \delta x_d \f$ */ - static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false); + static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of * the function diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5e84d44d8..3e3d34996 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -106,71 +106,32 @@ struct ISAM2Result { }; template -struct ISAM2Clique : public BayesTreeClique { +struct ISAM2Clique : public BayesTreeCliqueBase > { typedef ISAM2Clique This; - typedef BayesTreeClique Base; - + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typename Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; /** Access the cached factor */ typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - /** Construct from an elimination result, caches the eliminated factor */ - template - ISAM2Clique(const RESULT& result) : Base(result), cachedFactor_(result.second) {} + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } /** Construct from a conditional */ - ISAM2Clique(const typename Base::sharedConditional& conditional) : Base(conditional) {} + ISAM2Clique(const sharedConditional& conditional) : Base(conditional) { + throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); } - /** Create from an elimination result, overrides BayesTreeClique::Create(const RESULT&) to cache the eliminated factor */ - template - static shared_ptr Create(const RESULT& result) { return shared_ptr(new This(result)); } - - static shared_ptr Create(const typename Base::sharedConditional& conditional) { return shared_ptr(new This(conditional)); } - - void permuteWithInverse(const Permutation& inversePermutation) { - Base::conditional_->permuteWithInverse(inversePermutation); - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child; -#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - _child = boost::dynamic_pointer_cast(child); -#else - _child = boost::static_pointer_cast(child); -#endif - _child->permuteWithInverse(inversePermutation); - } - Base::assertInvariants(); - } - - bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = Base::conditional_->permuteSeparatorWithInverse(inversePermutation); -#ifndef NDEBUG - if(!changed) { - BOOST_FOREACH(Index& separatorKey, Base::conditional_->parents()) { - assert(separatorKey == inversePermutation[separatorKey]); } - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child = boost::dynamic_pointer_cast(child); // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - assert(_child->permuteSeparatorWithInverse(inversePermutation) == false); } - } -#endif - if(changed) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child; - #ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - _child = boost::dynamic_pointer_cast(child); - #else - _child = boost::static_pointer_cast(child); - #endif - (void)_child->permuteSeparatorWithInverse(inversePermutation); - } - } - Base::assertInvariants(); - return changed; + /** Construct from an elimination result */ + ISAM2Clique(const std::pair >& result) : + Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { + // Compute gradient contribution + const ConditionalType& conditional(*result.first); + gradient << -(conditional.get_R() * conditional.permutation().transpose()).transpose * conditional.get_d(), + -conditional.get_S() * conditional.get_d(); } private: @@ -180,6 +141,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(cachedFactor_); + ar & BOOST_SERIALIZATION_NVP(gradientContribution_); } };