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

Formatting only

release/4.3a0
dellaert 2014-05-31 16:31:48 -04:00
parent 42edec1066
commit 16c28b2e9c
1 changed files with 28 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/inference/VariableIndex-inl.h
View File

@ -23,43 +23,35 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices) void VariableIndex::augment(const FG& factors,
{ boost::optional<const FastVector<size_t>&> newFactorIndices) {
gttic(VariableIndex_augment); gttic(VariableIndex_augment);
// Augment index for each factor // Augment index for each factor
for(size_t i = 0; i < factors.size(); ++i) for (size_t i = 0; i < factors.size(); ++i) {
{ if (factors[i]) {
if(factors[i])
{
const size_t globalI = const size_t globalI =
newFactorIndices ? newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
(*newFactorIndices)[i] : BOOST_FOREACH(const Key key, *factors[i]) {
nFactors_;
BOOST_FOREACH(const Key key, *factors[i])
{
index_[key].push_back(globalI); index_[key].push_back(globalI);
++ nEntries_; ++nEntries_;
} }
} }
// Increment factor count even if factors are null, to keep indices consistent // Increment factor count even if factors are null, to keep indices consistent
if(newFactorIndices) if (newFactorIndices) {
{ if ((*newFactorIndices)[i] >= nFactors_)
if((*newFactorIndices)[i] >= nFactors_)
nFactors_ = (*newFactorIndices)[i] + 1; nFactors_ = (*newFactorIndices)[i] + 1;
} } else {
else ++nFactors_;
{
++ nFactors_;
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ITERATOR, class FG> template<typename ITERATOR, class FG>
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
{ const FG& factors) {
gttic(VariableIndex_remove); gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor // NOTE: We intentionally do not decrement nFactors_ because the factor
@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
// one greater than the highest-numbered factor referenced in a VariableIndex. // one greater than the highest-numbered factor referenced in a VariableIndex.
ITERATOR factorIndex = firstFactor; ITERATOR factorIndex = firstFactor;
size_t i = 0; size_t i = 0;
for( ; factorIndex != lastFactor; ++factorIndex, ++i) { for (; factorIndex != lastFactor; ++factorIndex, ++i) {
if(i >= factors.size()) if (i >= factors.size())
throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); throw std::invalid_argument(
if(factors[i]) { "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) {
BOOST_FOREACH(Key j, *factors[i]) { BOOST_FOREACH(Key j, *factors[i]) {
Factors& factorEntries = internalAt(j); Factors& factorEntries = internalAt(j);
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); Factors::iterator entry = std::find(factorEntries.begin(),
if(entry == factorEntries.end()) factorEntries.end(), *factorIndex);
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); if (entry == factorEntries.end())
throw std::invalid_argument(
"Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry); factorEntries.erase(entry);
-- nEntries_; --nEntries_;
} }
} }
} }
@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ITERATOR> template<typename ITERATOR>
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for(ITERATOR key = firstKey; key != lastKey; ++key) { for (ITERATOR key = firstKey; key != lastKey; ++key) {
KeyMap::iterator entry = index_.find(*key); KeyMap::iterator entry = index_.find(*key);
if(!entry->second.empty()) if (!entry->second.empty())
throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); throw std::invalid_argument(
"Asking to remove variables from the variable index that are not unused");
index_.erase(entry); index_.erase(entry);
} }
} }