gtsam  4.1.0
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 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
26 template<class FG>
27 void 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 /* ************************************************************************* */
53 template<typename ITERATOR, class FG>
54 void 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 /* ************************************************************************* */
84 template<typename ITERATOR>
85 void 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.h
Timing utilities.
VariableIndex.h
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
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::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::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