DSFVector.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 
19 #include <gtsam/base/DSFVector.h>
20 #include <algorithm>
21 #include <cassert>
22 
23 using namespace std;
24 
25 namespace gtsam {
26 
27 /* ************************************************************************* */
28 DSFBase::DSFBase(const size_t numNodes) {
29  v_ = std::make_shared < V > (numNodes);
30  int index = 0;
31  for (V::iterator it = v_->begin(); it != v_->end(); it++, index++)
32  *it = index;
33 }
34 
35 /* ************************************************************************* */
36 DSFBase::DSFBase(const std::shared_ptr<V>& v_in) {
37  v_ = v_in;
38  int index = 0;
39  for (V::iterator it = v_->begin(); it != v_->end(); it++, index++)
40  *it = index;
41 }
42 
43 /* ************************************************************************* */
44 size_t DSFBase::find(size_t key) const {
45  // follow parent pointers until we reach set representative
46  size_t parent = (*v_)[key];
47  if (parent != key)
48  parent = find(parent); // recursive call
49  (*v_)[key] = parent; // path compression
50  return parent;
51 }
52 
53 /* ************************************************************************* */
54 void DSFBase::merge(const size_t& i1, const size_t& i2) {
55  (*v_)[find(i2)] = find(i1);
56 }
57 
58 /* ************************************************************************* */
59 DSFVector::DSFVector(const size_t numNodes) :
60  DSFBase(numNodes) {
61  keys_.reserve(numNodes);
62  for (size_t index = 0; index < numNodes; index++)
63  keys_.push_back(index);
64 }
65 
66 /* ************************************************************************* */
67 DSFVector::DSFVector(const std::vector<size_t>& keys) :
68  DSFBase(1 + *std::max_element(keys.begin(), keys.end())), keys_(keys) {
69 }
70 
71 /* ************************************************************************* */
72 DSFVector::DSFVector(const std::shared_ptr<V>& v_in,
73  const std::vector<size_t>& keys) :
74  DSFBase(v_in), keys_(keys) {
75  assert(*(std::max_element(keys.begin(), keys.end()))<v_in->size());
76 }
77 
78 /* ************************************************************************* */
79 bool DSFVector::isSingleton(const size_t& label) const {
80  bool result = false;
81  for(size_t key: keys_) {
82  if (find(key) == label) {
83  if (!result) // find the first occurrence
84  result = true;
85  else
86  return false;
87  }
88  }
89  return result;
90 }
91 
92 /* ************************************************************************* */
93 std::set<size_t> DSFVector::set(const size_t& label) const {
94  std::set < size_t > set;
95  for(size_t key: keys_)
96  if (find(key) == label)
97  set.insert(key);
98  return set;
99 }
100 
101 /* ************************************************************************* */
102 std::map<size_t, std::set<size_t> > DSFVector::sets() const {
103  std::map<size_t, std::set<size_t> > sets;
104  for(size_t key: keys_)
105  sets[find(key)].insert(key);
106  return sets;
107 }
108 
109 /* ************************************************************************* */
110 std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
111  std::map<size_t, std::vector<size_t> > arrays;
112  for(size_t key: keys_)
113  arrays[find(key)].push_back(key);
114  return arrays;
115 }
116 
117 } // namespace gtsam
118 
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::DSFVector::DSFVector
DSFVector(const size_t numNodes)
Constructor that allocates new memory, uses sequential keys 0...numNodes-1.
Definition: DSFVector.cpp:59
gtsam::DSFVector::isSingleton
bool isSingleton(const size_t &label) const
Find whether there is one and only one occurrence for the given {label}.
Definition: DSFVector.cpp:79
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::DSFVector::arrays
std::map< size_t, std::vector< size_t > > arrays() const
Return all sets, i.e. a partition of all elements.
Definition: DSFVector.cpp:110
DSFVector.h
A faster implementation for DSF, which uses vector rather than btree.
gtsam::DSFVector::set
std::set< size_t > set(const size_t &label) const
Get the nodes in the tree with the given label.
Definition: DSFVector.cpp:93
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
i1
double i1(double x)
Definition: i1.c:150
std
Definition: BFloat16.h:88
gtsam::DSFVector::sets
std::map< size_t, std::set< size_t > > sets() const
Return all sets, i.e. a partition of all elements.
Definition: DSFVector.cpp:102
gtsam::DSFBase
Definition: DSFVector.h:38
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
gtsam::DSFVector::keys_
std::vector< size_t > keys_
stores keys to support more expensive operations
Definition: DSFVector.h:67
gtsam::DSFBase::find
size_t find(size_t key) const
Find the label of the set in which {key} lives.
Definition: DSFVector.cpp:44


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:14