replaced constructor taking iterator with a named constructor fromRange, as the former hid the three-argument constructor and lead to segfaults
							parent
							
								
									057050fa9f
								
							
						
					
					
						commit
						024c4b899e
					
				|  | @ -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<bool, true> 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<Index>& parents) : nFrontal_(1) { | ||||
|     factor_.keys_.resize(1+parents.size()); | ||||
|     *(beginFrontals()) = key; | ||||
|     std::copy(parents.begin(), parents.end(), beginParents()); } | ||||
|   Conditional(Index key, const std::vector<Index>& 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<typename Iterator> | ||||
|   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<Index>& 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<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } | ||||
|   boost::iterator_range<iterator> 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<Index>& 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<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } | ||||
|   boost::iterator_range<iterator> 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<class Archive> | ||||
| 	void serialize(Archive & ar, const unsigned int version) { | ||||
| 		ar & BOOST_SERIALIZATION_NVP(factor_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(nFrontal_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(nrFrontals_); | ||||
| 	} | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -44,14 +44,14 @@ Conditional::shared_ptr Factor::eliminateFirst() { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(Index nFrontals) { | ||||
|   assert(keys_.size() >= nFrontals); | ||||
| boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) { | ||||
|   assert(keys_.size() >= nrFrontals); | ||||
|   checkSorted(); | ||||
|   BayesNet<Conditional>::shared_ptr fragment(new BayesNet<Conditional>()); | ||||
|   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; | ||||
| } | ||||
|  |  | |||
|  | @ -100,9 +100,9 @@ public: | |||
|   boost::shared_ptr<Conditional> eliminateFirst(); | ||||
| 
 | ||||
|   /**
 | ||||
|    * eliminate the first nFrontals frontal variables. | ||||
|    * eliminate the first nrFrontals frontal variables. | ||||
|    */ | ||||
|   boost::shared_ptr<BayesNet<Conditional> > eliminate(Index nFrontals = 1); | ||||
|   boost::shared_ptr<BayesNet<Conditional> > eliminate(size_t nrFrontals = 1); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Permutes the GaussianFactor, but for efficiency requires the permutation | ||||
|  |  | |||
|  | @ -72,7 +72,7 @@ Inference::EliminateOneSymbolic(FactorGraph<Factor>& 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"); | ||||
| 
 | ||||
|  |  | |||
|  | @ -22,16 +22,16 @@ TEST(SymbolicFactor, eliminate) { | |||
|   BayesNet<Conditional> 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<Conditional>::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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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<pair<Index, Matrix> >& 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(); | ||||
|  |  | |||
|  | @ -90,7 +90,7 @@ public: | |||
| 	 * for multiple frontal variables. | ||||
| 	 */ | ||||
| 	template<typename Iterator, class Matrix> | ||||
| 	GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nFrontals, const VerticalBlockView<Matrix>& matrices, const Vector& sigmas); | ||||
| 	GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nrFrontals, const VerticalBlockView<Matrix>& matrices, const Vector& sigmas); | ||||
| 
 | ||||
| 	/** print */ | ||||
| 	void print(const std::string& = "GaussianConditional") const; | ||||
|  | @ -151,11 +151,16 @@ private: | |||
| }; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| template<typename Iterator, class Matrix> | ||||
| GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, size_t nFrontals, const VerticalBlockView<Matrix>& matrices, const Vector& sigmas) : | ||||
| Conditional(firstKey, lastKey, nFrontals), rsd_(matrix_), sigmas_(sigmas) { | ||||
|   rsd_.assignNoalias(matrices); | ||||
| } | ||||
| 	template<typename Iterator, class Matrix> | ||||
| 	GaussianConditional::GaussianConditional(Iterator firstKey, Iterator lastKey, | ||||
| 			size_t nrFrontals, const VerticalBlockView<Matrix>& matrices, | ||||
| 			const Vector& sigmas) : | ||||
| 		rsd_(matrix_), sigmas_(sigmas) { | ||||
|   	nrFrontals_ = nrFrontals; | ||||
| 		std::copy(firstKey, lastKey, back_inserter(factor_.keys_)); | ||||
| 		rsd_.assignNoalias(matrices); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 
 | ||||
| } | ||||
|  |  | |||
|  | @ -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; j<nFrontals; ++j) { | ||||
|   for(size_t j=0; j<nrFrontals; ++j) { | ||||
|     // Temporarily restrict the matrix view to the conditional blocks of the
 | ||||
|     // eliminated Ab matrix to create the GaussianConditional from it.
 | ||||
|     size_t varDim = Ab_(0).size2(); | ||||
|  | @ -612,7 +612,7 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nFrontals) { | |||
|   tic("eliminate: remaining factor"); | ||||
|   // Take lower-right block of Ab to get the new factor
 | ||||
|   Ab_.rowEnd() = noiseModel->dim(); | ||||
|   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())); | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue