diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 64f37c174..f75c658a2 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -139,6 +139,14 @@ namespace gtsam { DiscreteFactor::permuteWithInverse(inversePermutation); Potentials::permuteWithInverse(inversePermutation); } + + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + DiscreteFactor::reduceWithInverse(inverseReduction); + Potentials::reduceWithInverse(inverseReduction); + } /// @} }; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 9b1130bae..2f411997c 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -108,6 +108,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + IndexFactor::reduceWithInverse(inverseReduction); + } + /// @} }; // DiscreteFactor diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index f3c89721b..467611c5d 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -74,6 +74,24 @@ namespace gtsam { if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); } } + + /* ************************************************************************* */ + void DiscreteFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void DiscreteFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } /* ************************************************************************* */ std::pair // diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 0f399abdd..323adca7b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -79,6 +79,12 @@ public: /// print void print(const std::string& s = "DiscreteFactorGraph", const IndexFormatter& formatter =DefaultIndexFormatter) const; + + /** Permute the variables in the factors */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); }; // DiscreteFactorGraph diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index 2a2260d97..cf903a3a2 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -60,28 +60,39 @@ namespace gtsam { ADT::print(" "); } + /* ************************************************************************* */ + template + void Potentials::remapIndices(const P& remapping) { + // Permute the _cardinalities (TODO: Inefficient Consider Improving) + DiscreteKeys keys; + map ordering; + + // Get the original keys from cardinalities_ + BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + keys & key; + + // Perform Permutation + BOOST_FOREACH(DiscreteKey& key, keys) { + ordering[key.first] = remapping[key.first]; + key.first = ordering[key.first]; + } + + // Change *this + AlgebraicDecisionTree permuted((*this), ordering); + *this = permuted; + cardinalities_ = keys.cardinalities(); + } + /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& permutation) { - // Permute the _cardinalities (TODO: Inefficient Consider Improving) - DiscreteKeys keys; - map ordering; - - // Get the orginal keys from cardinalities_ - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - keys & key; - - // Perform Permutation - BOOST_FOREACH(DiscreteKey& key, keys) { - ordering[key.first] = permutation[key.first]; - key.first = ordering[key.first]; - } - - // Change *this - AlgebraicDecisionTree permuted((*this), ordering); - *this = permuted; - cardinalities_ = keys.cardinalities(); + void Potentials::permuteWithInverse(const Permutation& inversePermutation) { + remapIndices(inversePermutation); } + /* ************************************************************************* */ + void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { + remapIndices(inverseReduction); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 3ca222b5f..5d8ace371 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -49,6 +49,10 @@ namespace gtsam { // Safe division for probabilities static double safe_div(const double& a, const double& b); + // Apply either a permutation or a reduction + template + void remapIndices(const P& remapping); + public: /** Default constructor for I/O */ @@ -79,6 +83,11 @@ namespace gtsam { */ virtual void permuteWithInverse(const Permutation& inversePermutation); + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction); + }; // Potentials } // namespace gtsam diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index bf3b7f37c..fd37c4a4c 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -498,7 +498,11 @@ namespace gtsam { sharedClique clique = (*this)[j]; // calculate or retrieve its marginal P(C) = P(F,S) +#ifdef MARGINAL2 FactorGraph cliqueMarginal = clique->marginal2(root_,function); +#else + FactorGraph cliqueMarginal = clique->marginal(root_,function); +#endif // now, marginalize out everything that is not variable j GenericSequentialSolver solver(cliqueMarginal); diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 639e27202..6f090b587 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -17,6 +17,7 @@ #pragma once #include +#include namespace gtsam { @@ -53,8 +54,10 @@ namespace gtsam { 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; @@ -190,18 +193,42 @@ namespace gtsam { // systems and exceptions are thrown. However, we should be able to omit // this if we can get ATTEMPT_AT_NOT_ELIMINATING_ALL in // GenericSequentialSolver.* working... +#ifndef ATTEMPT_AT_NOT_ELIMINATING_ALL p_Cp_B.push_back(B->conditional()->toFactor()); // P(B) +#endif + + // Determine the variables we want to keepSet, S union B + std::vector keep = shortcut_indices(B, p_Cp_B); + //std::set keepSet; + //BOOST_FOREACH(Index j, this->conditional()->parents()) { + // keepSet.insert(j); } + //BOOST_FOREACH(Index j, (*B->conditional())) { + // keepSet.insert(j); } + //std::vector keep(keepSet.begin(), keepSet.end()); + + // Reduce the variable indices to start at zero + 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); // Create solver that will marginalize for us GenericSequentialSolver solver(p_Cp_B); - // Determine the variables we want to keep - std::vector keep = shortcut_indices(B, p_Cp_B); - // Finally, we only want to have S\B variables in the Bayes net, so size_t nrFrontals = S_setminus_B.size(); cachedShortcut_ = // *solver.conditionalBayesNet(keep, nrFrontals, function); + + // Undo the reduction + BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { + if (factor) { + factor->permuteWithInverse(reduction); + } + } + cachedShortcut_->permuteWithInverse(reduction); + assertInvariants(); } else { BayesNet empty; @@ -264,7 +291,7 @@ namespace gtsam { // Create solver that will marginalize for us GenericSequentialSolver solver(p_Cp); - // The variables we want to keep are exactly the ones in S + // 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()); diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 2e977aa4b..f2c16ac14 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -35,34 +35,42 @@ namespace gtsam { /* ************************************************************************* */ template typename EliminationTree::sharedFactor EliminationTree::eliminate_( - Eliminate function, Conditionals& conditionals) const { + Eliminate function, Conditionals& conditionals) const { - static const bool debug = false; + static const bool debug = false; - if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; + if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl; - // Create the list of factors to be eliminated, initially empty, and reserve space - FactorGraph factors; - factors.reserve(this->factors_.size() + this->subTrees_.size()); + 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 + FactorGraph 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()); + // 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 + // 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 FactorGraph::EliminationResult - eliminated(function(factors, 1)); - conditionals[this->key_] = eliminated.first; + // Combine all factors (from this node and from subtrees) into a joint factor + typename FactorGraph::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: "); + 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; + 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 + } } /* ************************************************************************* */ @@ -138,7 +146,8 @@ typename EliminationTree::shared_ptr EliminationTree::Create( if(derivedFactor) { sharedFactor factor(derivedFactor); Index j = *std::min_element(factor->begin(), factor->end()); - trees[j]->add(factor); + if(j < structure.size()) + trees[j]->add(factor); } } toc(3, "hang factors"); @@ -193,12 +202,13 @@ bool EliminationTree::equals(const EliminationTree& ex /* ************************************************************************* */ template typename EliminationTree::BayesNet::shared_ptr -EliminationTree::eliminate(Eliminate function) const { + EliminationTree::eliminatePartial(typename EliminationTree::Eliminate function, size_t nrToEliminate) const { // call recursive routine tic(1, "ET recursive eliminate"); - size_t nrConditionals = this->key_ + 1; // root key has highest index - Conditionals conditionals(nrConditionals); // reserve a vector of conditional shared pointers + 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 toc(1, "ET recursive eliminate"); @@ -214,4 +224,12 @@ EliminationTree::eliminate(Eliminate function) const { return bayesNet; } +/* ************************************************************************* */ +template +typename EliminationTree::BayesNet::shared_ptr +EliminationTree::eliminate(Eliminate function) const { + size_t nrConditionals = this->key_ + 1; // root key has highest index + return eliminatePartial(function, nrConditionals); +} + } diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index f2b0c1eaf..62540c1fe 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -110,6 +110,13 @@ public: */ 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 /// @{ diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 7800f4adb..171b2f078 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,11 +89,7 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - , boost::optional nrToEliminate -#endif - ) const { + Eliminate function, boost::optional nrToEliminate) const { // Create inverse permutation Permutation::shared_ptr permutationInverse(permutation.inverse()); @@ -106,15 +102,12 @@ namespace gtsam { factor->permuteWithInverse(*permutationInverse); // Eliminate using elimination tree provided - typename EliminationTree::shared_ptr etree; -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - if (nrToEliminate) { - VariableIndex structure(*factors_, *nrToEliminate); - etree = EliminationTree::Create(*factors_, structure); - } else -#endif - etree = EliminationTree::Create(*factors_); - sharedBayesNet bayesNet = etree->eliminate(function); + typename EliminationTree::shared_ptr etree = EliminationTree::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_) @@ -147,13 +140,13 @@ namespace gtsam { // my trick below (passing nrToEliminate to eliminate) sometimes leads // to a disconnected graph. // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = factors_->keys().size();// TODO expensive! + 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 (int i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); + bayesNet->pop_front(); #else // Eliminate all variables sharedBayesNet fullBayesNet = eliminate(*permutation, function); diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 6f0bf3b5b..fffcb209f 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -88,13 +88,9 @@ namespace gtsam { /** * Eliminate in a different order, given a permutation */ - sharedBayesNet - eliminate(const Permutation& permutation, Eliminate function -#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL - , boost::optional nrToEliminate = boost::none - // If given a number of variables to eliminate, will only eliminate that many -#endif - ) const; + 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: diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index b04338d5b..306defce4 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -61,5 +61,13 @@ namespace gtsam { key = inversePermutation[key]; assertInvariants(); } + /* ************************************************************************* */ + void IndexFactor::reduceWithInverse(const internal::Reduction& inverseReduction) { + BOOST_FOREACH(Index& key, keys()) + key = inverseReduction[key]; + assertInvariants(); + } + + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 8ab5a880a..7d0fdcc4f 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -156,6 +156,11 @@ namespace gtsam { */ void permuteWithInverse(const Permutation& inversePermutation); + /** + * Apply a reduction, which is a remapping of variable indices. + */ + void reduceWithInverse(const internal::Reduction& inverseReduction); + virtual ~IndexFactor() { } diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index 5bcb1fef8..f94b4e116 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -152,4 +152,64 @@ void Permutation::print(const std::string& str) const { std::cout << std::endl; } +namespace internal { +/* ************************************************************************* */ + Reduction Reduction::CreateAsInverse(const Permutation& p) { + Reduction result; + for(Index j = 0; j < p.size(); ++j) + result.insert(std::make_pair(p[j], j)); + return result; + } + + /* ************************************************************************* */ + void Reduction::applyInverse(std::vector& 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; + } + + /* ************************************************************************* */ + Index& Reduction::operator[](const Index& j) { + iterator it = this->find(j); + if(it == this->end()) + throw std::out_of_range("Index to Reduction::operator[] not present"); + else + return it->second; + } + + /* ************************************************************************* */ + const Index& Reduction::operator[](const Index& j) const { + const_iterator it = this->find(j); + if(it == this->end()) + throw std::out_of_range("Index to Reduction::operator[] not present"); + else + return it->second; + } + + /* ************************************************************************* */ + Permutation createReducingPermutation(const std::set& indices) { + Permutation p(indices.size()); + Index newJ = 0; + BOOST_FOREACH(Index j, indices) { + p[newJ] = j; + ++ newJ; + } + return p; + } +} + } diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 4e32b1f52..deec7367c 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -19,10 +19,12 @@ #include #include +#include #include #include #include +#include namespace gtsam { @@ -182,4 +184,20 @@ protected: /// @} }; + +namespace internal { + // An internal class used for storing and applying a permutation from a map + class Reduction : public gtsam::FastMap { + public: + static Reduction CreateAsInverse(const Permutation& p); + void applyInverse(std::vector& js) const; + Permutation inverse() const; + Index& operator[](const Index& j); + const Index& operator[](const Index& j) const; + }; + + // Reduce the variable indices so that those in the set are mapped to start at zero + Permutation createReducingPermutation(const std::set& indices); +} + } diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index ff7a91ca6..70da5a4c1 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -69,6 +69,24 @@ namespace gtsam { { return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); } + + /* ************************************************************************* */ + void SymbolicFactorGraph::permuteWithInverse( + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->permuteWithInverse(inversePermutation); + } + } + + /* ************************************************************************* */ + void SymbolicFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } /* ************************************************************************* */ IndexFactor::shared_ptr CombineSymbolic( diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h index 1b20ac239..d18c3ba6f 100644 --- a/gtsam/inference/SymbolicFactorGraph.h +++ b/gtsam/inference/SymbolicFactorGraph.h @@ -77,8 +77,6 @@ namespace gtsam { */ FastSet keys() const; - - /// @} /// @name Advanced Interface /// @{ @@ -94,6 +92,12 @@ namespace gtsam { /** Push back 4-way factor */ void push_factor(Index key1, Index key2, Index key3, Index key4); + + /** Permute the variables in the factors */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); }; diff --git a/gtsam/inference/tests/testSymbolicBayesTree.cpp b/gtsam/inference/tests/testSymbolicBayesTree.cpp index b69bdac5e..68693817b 100644 --- a/gtsam/inference/tests/testSymbolicBayesTree.cpp +++ b/gtsam/inference/tests/testSymbolicBayesTree.cpp @@ -30,9 +30,6 @@ using namespace gtsam; static bool debug = false; -typedef BayesNet SymbolicBayesNet; -typedef BayesTree SymbolicBayesTree; - /* ************************************************************************* */ TEST_UNSAFE( SymbolicBayesTree, thinTree ) { diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp index ef6f415e1..c017083a2 100644 --- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp +++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp @@ -122,27 +122,6 @@ TEST( SymbolicSequentialSolver, inference ) { } } -/* ************************************************************************* */ -// This test shows a problem with my (Frank) attempt at a faster conditionalBayesNet -TEST( SymbolicSequentialSolver, problematicConditional ) { - // Create factor graph - SymbolicFactorGraph fg; - fg.push_factor(9, 12, 14); - - // eliminate - SymbolicSequentialSolver solver(fg); - // conditionalBayesNet - vector js; - js.push_back(12); - js.push_back(14); - size_t nrFrontals = 1; - SymbolicBayesNet::shared_ptr actualBN = // - solver.conditionalBayesNet(js, nrFrontals); - SymbolicBayesNet expectedBN; - expectedBN.push_front(boost::make_shared(12,14)); - EXPECT( assert_equal(expectedBN,*actualBN)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 72bd1df72..0ad35c2e3 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -25,7 +25,6 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactor::GaussianFactor(const GaussianConditional& c) : - IndexFactor(c) { - } + IndexFactor(c) {} } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 0d435ce17..efe6f2036 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -115,6 +115,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Apply a reduction, which is a remapping of variable indices. + */ + virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { + IndexFactor::reduceWithInverse(inverseReduction); + } + /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 222f025a0..5ba150601 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -63,6 +63,15 @@ namespace gtsam { } } + /* ************************************************************************* */ + void GaussianFactorGraph::reduceWithInverse( + const internal::Reduction& inverseReduction) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + if(factor) + factor->reduceWithInverse(inverseReduction); + } + } + /* ************************************************************************* */ void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { for (const_iterator factor = lfg.factors_.begin(); factor diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5e6e792be..811623947 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -127,6 +127,9 @@ namespace gtsam { /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); + /** Apply a reduction, which is a remapping of variable indices. */ + void reduceWithInverse(const internal::Reduction& inverseReduction); + /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.;