Factors always sorted by key (for matrices and key vector), checkInvariants() function for several classes, fixes in print and equals for VariableIndex (were not accouting for permuted variable indices).
Richard Roberts
parent
e545f59fb2
commit
e2430fcbf8
|
|
@ -8,6 +8,8 @@
|
|||
#include "TestRegistry.h"
|
||||
#include "SimpleString.h"
|
||||
|
||||
//#include <iostream>
|
||||
//using namespace std;
|
||||
|
||||
void TestRegistry::addTest (Test *test)
|
||||
{
|
||||
|
|
@ -50,6 +52,7 @@ int TestRegistry::run (TestResult& result)
|
|||
for (Test *test = tests; test != 0; test = test->getNext ()) {
|
||||
if (test->safe()) {
|
||||
try {
|
||||
// cout << test->getName().asCharString() << ", " << test->getFilename().asCharString() << ":" << test->getLineNumber() << endl;
|
||||
test->run (result);
|
||||
} catch (std::exception& e) {
|
||||
// catch standard exceptions and derivatives
|
||||
|
|
|
|||
|
|
@ -103,9 +103,9 @@ public:
|
|||
template<typename Iterator>
|
||||
VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight);
|
||||
|
||||
size_t size1() const { checkInvariants(); return rowEnd_ - rowStart_; }
|
||||
size_t size2() const { checkInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
size_t nBlocks() const { checkInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; }
|
||||
size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
|
||||
size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
|
||||
|
||||
block_type operator()(size_t block) {
|
||||
return range(block, block+1);
|
||||
|
|
@ -116,7 +116,7 @@ public:
|
|||
}
|
||||
|
||||
block_type range(size_t startBlock, size_t endBlock) {
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
size_t actualStartBlock = startBlock + blockStart_;
|
||||
size_t actualEndBlock = endBlock + blockStart_;
|
||||
checkBlock(actualStartBlock);
|
||||
|
|
@ -127,7 +127,7 @@ public:
|
|||
}
|
||||
|
||||
const_block_type range(size_t startBlock, size_t endBlock) const {
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
size_t actualStartBlock = startBlock + blockStart_;
|
||||
size_t actualEndBlock = endBlock + blockStart_;
|
||||
checkBlock(actualStartBlock);
|
||||
|
|
@ -138,7 +138,7 @@ public:
|
|||
}
|
||||
|
||||
column_type column(size_t block, size_t columnOffset) {
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2());
|
||||
|
|
@ -147,7 +147,7 @@ public:
|
|||
}
|
||||
|
||||
const_column_type column(size_t block, size_t columnOffset) const {
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2());
|
||||
|
|
@ -156,7 +156,7 @@ public:
|
|||
}
|
||||
|
||||
size_t offset(size_t block) const {
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
size_t actualBlock = block + blockStart_;
|
||||
checkBlock(actualBlock);
|
||||
return variableColOffsets_[actualBlock] - variableColOffsets_[blockStart_];
|
||||
|
|
@ -184,8 +184,13 @@ public:
|
|||
template<class RhsMatrix>
|
||||
VerticalBlockView<Matrix>& assignNoalias(const VerticalBlockView<RhsMatrix>& rhs);
|
||||
|
||||
/** Swap the contents of the underlying matrix and the block information with
|
||||
* another VerticalBlockView.
|
||||
*/
|
||||
void swap(VerticalBlockView<Matrix>& other);
|
||||
|
||||
protected:
|
||||
void checkInvariants() const {
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.size2() == variableColOffsets_.back());
|
||||
assert(blockStart_ < variableColOffsets_.size());
|
||||
assert(rowStart_ <= matrix_.size1());
|
||||
|
|
@ -214,30 +219,34 @@ protected:
|
|||
friend class VerticalBlockView<Matrix>;
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
|
||||
fillOffsets((size_t*)0, (size_t*)0);
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
template<typename Iterator>
|
||||
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
template<typename Iterator>
|
||||
VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight) :
|
||||
matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) {
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false);
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
template<class RhsMatrix>
|
||||
void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs) {
|
||||
|
|
@ -257,9 +266,10 @@ void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMat
|
|||
rowStart_ = 0;
|
||||
rowEnd_ = matrix_.size1();
|
||||
blockStart_ = 0;
|
||||
checkInvariants();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
template<class RhsMatrix>
|
||||
VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const VerticalBlockView<RhsMatrix>& rhs) {
|
||||
|
|
@ -268,5 +278,17 @@ VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const Vertic
|
|||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Matrix>
|
||||
void VerticalBlockView<Matrix>::swap(VerticalBlockView<Matrix>& other) {
|
||||
matrix_.swap(other.matrix_);
|
||||
variableColOffsets_.swap(other.variableColOffsets_);
|
||||
std::swap(rowStart_, other.rowStart_);
|
||||
std::swap(rowEnd_, other.rowEnd_);
|
||||
std::swap(blockStart_, other.blockStart_);
|
||||
assertInvariants();
|
||||
other.assertInvariants();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,21 +27,13 @@ namespace gtsam {
|
|||
* kept in pointer containers. To be safe, you should make them
|
||||
* immutable, i.e., practicing functional programming.
|
||||
*/
|
||||
class Conditional: boost::noncopyable, public Testable<Conditional> {
|
||||
class Conditional: public Factor, boost::noncopyable, public Testable<Conditional> {
|
||||
|
||||
protected:
|
||||
/** Conditional just uses an internal Factor for storage (a conditional is
|
||||
* really just a special factor anyway, but we do this instead of inherit
|
||||
* to prevent "diamond" inheritance with GaussianFactor and
|
||||
* GaussianConditional.
|
||||
*/
|
||||
Factor factor_;
|
||||
|
||||
/** The first nFrontal variables are frontal and the rest are parents. */
|
||||
size_t nrFrontals_;
|
||||
|
||||
ValueWithDefault<bool, true> permuted_;
|
||||
|
||||
public:
|
||||
|
||||
/** convenience typename for a shared pointer to this class */
|
||||
|
|
@ -56,20 +48,20 @@ public:
|
|||
Conditional() : nrFrontals_(0) {}
|
||||
|
||||
/** No parents */
|
||||
Conditional(Index key) : factor_(key), nrFrontals_(1) {}
|
||||
Conditional(Index key) : Factor(key), nrFrontals_(1) {}
|
||||
|
||||
/** Single parent */
|
||||
Conditional(Index key, Index parent) : factor_(key, parent), nrFrontals_(1) {}
|
||||
Conditional(Index key, Index parent) : Factor(key, parent), nrFrontals_(1) {}
|
||||
|
||||
/** Two parents */
|
||||
Conditional(Index key, Index parent1, Index parent2) : factor_(key, parent1, parent2), nrFrontals_(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), nrFrontals_(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) : nrFrontals_(1) {
|
||||
factor_.keys_.resize(1 + parents.size());
|
||||
keys_.resize(1 + parents.size());
|
||||
*(beginFrontals()) = key;
|
||||
std::copy(parents.begin(), parents.end(), beginParents());
|
||||
}
|
||||
|
|
@ -79,37 +71,34 @@ public:
|
|||
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_));
|
||||
std::copy(firstKey, lastKey, back_inserter(conditional->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 nrFrontals_ == c.nrFrontals_ && Factor::equals(c, 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_; }
|
||||
size_t nrParents() const { return keys_.size() - nrFrontals_; }
|
||||
|
||||
/** Special accessor when there is only one frontal variable. */
|
||||
Index key() const { assert(nrFrontals_==1); return factor_.keys_[0]; }
|
||||
|
||||
/** return a const reference to all keys */
|
||||
const std::vector<Index>& keys() const { return factor_.keys(); }
|
||||
Index key() const { assert(nrFrontals_==1); return keys_[0]; }
|
||||
|
||||
/** 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(); }
|
||||
const_iterator beginFrontals() const { return keys_.begin(); }
|
||||
const_iterator endFrontals() const { return keys_.begin()+nrFrontals_; }
|
||||
const_iterator beginParents() const { return keys_.begin()+nrFrontals_; }
|
||||
const_iterator endParents() const { return 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(); }
|
||||
iterator beginFrontals() { return keys_.begin(); }
|
||||
iterator endFrontals() { return keys_.begin()+nrFrontals_; }
|
||||
iterator beginParents() { return keys_.begin()+nrFrontals_; }
|
||||
iterator endParents() { return 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()); }
|
||||
|
||||
|
|
@ -154,13 +143,22 @@ public:
|
|||
* to already be inverted.
|
||||
*/
|
||||
void permuteWithInverse(const Permutation& inversePermutation) {
|
||||
factor_.permuteWithInverse(inversePermutation); }
|
||||
// The permutation may not move the separators into the frontals
|
||||
#ifndef NDEBUG
|
||||
BOOST_FOREACH(const Index frontal, this->frontals()) {
|
||||
BOOST_FOREACH(const Index separator, this->parents()) {
|
||||
assert(inversePermutation[frontal] < inversePermutation[separator]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
Factor::permuteWithInverse(inversePermutation);
|
||||
}
|
||||
|
||||
protected:
|
||||
/** Debugging invariant that the keys should be in order, including that the
|
||||
* conditioned variable is numbered lower than the parents.
|
||||
*/
|
||||
void checkSorted() const;
|
||||
void assertInvariants() const;
|
||||
|
||||
friend class Factor;
|
||||
|
||||
|
|
@ -169,7 +167,6 @@ private:
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(factor_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -29,13 +29,9 @@ namespace gtsam {
|
|||
// checkSorted();
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Derived> Factor::Factor(const Derived& c) : keys_(c.keys()), permuted_(c.permuted_) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class KeyIterator> Factor::Factor(KeyIterator beginKey, KeyIterator endKey) :
|
||||
keys_(beginKey, endKey) { checkSorted(); }
|
||||
keys_(beginKey, endKey) { assertInvariants(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FactorGraphType, class VariableIndexStorage>
|
||||
|
|
@ -43,7 +39,7 @@ Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph,
|
|||
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
|
||||
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
|
||||
|
||||
return shared_ptr(boost::make_shared<Factor>(variables.begin(), variables.end()));
|
||||
return shared_ptr(new Factor(variables.begin(), variables.end()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -20,7 +20,10 @@ using namespace boost::lambda;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Factor::Factor(const Factor& f) : keys_(f.keys_), permuted_(f.permuted_) {}
|
||||
Factor::Factor(const Factor& f) : keys_(f.keys_) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Factor::Factor(const Conditional& c) : keys_(c.keys()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Factor::print(const std::string& s) const {
|
||||
|
|
@ -37,7 +40,7 @@ bool Factor::equals(const Factor& other, double tol) const {
|
|||
/* ************************************************************************* */
|
||||
Conditional::shared_ptr Factor::eliminateFirst() {
|
||||
assert(!keys_.empty());
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
Index eliminated = keys_.front();
|
||||
keys_.erase(keys_.begin());
|
||||
return Conditional::shared_ptr(new Conditional(eliminated, keys_));
|
||||
|
|
@ -46,7 +49,7 @@ Conditional::shared_ptr Factor::eliminateFirst() {
|
|||
/* ************************************************************************* */
|
||||
boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) {
|
||||
assert(keys_.size() >= nrFrontals);
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
BayesNet<Conditional>::shared_ptr fragment(new BayesNet<Conditional>());
|
||||
const_iterator nextFrontal = this->begin();
|
||||
for(Index n = 0; n < nrFrontals; ++n, ++nextFrontal)
|
||||
|
|
@ -58,7 +61,6 @@ boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Factor::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
this->permuted_.value = true;
|
||||
BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -40,11 +40,10 @@ class Factor : public Testable<Factor> {
|
|||
protected:
|
||||
|
||||
std::vector<Index> keys_;
|
||||
ValueWithDefault<bool,true> permuted_;
|
||||
|
||||
/** Internal check to make sure keys are sorted (invariant during elimination).
|
||||
/** Internal check to make sure keys are sorted.
|
||||
* If NDEBUG is defined, this is empty and optimized out. */
|
||||
void checkSorted() const;
|
||||
void assertInvariants() const;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -57,29 +56,29 @@ public:
|
|||
Factor(const Factor& f);
|
||||
|
||||
/** Construct from derived type */
|
||||
template<class Derived> Factor(const Derived& c);
|
||||
Factor(const Conditional& c);
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KeyIterator> Factor(KeyIterator beginKey, KeyIterator endKey);
|
||||
|
||||
/** Default constructor for I/O */
|
||||
Factor() : permuted_(false) {}
|
||||
Factor() {}
|
||||
|
||||
/** Construct unary factor */
|
||||
Factor(Index key) : keys_(1), permuted_(false) {
|
||||
keys_[0] = key; checkSorted(); }
|
||||
Factor(Index key) : keys_(1) {
|
||||
keys_[0] = key; assertInvariants(); }
|
||||
|
||||
/** Construct binary factor */
|
||||
Factor(Index key1, Index key2) : keys_(2), permuted_(false) {
|
||||
keys_[0] = key1; keys_[1] = key2; checkSorted(); }
|
||||
Factor(Index key1, Index key2) : keys_(2) {
|
||||
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
|
||||
|
||||
/** Construct ternary factor */
|
||||
Factor(Index key1, Index key2, Index key3) : keys_(3), permuted_(false) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; checkSorted(); }
|
||||
Factor(Index key1, Index key2, Index key3) : keys_(3) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
|
||||
|
||||
/** Construct 4-way factor */
|
||||
Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4), permuted_(false) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; checkSorted(); }
|
||||
Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
|
||||
|
||||
/** Named constructor for combining a set of factors with pre-computed set of
|
||||
* variables. (Old style - will be removed when scalar elimination is
|
||||
|
|
@ -158,10 +157,11 @@ protected:
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
inline void Factor::checkSorted() const {
|
||||
inline void Factor::assertInvariants() const {
|
||||
#ifndef NDEBUG
|
||||
for(size_t pos=1; pos<keys_.size(); ++pos)
|
||||
assert(keys_[pos-1] < keys_[pos]);
|
||||
std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
|
||||
assert(uniqueSorted.size() == keys_.size());
|
||||
assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -34,17 +34,6 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<class Conditional>
|
||||
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
|
||||
typename BayesNet<Conditional>::const_iterator it = bayesNet.begin();
|
||||
for (; it != bayesNet.end(); it++) {
|
||||
sharedFactor factor(new Factor(**it));
|
||||
push_back(factor);
|
||||
}
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// template<class Conditional>
|
||||
// FactorGraph::FactorGraph(const BayesNet<Conditional>& bayesNet, const Inference::Permutation& permutation) {
|
||||
|
|
|
|||
|
|
@ -63,13 +63,9 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
FactorGraph(const BayesNet<Conditional>& bayesNet);
|
||||
|
||||
// /** convert from Bayes net while permuting at the same time */
|
||||
// template<class Conditional>
|
||||
// FactorGraph(const BayesNet<Conditional>& bayesNet, const Inference::Permutation& permutation);
|
||||
|
||||
/** convert from a derived type */
|
||||
template<class Derived>
|
||||
FactorGraph(const Derived& factorGraph);
|
||||
template<class DerivedFactor>
|
||||
FactorGraph(const FactorGraph<DerivedFactor>& factorGraph);
|
||||
|
||||
/** Add a factor */
|
||||
template<class DerivedFactor>
|
||||
|
|
@ -210,10 +206,10 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<class Derived>
|
||||
FactorGraph<Factor>::FactorGraph(const Derived& factorGraph) {
|
||||
template<class DerivedFactor>
|
||||
FactorGraph<Factor>::FactorGraph(const FactorGraph<DerivedFactor>& factorGraph) {
|
||||
factors_.reserve(factorGraph.size());
|
||||
BOOST_FOREACH(const typename Derived::sharedFactor& factor, factorGraph) {
|
||||
BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) {
|
||||
if(factor)
|
||||
this->push_back(sharedFactor(new Factor(*factor)));
|
||||
else
|
||||
|
|
@ -221,6 +217,16 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<class Conditional>
|
||||
FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
|
||||
factors_.reserve(bayesNet.size());
|
||||
BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) {
|
||||
this->push_back(sharedFactor(new Factor(*cond)));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Factor>
|
||||
template<class DerivedFactor>
|
||||
|
|
|
|||
|
|
@ -212,9 +212,13 @@ void VariableIndex<Storage>::rebaseFactors(ptrdiff_t baseIndexChange) {
|
|||
template<class Storage>
|
||||
template<class Derived>
|
||||
bool VariableIndex<Storage>::equals(const Derived& other, double tol) const {
|
||||
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ && this->index_.size() == other.index_.size()) {
|
||||
for(size_t var=0; var<this->index_.size(); ++var)
|
||||
if(this->index_[var] != other.index_[var])
|
||||
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
|
||||
for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var)
|
||||
if(var >= this->index_.size() || var >= other.index_.size()) {
|
||||
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
|
||||
(var >= other.index_.size() && this->index_[var].empty())))
|
||||
return false;
|
||||
} else if(this->index_[var] != other.index_[var])
|
||||
return false;
|
||||
} else
|
||||
return false;
|
||||
|
|
@ -229,7 +233,7 @@ void VariableIndex<Storage>::print(const std::string& str) const {
|
|||
BOOST_FOREACH(const mapped_type& variable, index_.container()) {
|
||||
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
|
||||
assert(rvar != index_.permutation().end());
|
||||
std::cout << "var " << *rvar << ":";
|
||||
std::cout << "var " << (rvar-index_.permutation().begin()) << ":";
|
||||
BOOST_FOREACH(const mapped_factor_type& factor, variable) {
|
||||
std::cout << " " << factor.factorIndex << "-" << factor.variablePosition;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -220,32 +220,31 @@ typedef vector<boost::optional<Index> > optionalIndices;
|
|||
*/
|
||||
optionalIndices buildETree(const StructA& structA) {
|
||||
|
||||
// todo: get n
|
||||
size_t n = 5;
|
||||
optionalIndices parent(n);
|
||||
// todo: get n
|
||||
size_t n = 5;
|
||||
optionalIndices parent(n);
|
||||
|
||||
// todo: get m
|
||||
size_t m = 5;
|
||||
optionalIndices prev_col(m);
|
||||
// todo: get m
|
||||
size_t m = 5;
|
||||
optionalIndices prev_col(m);
|
||||
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(RowIndex i, structA[j])
|
||||
{
|
||||
if (prev_col[i]) {
|
||||
Index k = *(prev_col[i]);
|
||||
// find root r of the current tree that contains k
|
||||
Index r = k;
|
||||
while (parent[r])
|
||||
r = *parent[r];
|
||||
if (r != j) parent[r].reset(j);
|
||||
}
|
||||
prev_col[i].reset(j);
|
||||
}
|
||||
}
|
||||
// for column j \in 1 to n do
|
||||
for (Index j = 0; j < n; j++) {
|
||||
// for row i \in Struct[A*j] do
|
||||
BOOST_FOREACH(RowIndex i, structA[j]) {
|
||||
if (prev_col[i]) {
|
||||
Index k = *(prev_col[i]);
|
||||
// find root r of the current tree that contains k
|
||||
Index r = k;
|
||||
while (parent[r])
|
||||
r = *parent[r];
|
||||
if (r != j) parent[r].reset(j);
|
||||
}
|
||||
prev_col[i].reset(j);
|
||||
}
|
||||
}
|
||||
|
||||
return parent;
|
||||
return parent;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -64,13 +64,13 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
|
|||
rsd_(matrix_), sigmas_(sigmas) {
|
||||
assert(R.size1() <= R.size2());
|
||||
Conditional::nrFrontals_ = 1;
|
||||
Conditional::factor_.keys_.resize(1+parents.size());
|
||||
keys_.resize(1+parents.size());
|
||||
size_t dims[1+parents.size()+1];
|
||||
dims[0] = R.size2();
|
||||
Conditional::factor_.keys_[0] = key;
|
||||
keys_[0] = key;
|
||||
size_t j=1;
|
||||
for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
|
||||
Conditional::factor_.keys_[j] = parent->first;
|
||||
keys_[j] = parent->first;
|
||||
dims[j] = parent->second.size2();
|
||||
++ j;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ public:
|
|||
/** return stuff contained in GaussianConditional */
|
||||
rsd_type::const_column_type get_d() const { return rsd_.column(1+nrParents(), 0); }
|
||||
rsd_type::const_block_type get_R() const { return rsd_(0); }
|
||||
rsd_type::const_block_type get_S(const_iterator variable) const { return rsd_(variable-Conditional::factor_.begin()); }
|
||||
rsd_type::const_block_type get_S(const_iterator variable) const { return rsd_(variable - this->begin()); }
|
||||
const Vector& get_sigmas() const {return sigmas_;}
|
||||
|
||||
/**
|
||||
|
|
@ -133,7 +133,7 @@ public:
|
|||
protected:
|
||||
rsd_type::column_type get_d_() { return rsd_.column(1+nrParents(), 0); }
|
||||
rsd_type::block_type get_R_() { return rsd_(0); }
|
||||
rsd_type::block_type get_S_(iterator variable) { return rsd_(variable-const_cast<const Factor&>(Conditional::factor_).begin()); }
|
||||
rsd_type::block_type get_S_(iterator variable) { return rsd_(variable - this->begin()); }
|
||||
|
||||
friend class GaussianFactor;
|
||||
|
||||
|
|
@ -157,7 +157,7 @@ private:
|
|||
const Vector& sigmas) :
|
||||
rsd_(matrix_), sigmas_(sigmas) {
|
||||
nrFrontals_ = nrFrontals;
|
||||
std::copy(firstKey, lastKey, back_inserter(factor_.keys_));
|
||||
std::copy(firstKey, lastKey, back_inserter(keys_));
|
||||
rsd_.assignNoalias(matrices);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,15 +32,10 @@ using namespace boost::lambda;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
inline void GaussianFactor::checkSorted() const {
|
||||
inline void GaussianFactor::assertInvariants() const {
|
||||
#ifndef NDEBUG
|
||||
// Make sure variables are sorted
|
||||
assert(keys_.size()+1 == Ab_.nBlocks());
|
||||
for(size_t varpos=0; varpos<keys_.size(); ++varpos) {
|
||||
if(varpos > 0) {
|
||||
assert(keys_[varpos] > keys_[varpos-1]);
|
||||
}
|
||||
}
|
||||
Factor::assertInvariants();
|
||||
assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks());
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -48,16 +43,18 @@ inline void GaussianFactor::checkSorted() const {
|
|||
GaussianFactor::GaussianFactor(const GaussianFactor& gf) :
|
||||
Factor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) {
|
||||
Ab_.assignNoalias(gf.Ab_);
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactor::GaussianFactor() : Ab_(matrix_) {}
|
||||
GaussianFactor::GaussianFactor() : Ab_(matrix_) { assertInvariants(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.size(), 0), Ab_(matrix_) {
|
||||
size_t dims[] = { 1 };
|
||||
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+1, b_in.size()));
|
||||
getb() = b_in;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -68,6 +65,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
|
|||
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size()));
|
||||
Ab_(0) = A1;
|
||||
getb() = b;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -79,6 +77,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri
|
|||
Ab_(0) = A1;
|
||||
Ab_(1) = A2;
|
||||
getb() = b;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -91,6 +90,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri
|
|||
Ab_(1) = A2;
|
||||
Ab_(2) = A3;
|
||||
getb() = b;
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -108,8 +108,7 @@ GaussianFactor::GaussianFactor(const std::vector<std::pair<Index, Matrix> > &ter
|
|||
for(size_t j=0; j<terms.size(); ++j)
|
||||
Ab_(j) = terms[j].second;
|
||||
getb() = b;
|
||||
Factor::permuted_ = false;
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -133,8 +132,7 @@ GaussianFactor::GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms
|
|||
++ j;
|
||||
}
|
||||
getb() = b;
|
||||
Factor::permuted_ = false;
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -142,60 +140,9 @@ GaussianFactor::GaussianFactor(const GaussianConditional& cg) : Factor(cg), mode
|
|||
Ab_.assignNoalias(cg.rsd_);
|
||||
// todo SL: make firstNonzeroCols triangular?
|
||||
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
|
||||
//{
|
||||
// bool verbose = false;
|
||||
// if (verbose) cout << "GaussianFactor::GaussianFactor (factors)" << endl;
|
||||
//
|
||||
// // Create RHS and sigmas of right size by adding together row counts
|
||||
// size_t m = 0;
|
||||
// BOOST_FOREACH(const shared_ptr& factor, factors) m += factor->numberOfRows();
|
||||
// b_ = Vector(m);
|
||||
// Vector sigmas(m);
|
||||
//
|
||||
// size_t pos = 0; // save last position inserted into the new rhs vector
|
||||
//
|
||||
// // iterate over all factors
|
||||
// bool constrained = false;
|
||||
// BOOST_FOREACH(const shared_ptr& factor, factors){
|
||||
// if (verbose) factor->print();
|
||||
// // number of rows for factor f
|
||||
// const size_t mf = factor->numberOfRows();
|
||||
//
|
||||
// // copy the rhs vector from factor to b
|
||||
// const Vector bf = factor->get_b();
|
||||
// for (size_t i=0; i<mf; i++) b_(pos+i) = bf(i);
|
||||
//
|
||||
// // copy the model_
|
||||
// for (size_t i=0; i<mf; i++) sigmas(pos+i) = factor->model_->sigma(i);
|
||||
//
|
||||
// // update the matrices
|
||||
// append_factor(factor,m,pos);
|
||||
//
|
||||
// // check if there are constraints
|
||||
// if (verbose) factor->model_->print("Checking for zeros");
|
||||
// if (!constrained && factor->model_->isConstrained()) {
|
||||
// constrained = true;
|
||||
// if (verbose) cout << "Found a constraint!" << endl;
|
||||
// }
|
||||
//
|
||||
// pos += mf;
|
||||
// }
|
||||
//
|
||||
// if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl;
|
||||
//
|
||||
// if (constrained) {
|
||||
// model_ = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
// if (verbose) model_->print("Just created Constraint ^");
|
||||
// } else {
|
||||
// model_ = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
// if (verbose) model_->print("Just created Diagonal");
|
||||
// }
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactor::print(const string& s) const {
|
||||
cout << s << "\n";
|
||||
|
|
@ -211,15 +158,6 @@ void GaussianFactor::print(const string& s) const {
|
|||
}
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//size_t GaussianFactor::getDim(Index key) const {
|
||||
// const_iterator it = findA(key);
|
||||
// if (it != end())
|
||||
// return it->second.size2();
|
||||
// else
|
||||
// return 0;
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check if two linear factors are equal
|
||||
bool GaussianFactor::equals(const GaussianFactor& f, double tol) const {
|
||||
|
|
@ -241,11 +179,39 @@ bool GaussianFactor::equals(const GaussianFactor& f, double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactor::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
this->permuted_.value = true;
|
||||
BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
|
||||
|
||||
// Build a map from the new variable indices to the old slot positions.
|
||||
typedef map<size_t, size_t, std::less<size_t>, boost::fast_pool_allocator<std::pair<const size_t, size_t> > > SourceSlots;
|
||||
SourceSlots sourceSlots;
|
||||
for(size_t j=0; j<keys_.size(); ++j)
|
||||
sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
|
||||
|
||||
// Build a vector of variable dimensions in the new order
|
||||
vector<size_t> dimensions(keys_.size() + 1);
|
||||
size_t j = 0;
|
||||
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
|
||||
dimensions[j++] = Ab_(sourceSlot.second).size2();
|
||||
}
|
||||
assert(j == keys_.size());
|
||||
dimensions.back() = 1;
|
||||
|
||||
// Copy the variables and matrix into the new order
|
||||
vector<Index> oldKeys(keys_.size());
|
||||
keys_.swap(oldKeys);
|
||||
matrix_type oldMatrix;
|
||||
ab_type oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1());
|
||||
Ab_.swap(oldAb);
|
||||
j = 0;
|
||||
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
|
||||
keys_[j] = sourceSlot.first;
|
||||
ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second);
|
||||
}
|
||||
ublas::noalias(Ab_(j)) = oldAb(j);
|
||||
|
||||
// Since we're permuting the variables, ensure that entire rows from this
|
||||
// factor are copied when Combine is called
|
||||
BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; }
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -334,21 +300,6 @@ pair<Matrix,Vector> GaussianFactor::matrix(bool weight) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactor::matrix_augmented(bool weight) const {
|
||||
// // get pointers to the matrices
|
||||
// vector<const Matrix *> matrices;
|
||||
// BOOST_FOREACH(Index j, ordering) {
|
||||
// const Matrix& Aj = get_A(j);
|
||||
// matrices.push_back(&Aj);
|
||||
// }
|
||||
//
|
||||
// // load b into a matrix
|
||||
// size_t rows = b_.size();
|
||||
// Matrix B_mat(rows, 1);
|
||||
// memcpy(B_mat.data().begin(), b_.data().begin(), rows*sizeof(double));
|
||||
// matrices.push_back(&B_mat);
|
||||
|
||||
// divide in sigma so error is indeed 0.5*|Ax-b|
|
||||
// Matrix Ab = collect(matrices);
|
||||
if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; }
|
||||
else return Ab_.range(0, Ab_.nBlocks());
|
||||
}
|
||||
|
|
@ -381,40 +332,12 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
|
|||
return boost::tuple<list<int>, list<int>, list<double> >(I,J,S);
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos) {
|
||||
//
|
||||
// // iterate over all matrices from the factor f
|
||||
// BOOST_FOREACH(const NamedMatrix& p, f->As_) {
|
||||
// Index key = p.first;
|
||||
// const Matrix& Aj = p.second;
|
||||
//
|
||||
// // find the corresponding matrix among As
|
||||
// iterator mine = findA(key);
|
||||
// const bool exists = (mine != end());
|
||||
//
|
||||
// // find rows and columns
|
||||
// const size_t n = Aj.size2();
|
||||
//
|
||||
// // use existing or create new matrix
|
||||
// if (exists)
|
||||
// copy(Aj.data().begin(), Aj.data().end(), (mine->second).data().begin()+pos*n);
|
||||
// else {
|
||||
// Matrix Z = zeros(m, n);
|
||||
// copy(Aj.data().begin(), Aj.data().end(), Z.data().begin()+pos*n);
|
||||
// insert(key, Z);
|
||||
// }
|
||||
//
|
||||
// } // FOREACH
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
|
||||
|
||||
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0);
|
||||
assert(!permuted_.value);
|
||||
assert(!keys_.empty());
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
@ -514,12 +437,12 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
|
|||
}
|
||||
toc("eliminateFirst: rowstarts");
|
||||
|
||||
checkSorted();
|
||||
|
||||
if(debug) print("Eliminated factor: ");
|
||||
|
||||
toc("eliminateFirst");
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return conditional;
|
||||
}
|
||||
|
||||
|
|
@ -527,9 +450,8 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
|
|||
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() >= nrFrontals);
|
||||
checkSorted();
|
||||
assertInvariants();
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
@ -635,12 +557,12 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) {
|
|||
}
|
||||
toc("eliminate: rowstarts");
|
||||
|
||||
checkSorted();
|
||||
|
||||
if(debug) print("Eliminated factor: ");
|
||||
|
||||
toc("eliminate");
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return conditionals;
|
||||
|
||||
}
|
||||
|
|
@ -727,10 +649,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
assert(factor.firstNonzeroBlocks_.size() == factor.numberOfRows());
|
||||
BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.firstNonzeroBlocks_) {
|
||||
Index firstNonzeroVar;
|
||||
if(factor.permuted_.value == true)
|
||||
firstNonzeroVar = *std::min_element(factor.keys_.begin(), factor.keys_.end());
|
||||
else
|
||||
firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos];
|
||||
firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos];
|
||||
rowSources.push_back(_RowSource(firstNonzeroVar, factorI, factorRowI));
|
||||
++ factorRowI;
|
||||
}
|
||||
|
|
@ -779,10 +698,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
std::vector<Index>::const_iterator keyitend = factor.keys_.end();
|
||||
while(keyit != keyitend) {
|
||||
const size_t varpos = *varposIt;
|
||||
// If the factor is permuted, the varpos's in the joint factor could be
|
||||
// out of order.
|
||||
if(factor.permuted_.value == true && varpos < ret->firstNonzeroBlocks_[row])
|
||||
ret->firstNonzeroBlocks_[row] = varpos;
|
||||
assert(variables[varpos] == *keyit);
|
||||
ab_type::block_type retBlock(ret->Ab_(varpos));
|
||||
const ab_type::const_block_type factorBlock(factor.getA(keyit));
|
||||
|
|
@ -811,7 +726,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
|
||||
if(debug) ret->print("Combined factor: ");
|
||||
|
||||
ret->checkSorted();
|
||||
ret->assertInvariants();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -889,10 +804,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
const GaussianFactor& sourceFactor(*factors[sourceFactorI]);
|
||||
for(size_t sourceFactorRow = 0; sourceFactorRow < sourceFactor.numberOfRows(); ++sourceFactorRow) {
|
||||
Index firstNonzeroVar;
|
||||
if(sourceFactor.permuted_.value)
|
||||
firstNonzeroVar = *std::min_element(sourceFactor.begin(), sourceFactor.end());
|
||||
else
|
||||
firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]];
|
||||
firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]];
|
||||
rowSources.push_back(_RowSource(firstNonzeroVar, sourceFactorI, sourceFactorRow));
|
||||
}
|
||||
if(sourceFactor.model_->isConstrained()) anyConstrained = true;
|
||||
|
|
@ -922,7 +834,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
if(sourceSlot != numeric_limits<Index>::max()) {
|
||||
const GaussianFactor& source(*factors[rowSources[row].factorI]);
|
||||
const size_t sourceRow = rowSources[row].factorRowI;
|
||||
assert(!source.permuted_.value || source.firstNonzeroBlocks_[sourceRow] == 0);
|
||||
if(source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
|
||||
const ab_type::const_block_type sourceBlock(source.Ab_(sourceSlot));
|
||||
ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow);
|
||||
|
|
@ -955,159 +866,9 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
|
|||
else combined->model_ = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
toc("Combine 6: noise model");
|
||||
|
||||
combined->checkSorted();
|
||||
combined->assertInvariants();
|
||||
|
||||
return combined;
|
||||
}
|
||||
|
||||
|
||||
///* ************************************************************************* */
|
||||
//static GaussianFactor::shared_ptr
|
||||
//GaussianFactor::Combine(const GaussianFactorGraph& factorGraph, const std::vector<size_t>& factors) {
|
||||
//
|
||||
// // Determine row count
|
||||
// size_t m = 0;
|
||||
// BOOST_FOREACH(const size_t& factor, factors) {
|
||||
// m += factorGraph[factor]->numberOfRows();
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
///* Note, in place !!!!
|
||||
// * Do incomplete QR factorization for the first n columns
|
||||
// * We will do QR on all matrices and on RHS
|
||||
// * Then take first n rows and make a GaussianConditional,
|
||||
// * and last rows to make a new joint linear factor on separator
|
||||
// */
|
||||
///* ************************************************************************* */
|
||||
//
|
||||
//pair<GaussianBayesNet, GaussianFactor::shared_ptr>
|
||||
//GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
// const Ordering& frontals, const Ordering& separators,
|
||||
// const Dimensions& dimensions) {
|
||||
// bool verbose = false;
|
||||
//
|
||||
// // Use in-place QR on dense Ab appropriate to NoiseModel
|
||||
// if (verbose) model->print("Before QR");
|
||||
// SharedDiagonal noiseModel = model->QR(Ab);
|
||||
// if (verbose) model->print("After QR");
|
||||
//
|
||||
// // get dimensions of the eliminated variable
|
||||
// // TODO: this is another map find that should be avoided !
|
||||
// size_t n1 = dimensions.at(frontals.front()), n = Ab.size2() - 1;
|
||||
//
|
||||
// // Get alias to augmented RHS d
|
||||
// ublas::matrix_column<Matrix> d(Ab,n);
|
||||
//
|
||||
// // extract the conditionals
|
||||
// GaussianBayesNet bn;
|
||||
// size_t n0 = 0;
|
||||
// Ordering::const_iterator itFrontal1 = frontals.begin(), itFrontal2;
|
||||
// for(; itFrontal1!=frontals.end(); itFrontal1++) {
|
||||
// n1 = n0 + dimensions.at(*itFrontal1);
|
||||
// // create base conditional Gaussian
|
||||
// GaussianConditional::shared_ptr conditional(new GaussianConditional(*itFrontal1,
|
||||
// sub(d, n0, n1), // form d vector
|
||||
// sub(Ab, n0, n1, n0, n1), // form R matrix
|
||||
// sub(noiseModel->sigmas(),n0,n1))); // get standard deviations
|
||||
//
|
||||
// // add parents to the conditional
|
||||
// itFrontal2 = itFrontal1;
|
||||
// itFrontal2 ++;
|
||||
// size_t j = n1;
|
||||
// for (; itFrontal2!=frontals.end(); itFrontal2++) {
|
||||
// size_t dim = dimensions.at(*itFrontal2);
|
||||
// conditional->add(*itFrontal2, sub(Ab, n0, n1, j, j+dim));
|
||||
// j+=dim;
|
||||
// }
|
||||
// BOOST_FOREACH(Index cur_key, separators) {
|
||||
// size_t dim = dimensions.at(cur_key);
|
||||
// conditional->add(cur_key, sub(Ab, n0, n1, j, j+dim));
|
||||
// j+=dim;
|
||||
// }
|
||||
// n0 = n1;
|
||||
// bn.push_back(conditional);
|
||||
// }
|
||||
//
|
||||
// // if m<n1, this factor cannot be eliminated
|
||||
// size_t maxRank = noiseModel->dim();
|
||||
// if (maxRank<n1) {
|
||||
// cout << "Perhaps your factor graph is singular." << endl;
|
||||
// cout << "Here are the keys involved in the factor now being eliminated:" << endl;
|
||||
// separators.print("Keys");
|
||||
// cout << "The first key, '" << frontals.front() << "', corresponds to the variable being eliminated" << endl;
|
||||
// throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
|
||||
// }
|
||||
//
|
||||
// // extract the new factor
|
||||
// GaussianFactor::shared_ptr factor(new GaussianFactor);
|
||||
// size_t j = n1;
|
||||
// BOOST_FOREACH(Index cur_key, separators) {
|
||||
// size_t dim = dimensions.at(cur_key); // TODO avoid find !
|
||||
// factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
|
||||
// j+=dim;
|
||||
// }
|
||||
//
|
||||
// // Set sigmas
|
||||
// // set the right model here
|
||||
// if (noiseModel->isConstrained())
|
||||
// factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank));
|
||||
// else
|
||||
// factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank));
|
||||
//
|
||||
// // extract ds vector for the new b
|
||||
// factor->set_b(sub(d, n1, maxRank));
|
||||
//
|
||||
// return make_pair(bn, factor);
|
||||
//
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
|
||||
//GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
|
||||
// Index frontal, const Ordering& separator,
|
||||
// const Dimensions& dimensions) {
|
||||
// Ordering frontals; frontals += frontal;
|
||||
// pair<GaussianBayesNet, shared_ptr> ret =
|
||||
// eliminateMatrix(Ab, model, frontals, separator, dimensions);
|
||||
// return make_pair(*ret.first.begin(), ret.second);
|
||||
//}
|
||||
///* ************************************************************************* */
|
||||
//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
|
||||
//GaussianFactor::eliminate(Index key) const
|
||||
//{
|
||||
// // if this factor does not involve key, we exit with empty CG and LF
|
||||
// const_iterator it = findA(key);
|
||||
// if (it==end()) {
|
||||
// // Conditional Gaussian is just a parent-less node with P(x)=1
|
||||
// GaussianFactor::shared_ptr lf(new GaussianFactor);
|
||||
// GaussianConditional::shared_ptr cg(new GaussianConditional(key));
|
||||
// return make_pair(cg,lf);
|
||||
// }
|
||||
//
|
||||
// // create an internal ordering that eliminates key first
|
||||
// Ordering ordering;
|
||||
// ordering += key;
|
||||
// BOOST_FOREACH(Index k, keys())
|
||||
// if (k != key) ordering += k;
|
||||
//
|
||||
// // extract [A b] from the combined linear factor (ensure that x is leading)
|
||||
// Matrix Ab = matrix_augmented(ordering,false);
|
||||
//
|
||||
// // TODO: this is where to split
|
||||
// ordering.pop_front();
|
||||
// return eliminateMatrix(Ab, model_, key, ordering, dimensions());
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
string symbol(char c, int index) {
|
||||
stringstream ss;
|
||||
ss << c << index;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
;
|
||||
|
|
|
|||
|
|
@ -153,7 +153,7 @@ protected:
|
|||
/** Protected mutable accessor for the r.h.s. b. */
|
||||
|
||||
/** Internal debug check to make sure variables are sorted */
|
||||
void checkSorted() const;
|
||||
void assertInvariants() const;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -215,14 +215,5 @@ public:
|
|||
|
||||
}; // GaussianFactor
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
/**
|
||||
* creates a C++ string a la "x3", "m768"
|
||||
* @param c the base character
|
||||
* @param index the integer to be added
|
||||
* @return a C++ string
|
||||
*/
|
||||
std::string symbol(char c, int index);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -358,11 +358,8 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
|
|||
GaussianFactorGraph result = *this;
|
||||
|
||||
// for each of the variables, add a prior
|
||||
|
||||
for(Index var=0; var<variableIndex.size(); ++var) {
|
||||
const GaussianVariableIndex<>::mapped_factor_type& factor_pos(variableIndex[var].front());
|
||||
const GaussianFactor& factor(*operator[](factor_pos.factorIndex));
|
||||
size_t dim = factor.getDim(factor.begin() + factor_pos.variablePosition);
|
||||
size_t dim = variableIndex.dim(var);
|
||||
Matrix A = eye(dim);
|
||||
Vector b = zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
|
||||
|
|
|
|||
|
|
@ -59,6 +59,11 @@ public:
|
|||
*/
|
||||
VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
|
||||
|
||||
/** Named constructor to create a VectorValues that matches the structure of
|
||||
* the specified VectorValues, but do not initialize the new values.
|
||||
*/
|
||||
static VectorValues SameStructure(const VectorValues& otherValues);
|
||||
|
||||
/** Element access */
|
||||
mapped_type operator[](Index variable);
|
||||
const_mapped_type operator[](Index variable) const;
|
||||
|
|
@ -201,6 +206,13 @@ inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const V
|
|||
assert(varStarts_.back() == values.size());
|
||||
}
|
||||
|
||||
inline VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
|
||||
VectorValues ret;
|
||||
ret.varStarts_ = otherValues.varStarts_;
|
||||
ret.values_.resize(ret.varStarts_.back(), false);
|
||||
return ret;
|
||||
}
|
||||
|
||||
inline VectorValues::mapped_type VectorValues::operator[](Index variable) {
|
||||
checkVariable(variable);
|
||||
return boost::numeric::ublas::project(values_,
|
||||
|
|
|
|||
|
|
@ -678,6 +678,52 @@ TEST ( GaussianFactor, constraint_eliminate2 )
|
|||
CHECK(assert_equal(expectedCG, *actualCG, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianFactor, permuteWithInverse)
|
||||
{
|
||||
Matrix A1 = Matrix_(2,2,
|
||||
1.0, 2.0,
|
||||
3.0, 4.0);
|
||||
Matrix A2 = Matrix_(2,1,
|
||||
5.0,
|
||||
6.0);
|
||||
Matrix A3 = Matrix_(2,3,
|
||||
7.0, 8.0, 9.0,
|
||||
10.0, 11.0, 12.0);
|
||||
Vector b = Vector_(2, 13.0, 14.0);
|
||||
|
||||
Permutation inversePermutation(6);
|
||||
inversePermutation[0] = 5;
|
||||
inversePermutation[1] = 4;
|
||||
inversePermutation[2] = 3;
|
||||
inversePermutation[3] = 2;
|
||||
inversePermutation[4] = 1;
|
||||
inversePermutation[5] = 0;
|
||||
|
||||
GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
|
||||
GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual)));
|
||||
GaussianVariableIndex<> actualIndex(actualFG);
|
||||
actual.permuteWithInverse(inversePermutation);
|
||||
actualIndex.permute(*inversePermutation.inverse());
|
||||
|
||||
GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
|
||||
GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected)));
|
||||
GaussianVariableIndex<> expectedIndex(expectedFG);
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// todo: fix this!!! VariableIndex should not hold slots
|
||||
for(Index j=0; j<actualIndex.size(); ++j) {
|
||||
BOOST_FOREACH(typename GaussianVariableIndex<>::mapped_factor_type& factor_pos, actualIndex[j]) {
|
||||
factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
}
|
||||
for(Index j=0; j<expectedIndex.size(); ++j) {
|
||||
BOOST_FOREACH(typename GaussianVariableIndex<>::mapped_factor_type& factor_pos, expectedIndex[j]) {
|
||||
factor_pos.variablePosition = numeric_limits<Index>::max(); }
|
||||
}
|
||||
CHECK(assert_equal(expectedIndex, actualIndex));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( GaussianFactor, erase)
|
||||
//{
|
||||
|
|
@ -728,6 +774,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
|
|||
// CHECK(assert_equal(factor_expected, *factor));
|
||||
//}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue