VariableSlots.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 
19 #pragma once
20 
21 #include <gtsam/global_includes.h>
22 #include <gtsam/base/FastMap.h>
23 #include <gtsam/base/FastVector.h>
24 #include <gtsam/base/timing.h>
25 #include <gtsam/base/Testable.h>
26 
27 
28 #include <iostream>
29 #include <string>
30 
31 namespace gtsam {
32 
51 class VariableSlots : public FastMap<Key, FastVector<size_t> > {
52 
53 public:
54 
56  GTSAM_EXPORT static const size_t Empty;
57 
60 
66  template<class FG>
67  VariableSlots(const FG& factorGraph);
68 
70 
73 
75  GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
76 
78  GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
79 
81 };
82 
84 template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
85 
86 /* ************************************************************************* */
87 template<class FG>
88 VariableSlots::VariableSlots(const FG& factorGraph)
89 {
90  gttic(VariableSlots_constructor);
91  static const bool debug = false;
92 
93  // Compute a mapping (called variableSlots) *from* each involved
94  // variable that will be in the new joint factor *to* the slot in each
95  // removed factor in which that variable appears. For each variable,
96  // this is stored as a vector of slot numbers, stored in order of the
97  // removed factors. The slot number is the max integer value if the
98  // factor does not involve that variable.
99  size_t jointFactorPos = 0;
100  for(const typename FG::sharedFactor& factor: factorGraph) {
101  if (!factor) {
102  continue;
103  }
104  size_t factorVarSlot = 0;
105  for(const Key involvedVariable: *factor) {
106  // Set the slot in this factor for this variable. If the
107  // variable was not already discovered, create an array for it
108  // that we'll fill with the slot indices for each factor that
109  // we're combining. Initially we put the max integer value in
110  // the array entry for each factor that will indicate the factor
111  // does not involve the variable.
112  auto [thisVarSlots, inserted] = this->insert({involvedVariable, FastVector<size_t>()});
113  if(inserted)
114  thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
115  thisVarSlots->second[jointFactorPos] = factorVarSlot;
116  if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
117  ++ factorVarSlot;
118  }
119  ++ jointFactorPos;
120  }
121 }
122 
123 }
A insert(1, 2)=0
Concept check for values that can be used in unit tests.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
static constexpr bool debug
GTSAM_EXPORT bool equals(const VariableSlots &rhs, double tol=0.0) const
Included from all GTSAM files.
#define gttic(label)
Definition: timing.h:295
FastMap< Key, FastVector< size_t > > Base
Definition: VariableSlots.h:55
Definition: pytypes.h:1403
static GTSAM_EXPORT const size_t Empty
Definition: VariableSlots.h:56
VariableSlots(const FG &factorGraph)
Definition: VariableSlots.h:88
A thin wrapper around std::vector that uses a custom allocator.
traits
Definition: chartTesting.h:28
A thin wrapper around std::map that uses boost&#39;s fast_pool_allocator.
GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
const G double tol
Definition: Group.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Timing utilities.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43