diff --git a/inference/Conditional.h b/inference/Conditional.h index cdb71f329..e8eb024d8 100644 --- a/inference/Conditional.h +++ b/inference/Conditional.h @@ -38,7 +38,7 @@ protected: Factor factor_; /** The first nFrontal variables are frontal and the rest are parents. */ - size_t nFrontal_; + size_t nrFrontals_; ValueWithDefault permuted_; @@ -56,36 +56,79 @@ public: Conditional(){} /** No parents */ - Conditional(Index key) : factor_(key), nFrontal_(1) {} + Conditional(Index key) : factor_(key), nrFrontals_(1) {} /** Single parent */ - Conditional(Index key, Index parent) : factor_(key, parent), nFrontal_(1) {} + Conditional(Index key, Index parent) : factor_(key, parent), nrFrontals_(1) {} /** Two parents */ - Conditional(Index key, Index parent1, Index parent2) : factor_(key, parent1, parent2), nFrontal_(1) {} + Conditional(Index key, Index parent1, Index parent2) : factor_(key, parent1, parent2), nrFrontals_(1) {} /** Three parents */ - Conditional(Index key, Index parent1, Index parent2, Index parent3) : factor_(key, parent1, parent2, parent3), nFrontal_(1) {} + Conditional(Index key, Index parent1, Index parent2, Index parent3) : factor_(key, parent1, parent2, parent3), nrFrontals_(1) {} /** Constructor from a frontal variable and a vector of parents */ - Conditional(Index key, const std::vector& parents) : nFrontal_(1) { - factor_.keys_.resize(1+parents.size()); - *(beginFrontals()) = key; - std::copy(parents.begin(), parents.end(), beginParents()); } + Conditional(Index key, const std::vector& parents) : nrFrontals_(1) { + factor_.keys_.resize(1 + parents.size()); + *(beginFrontals()) = key; + std::copy(parents.begin(), parents.end(), beginParents()); + } - /** Constructor from any number of frontal variables and parents */ + /** Named constructor from any number of frontal variables and parents */ template - Conditional(Iterator firstKey, Iterator lastKey, size_t nFrontals) : factor_(firstKey, lastKey), nFrontal_(nFrontals) {} + static shared_ptr fromRange(Iterator firstKey, Iterator lastKey, size_t nrFrontals) { + shared_ptr conditional(new Conditional); + conditional->nrFrontals_ = nrFrontals; + std::copy(firstKey, lastKey, back_inserter(conditional->factor_.keys_)); + return conditional; + } + + /** check equality */ + bool equals(const Conditional& c, double tol = 1e-9) const { + return nrFrontals_ == c.nrFrontals_ && factor_.equals(c.factor_, tol); } + + /** return the number of frontals */ + size_t nrFrontals() const { return nrFrontals_; } + + /** return the number of parents */ + size_t nrParents() const { return factor_.keys_.size() - nrFrontals_; } /** Special accessor when there is only one frontal variable. */ - Index key() const { assert(nFrontal_==1); return factor_.keys_[0]; } + Index key() const { assert(nrFrontals_==1); return factor_.keys_[0]; } - /** - * Permutes the Conditional, but for efficiency requires the permutation - * to already be inverted. - */ - void permuteWithInverse(const Permutation& inversePermutation) { - factor_.permuteWithInverse(inversePermutation); } + /** return a const reference to all keys */ + const std::vector& keys() const { return factor_.keys(); } + + /** Iterators over frontal and parent variables. */ + const_iterator beginFrontals() const { return factor_.keys_.begin(); } + const_iterator endFrontals() const { return factor_.keys_.begin()+nrFrontals_; } + const_iterator beginParents() const { return factor_.keys_.begin()+nrFrontals_; } + const_iterator endParents() const { return factor_.keys_.end(); } + + /** Mutable iterators and accessors */ + iterator beginFrontals() { return factor_.keys_.begin(); } + iterator endFrontals() { return factor_.keys_.begin()+nrFrontals_; } + iterator beginParents() { return factor_.keys_.begin()+nrFrontals_; } + iterator endParents() { return factor_.keys_.end(); } + boost::iterator_range frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } + boost::iterator_range parents() { return boost::make_iterator_range(beginParents(), endParents()); } + + /** 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()); } + + /** print */ + void print(const std::string& s = "Conditional") const { + std::cout << s << " P("; + BOOST_FOREACH(Index key, frontals()) std::cout << " " << key; + if (nrParents()>0) std::cout << " |"; + BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent; + std::cout << ")" << std::endl; + } /** Permute the variables when only separator variables need to be permuted. * Returns true if any reordered variables appeared in the separator and @@ -106,49 +149,12 @@ public: return parentChanged; } - /** return a const reference to all keys */ - const std::vector& keys() const { return factor_.keys(); } - - /** 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()); } - - /** return the number of frontals */ - size_t nrFrontals() const { return nFrontal_; } - - /** return the number of parents */ - size_t nrParents() const { return factor_.keys_.size() - nFrontal_; } - - /** print */ - void print(const std::string& s = "Conditional") const { - std::cout << s << " P("; - BOOST_FOREACH(Index key, frontals()) std::cout << " " << key; - if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent; - std::cout << ")" << std::endl; - } - - /** check equality */ - bool equals(const Conditional& c, double tol = 1e-9) const { - return nFrontal_ == c.nFrontal_ && factor_.equals(c.factor_, tol); } - - /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { return factor_.keys_.begin(); } - const_iterator endFrontals() const { return factor_.keys_.begin()+nFrontal_; } - const_iterator beginParents() const { return factor_.keys_.begin()+nFrontal_; } - const_iterator endParents() const { return factor_.keys_.end(); } - - /** Mutable iterators and accessors */ - iterator beginFrontals() { return factor_.keys_.begin(); } - iterator endFrontals() { return factor_.keys_.begin()+nFrontal_; } - iterator beginParents() { return factor_.keys_.begin()+nFrontal_; } - iterator endParents() { return factor_.keys_.end(); } - boost::iterator_range frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } - boost::iterator_range parents() { return boost::make_iterator_range(beginParents(), endParents()); } + /** + * Permutes the Conditional, but for efficiency requires the permutation + * to already be inverted. + */ + void permuteWithInverse(const Permutation& inversePermutation) { + factor_.permuteWithInverse(inversePermutation); } protected: /** Debugging invariant that the keys should be in order, including that the @@ -164,7 +170,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(factor_); - ar & BOOST_SERIALIZATION_NVP(nFrontal_); + ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } }; diff --git a/inference/Factor.cpp b/inference/Factor.cpp index 88b3fd02b..7ae8c7f71 100644 --- a/inference/Factor.cpp +++ b/inference/Factor.cpp @@ -44,14 +44,14 @@ Conditional::shared_ptr Factor::eliminateFirst() { } /* ************************************************************************* */ -boost::shared_ptr > Factor::eliminate(Index nFrontals) { - assert(keys_.size() >= nFrontals); +boost::shared_ptr > Factor::eliminate(size_t nrFrontals) { + assert(keys_.size() >= nrFrontals); checkSorted(); BayesNet::shared_ptr fragment(new BayesNet()); const_iterator nextFrontal = this->begin(); - for(Index n = 0; n < nFrontals; ++n, ++nextFrontal) - fragment->push_back(Conditional::shared_ptr(new Conditional(nextFrontal, const_iterator(this->end()), 1))); - if(nFrontals > 0) + for(Index n = 0; n < nrFrontals; ++n, ++nextFrontal) + fragment->push_back(Conditional::fromRange(nextFrontal, const_iterator(this->end()), 1)); + if(nrFrontals > 0) keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents()); return fragment; } diff --git a/inference/Factor.h b/inference/Factor.h index 9c407709b..f10e2f1bf 100644 --- a/inference/Factor.h +++ b/inference/Factor.h @@ -100,9 +100,9 @@ public: boost::shared_ptr eliminateFirst(); /** - * eliminate the first nFrontals frontal variables. + * eliminate the first nrFrontals frontal variables. */ - boost::shared_ptr > eliminate(Index nFrontals = 1); + boost::shared_ptr > eliminate(size_t nrFrontals = 1); /** * Permutes the GaussianFactor, but for efficiency requires the permutation diff --git a/inference/inference.cpp b/inference/inference.cpp index bf05409c2..23732cf8c 100644 --- a/inference/inference.cpp +++ b/inference/inference.cpp @@ -72,7 +72,7 @@ Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex< // Join the factors and eliminate the variable from the joint factor tic("EliminateOne: Combine"); - Conditional::shared_ptr conditional(new Conditional(involvedKeys.begin(), involvedKeys.end(), 1)); + Conditional::shared_ptr conditional = Conditional::fromRange(involvedKeys.begin(), involvedKeys.end(), 1); Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents())); toc("EliminateOne: Combine"); diff --git a/inference/tests/testSymbolicFactor.cpp b/inference/tests/testSymbolicFactor.cpp index 1bb5884c3..307155f5b 100644 --- a/inference/tests/testSymbolicFactor.cpp +++ b/inference/tests/testSymbolicFactor.cpp @@ -22,16 +22,16 @@ TEST(SymbolicFactor, eliminate) { BayesNet fragment = *actual.eliminate(3); Factor expected(keys.begin()+3, keys.end()); - Conditional expected0(keys.begin(), keys.end(), 1); - Conditional expected1(keys.begin()+1, keys.end(), 1); - Conditional expected2(keys.begin()+2, keys.end(), 1); + Conditional::shared_ptr expected0 = Conditional::fromRange(keys.begin(), keys.end(), 1); + Conditional::shared_ptr expected1 = Conditional::fromRange(keys.begin()+1, keys.end(), 1); + Conditional::shared_ptr expected2 = Conditional::fromRange(keys.begin()+2, keys.end(), 1); CHECK(assert_equal(fragment.size(), size_t(3))); CHECK(assert_equal(expected, actual)); BayesNet::const_iterator fragmentCond = fragment.begin(); - CHECK(assert_equal(**fragmentCond++, expected0)); - CHECK(assert_equal(**fragmentCond++, expected1)); - CHECK(assert_equal(**fragmentCond++, expected2)); + CHECK(assert_equal(**fragmentCond++, *expected0)); + CHECK(assert_equal(**fragmentCond++, *expected1)); + CHECK(assert_equal(**fragmentCond++, *expected2)); } /* ************************************************************************* */ diff --git a/linear/GaussianConditional.cpp b/linear/GaussianConditional.cpp index e77ad14f4..6d3e78aad 100644 --- a/linear/GaussianConditional.cpp +++ b/linear/GaussianConditional.cpp @@ -63,7 +63,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list >& parents, const Vector& sigmas) : rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); - Conditional::nFrontal_ = 1; + Conditional::nrFrontals_ = 1; Conditional::factor_.keys_.resize(1+parents.size()); size_t dims[1+parents.size()+1]; dims[0] = R.size2(); diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index 3d835ba57..0d0de3999 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -90,7 +90,7 @@ public: * for multiple frontal variables. */ template - GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nFrontals, const VerticalBlockView& matrices, const Vector& sigmas); + GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nrFrontals, const VerticalBlockView& matrices, const Vector& sigmas); /** print */ void print(const std::string& = "GaussianConditional") const; @@ -151,11 +151,16 @@ private: }; /* ************************************************************************* */ -template -GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nFrontals, const VerticalBlockView& matrices, const Vector& sigmas) : -Conditional(firstKey, lastKey, nFrontals), rsd_(matrix_), sigmas_(sigmas) { - rsd_.assignNoalias(matrices); -} + template + GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, + size_t nrFrontals, const VerticalBlockView& matrices, + const Vector& sigmas) : + rsd_(matrix_), sigmas_(sigmas) { + nrFrontals_ = nrFrontals; + std::copy(firstKey, lastKey, back_inserter(factor_.keys_)); + rsd_.assignNoalias(matrices); + } + /* ************************************************************************* */ } diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index cb920367e..7f7459314 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -524,11 +524,11 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() { } /* ************************************************************************* */ -GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nFrontals) { +GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0); assert(!permuted_.value); - assert(keys_.size() >= nFrontals); + assert(keys_.size() >= nrFrontals); checkSorted(); static const bool debug = false; @@ -582,7 +582,7 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nFrontals) { if(debug) gtsam::print(matrix_, "QR result: "); - size_t frontalDim = Ab_.range(0,nFrontals).size2(); + size_t frontalDim = Ab_.range(0,nrFrontals).size2(); // Check for singular factor if(noiseModel->dim() < frontalDim) { @@ -594,7 +594,7 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nFrontals) { // Extract conditionals tic("eliminate: cond Rd"); GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet()); - for(size_t j=0; jdim(); - keys_.assign(keys_.begin() + nFrontals, keys_.end()); + keys_.assign(keys_.begin() + nrFrontals, keys_.end()); // Set sigmas with the right model if (noiseModel->isConstrained()) model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index d47408fc0..c4de6bcb3 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -208,7 +208,7 @@ public: GaussianConditional::shared_ptr eliminateFirst(); - GaussianBayesNet::shared_ptr eliminate(Index nFrontals = 1); + GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1); friend class GaussianFactorGraph; friend class Inference;