Scatter.cpp
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 
20 #include <gtsam/linear/Scatter.h>
22 
23 #include <algorithm>
24 
25 using namespace std;
26 
27 namespace gtsam {
28 
29 /* ************************************************************************* */
30 string SlotEntry::toString() const {
31  ostringstream oss;
32  oss << "SlotEntry: key=" << key << ", dim=" << dimension;
33  return oss.str();
34 }
35 
36 Scatter::Scatter(const GaussianFactorGraph& gfg) : Scatter(gfg, Ordering()) {}
37 
38 /* ************************************************************************* */
40  const Ordering& ordering) {
41  gttic(Scatter_Constructor);
42 
43  // If we have an ordering, pre-fill the ordered variables first
44  for (Key key : ordering) {
45  add(key, 0);
46  }
47 
48  // Now, find dimensions of variables and/or extend
49  for (const auto& factor : gfg) {
50  if (!factor)
51  continue;
52 
53  // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
54  const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(factor.get());
55  if (asJacobian && asJacobian->cols() <= 1) continue;
56 
57  // loop over variables
58  for (GaussianFactor::const_iterator variable = factor->begin();
59  variable != factor->end(); ++variable) {
60  const Key key = *variable;
61  iterator it = find(key); // theoretically expensive, yet cache friendly
62  if (it!=end())
63  it->dimension = factor->getDim(variable);
64  else
65  add(key, factor->getDim(variable));
66  }
67  }
68 
69  // To keep the same behavior as before, sort the keys after the ordering
70  iterator first = begin();
71  first += ordering.size();
72  if (first != end()) std::sort(first, end());
73 }
74 
75 /* ************************************************************************* */
76 void Scatter::add(Key key, size_t dim) {
77  emplace_back(SlotEntry(key, dim));
78 }
79 
80 /* ************************************************************************* */
82  iterator it = begin();
83  while(it != end()) {
84  if (it->key == key)
85  return it;
86  ++it;
87  }
88  return it; // end()
89 }
90 
91 /* ************************************************************************* */
92 
93 } // gtsam
const gtsam::Symbol key('X', 0)
One SlotEntry stores the slot index for a variable, as well its dim.
Definition: Scatter.h:32
GTSAM_EXPORT void add(Key key, size_t dim)
Add a key/dim pair.
Definition: Scatter.cpp:76
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Definition: BFloat16.h:88
static enum @1107 ordering
#define gttic(label)
Definition: timing.h:295
iterator find(Key key)
Find the SlotEntry with the right key (linear time worst case)
Definition: Scatter.cpp:81
Linear Factor Graph where all factors are Gaussians.
Maps global variable indices to slot indices.
traits
Definition: chartTesting.h:28
static EIGEN_DEPRECATED const end_t end
GTSAM_EXPORT Scatter()
Default Constructor.
Definition: Scatter.h:52
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:36