gtsam  4.0.0
gtsam
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 #include <boost/tuple/tuple.hpp>
28 
29 #include <iostream>
30 #include <string>
31 
32 namespace gtsam {
33 
52 class VariableSlots : public FastMap<Key, FastVector<size_t> > {
53 
54 public:
55 
57  GTSAM_EXPORT static const size_t Empty;
58 
61 
67  template<class FG>
68  VariableSlots(const FG& factorGraph);
69 
71 
74 
76  GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
77 
79  GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
80 
82 };
83 
85 template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
86 
87 /* ************************************************************************* */
88 template<class FG>
89 VariableSlots::VariableSlots(const FG& factorGraph)
90 {
91  gttic(VariableSlots_constructor);
92  static const bool debug = false;
93 
94  // Compute a mapping (called variableSlots) *from* each involved
95  // variable that will be in the new joint factor *to* the slot in each
96  // removed factor in which that variable appears. For each variable,
97  // this is stored as a vector of slot numbers, stored in order of the
98  // removed factors. The slot number is the max integer value if the
99  // factor does not involve that variable.
100  size_t jointFactorPos = 0;
101  for(const typename FG::sharedFactor& factor: factorGraph) {
102  assert(factor);
103  size_t factorVarSlot = 0;
104  for(const Key involvedVariable: *factor) {
105  // Set the slot in this factor for this variable. If the
106  // variable was not already discovered, create an array for it
107  // that we'll fill with the slot indices for each factor that
108  // we're combining. Initially we put the max integer value in
109  // the array entry for each factor that will indicate the factor
110  // does not involve the variable.
111  iterator thisVarSlots; bool inserted;
112  boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
113  if(inserted)
114  thisVarSlots->second.resize(factorGraph.size(), 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 }
VariableSlots(const FG &factorGraph)
Constructor from a set of factors to be combined.
Definition: VariableSlots.h:89
A thin wrapper around std::vector that uses a custom allocator.
GTSAM_EXPORT bool equals(const VariableSlots &rhs, double tol=0.0) const
equals
Definition: VariableSlots.cpp:52
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
Definition: FastMap.h:37
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
Included from all GTSAM files.
A combined factor is assembled as one block of rows for each component factor.
Definition: VariableSlots.h:52
A thin wrapper around std::map that uses boost's fast_pool_allocator.
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
print
Definition: VariableSlots.cpp:29
Timing utilities.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Concept check for values that can be used in unit tests.