Loading...
Searching...
No Matches
VariableIndex-inl.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/timing.h>
namespace gtsam {
/* ************************************************************************* */
template<class FG>
void VariableIndex::augment(const FG& factors,
boost::optional<const FactorIndices&> newFactorIndices) {
gttic(VariableIndex_augment);
// Augment index for each factor
for (size_t i = 0; i < factors.size(); ++i) {
if (factors[i]) {
const size_t globalI =
newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
for(const Key key: *factors[i]) {
index_[key].push_back(globalI);
++nEntries_;
}
}
// Increment factor count even if factors are null, to keep indices consistent
if (newFactorIndices) {
if ((*newFactorIndices)[i] >= nFactors_)
nFactors_ = (*newFactorIndices)[i] + 1;
} else {
++nFactors_;
}
}
}
/* ************************************************************************* */
template<typename ITERATOR, class FG>
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
// indices need to remain consistent. Removing factors from a factor graph
// does not shift the indices of other factors. Also, we keep nFactors_
// one greater than the highest-numbered factor referenced in a VariableIndex.
ITERATOR factorIndex = firstFactor;
size_t i = 0;
for (; factorIndex != lastFactor; ++factorIndex, ++i) {
if (i >= factors.size())
throw std::invalid_argument(
"Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) {
for(Key j: *factors[i]) {
FactorIndices& factorEntries = internalAt(j);
auto entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex);
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);
--nEntries_;
}
}
}
}
/* ************************************************************************* */
template<typename ITERATOR>
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for (ITERATOR key = firstKey; key != lastKey; ++key) {
KeyMap::iterator entry = index_.find(*key);
if (!entry->second.empty())
throw std::invalid_argument(
"Asking to remove variables from the variable index that are not unused");
index_.erase(entry);
}
}
}
timing.h
Timing utilities.
VariableIndex.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::VariableIndex::internalAt
const FactorIndices & internalAt(Key variable) const
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:172
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition VariableIndex-inl.h:54
gtsam::VariableIndex::removeUnusedVariables
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
Remove unused empty variables (in debug mode verifies they are empty).
Definition VariableIndex-inl.h:85
gtsam::VariableIndex::augment
void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
Augment the variable index with new factors.
Definition VariableIndex-inl.h:27