gtsam 4.1.1
gtsam
VariableIndex-inl.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
18#pragma once
19
21#include <gtsam/base/timing.h>
22
23namespace gtsam {
24
25/* ************************************************************************* */
26template<class FG>
27void VariableIndex::augment(const FG& factors,
28 boost::optional<const FactorIndices&> newFactorIndices) {
29 gttic(VariableIndex_augment);
30
31 // Augment index for each factor
32 for (size_t i = 0; i < factors.size(); ++i) {
33 if (factors[i]) {
34 const size_t globalI =
35 newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
36 for(const Key key: *factors[i]) {
37 index_[key].push_back(globalI);
38 ++nEntries_;
39 }
40 }
41
42 // Increment factor count even if factors are null, to keep indices consistent
43 if (newFactorIndices) {
44 if ((*newFactorIndices)[i] >= nFactors_)
45 nFactors_ = (*newFactorIndices)[i] + 1;
46 } else {
47 ++nFactors_;
48 }
49 }
50}
51
52/* ************************************************************************* */
53template<typename ITERATOR, class FG>
54void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
55 const FG& factors) {
56 gttic(VariableIndex_remove);
57
58 // NOTE: We intentionally do not decrement nFactors_ because the factor
59 // indices need to remain consistent. Removing factors from a factor graph
60 // does not shift the indices of other factors. Also, we keep nFactors_
61 // one greater than the highest-numbered factor referenced in a VariableIndex.
62 ITERATOR factorIndex = firstFactor;
63 size_t i = 0;
64 for (; factorIndex != lastFactor; ++factorIndex, ++i) {
65 if (i >= factors.size())
66 throw std::invalid_argument(
67 "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
68 if (factors[i]) {
69 for(Key j: *factors[i]) {
70 FactorIndices& factorEntries = internalAt(j);
71 auto entry = std::find(factorEntries.begin(),
72 factorEntries.end(), *factorIndex);
73 if (entry == factorEntries.end())
74 throw std::invalid_argument(
75 "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
76 factorEntries.erase(entry);
77 --nEntries_;
78 }
79 }
80 }
81}
82
83/* ************************************************************************* */
84template<typename ITERATOR>
85void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
86 for (ITERATOR key = firstKey; key != lastKey; ++key) {
87 KeyMap::iterator entry = index_.find(*key);
88 if (!entry->second.empty())
89 throw std::invalid_argument(
90 "Asking to remove variables from the variable index that are not unused");
91 index_.erase(entry);
92 }
93}
94
95}
Timing utilities.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:33
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
const FactorIndices & internalAt(Key variable) const
Internal version of 'at' that asserts existence.
Definition: VariableIndex.h:172
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition: VariableIndex-inl.h:54
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
Remove unused empty variables (in debug mode verifies they are empty).
Definition: VariableIndex-inl.h:85
void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
Augment the variable index with new factors.
Definition: VariableIndex-inl.h:27