12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

replaced constructor taking iterator with a named constructor fromRange, as the former hid the three-argument constructor and lead to segfaults

release/4.3a0
Frank Dellaert 2010-10-12 00:14:50 +00:00
parent 057050fa9f
commit 024c4b899e
9 changed files with 100 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
inference/Conditional.h
View File

@ -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_);
}
};

10
inference/Factor.cpp
View File

@ -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;
}

4
inference/Factor.h
View File

@ -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

2
inference/inference.cpp
View File

@ -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");

12
inference/tests/testSymbolicFactor.cpp
View File

@ -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));
}
/* ************************************************************************* */

2
linear/GaussianConditional.cpp
View File

@ -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();

17
linear/GaussianConditional.h
View File

@ -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);
}
/* ************************************************************************* */
}

10
linear/GaussianFactor.cpp
View File

@ -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()));

2
linear/GaussianFactor.h
View File

@ -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;