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

Yet even more utility functions

release/4.3a0
Alex Cunningham 2010-10-21 08:18:20 +00:00
parent 35e5953d34
commit 145f273e98
2 changed files with 28 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
linear/GaussianFactor.cpp
View File

@ -669,6 +669,27 @@ vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorI
return rowSources;
}
void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row,
const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions,
const size_t factorRow, const size_t factorI, const vector<Index>& variables) {
const static bool debug = false;
const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow];
std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos;
std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
combinedFactor->set_firstNonzeroBlocks(row, *varposIt);
if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
std::vector<Index>::const_iterator keyitend = factor.keys().end();
while(keyit != keyitend) {
const size_t varpos = *varposIt;
assert(variables[varpos] == *keyit);
GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos));
const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit));
ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
++ keyit;
++ varposIt;
}
}
template<class STORAGE>
GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
@ -722,7 +743,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
const size_t factorI = rowSource.factorI;
const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]);
const size_t factorRow = rowSource.factorRowI;
const size_t factorFirstNonzeroVarpos = factor.firstNonzeroBlocks_[factorRow];
if(debug)
cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl;
@ -731,22 +751,9 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
combinedFactor->getb()(row) = factor.getb()(factorRow);
sigmas(row) = factor.get_sigmas()(factorRow);
// Copy the row of A variable by variable, starting at the first nonzero
// variable.
std::vector<Index>::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos;
std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
combinedFactor->firstNonzeroBlocks_[row] = *varposIt;
if(debug) cout << " copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
std::vector<Index>::const_iterator keyitend = factor.keys_.end();
while(keyit != keyitend) {
const size_t varpos = *varposIt;
assert(variables[varpos] == *keyit);
ab_type::block_type retBlock(combinedFactor->Ab_(varpos));
const ab_type::const_block_type factorBlock(factor.getA(keyit));
ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
++ keyit;
++ varposIt;
}
// Copy the row of A variable by variable, starting at the first nonzero variable.
copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables);
#ifndef NDEBUG
// Debug check, make sure the first column of nonzeros increases monotonically
if(row != 0)

4
linear/GaussianFactor.h
View File

@ -134,6 +134,8 @@ public:
/** Get a view of the A matrix for the variable pointed to be the given key iterator */
ab_type::const_block_type getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); }
ab_type::block_type getA(iterator variable) { return Ab_(variable - keys_.begin()); }
ab_type::block_type getAb(size_t block) { return Ab_(block); }
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
@ -227,6 +229,8 @@ public:
GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1);
void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; }
friend class GaussianFactorGraph;
friend class Inference;