kruskal-inl.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 #pragma once
21 
22 #include <gtsam/base/DSFMap.h>
23 #include <gtsam/base/FastMap.h>
24 #include <gtsam/base/types.h>
26 
27 #include <algorithm>
28 #include <numeric>
29 #include <vector>
30 
31 namespace gtsam::utils {
32 
33 /*****************************************************************************/
34 /* Sort the 'weights' in increasing order and return the sorted order of its
35  * indices. */
36 inline std::vector<size_t> sortedIndices(const std::vector<double> &weights) {
37  // Get the number of elements in the 'weights' vector.
38  const size_t n = weights.size();
39 
40  // Create a vector of 'indices'.
41  std::vector<size_t> indices(n);
42  std::iota(indices.begin(), indices.end(), 0);
43 
44  // Sort the 'indices' based on the values in 'weights'.
45  std::stable_sort(indices.begin(), indices.end(),
46  [&weights](const size_t i0, const size_t i1) {
47  return weights[i0] < weights[i1];
48  });
49 
50  return indices;
51 }
52 
53 /****************************************************************/
54 template <class FACTOR>
55 std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
56  const std::vector<double> &weights) {
57  if (fg.size() != weights.size()) {
58  throw std::runtime_error(
59  "kruskal() failure: fg.size() != weights.size(), all factors need to "
60  "assigned a weight");
61  }
62 
63  // Create an index from variables to factor indices.
64  const VariableIndex variableIndex(fg);
65 
66  // Get indices in sort-order of the weights.
67  const std::vector<size_t> sortedIndices =
69 
70  // Create a vector to hold MST indices.
71  const size_t n = variableIndex.size();
72  std::vector<size_t> treeIndices;
73  treeIndices.reserve(n - 1);
74 
75  // Initialize disjoint-set forest to keep track of merged Keys.
76  DSFMap<Key> dsf;
77 
78  // Loop over all edges in order of increasing weight.
79  size_t count = 0;
80  for (const size_t index : sortedIndices) {
81  const auto factor = fg[index];
82 
83  // Ignore non-binary edges.
84  if (factor->size() != 2) continue;
85 
86  // Find the representatives of the sets for both the Keys in the binary
87  // factor.
88  const auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
89 
90  // Check if there is a potential loop.
91  const bool loop = (u == v);
92  if (!loop) {
93  // Merge the sets.
94  dsf.merge(u, v);
95 
96  // Add the current index to the tree.
97  treeIndices.push_back(index);
98 
99  // Break if all the Keys have been added to the tree.
100  if (++count == n - 1) break;
101  }
102  }
103  return treeIndices;
104 }
105 
106 } // namespace gtsam::utils
Typedefs for easier changing of types.
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:78
void merge(const KEY &x, const KEY &y)
Merge two sets.
Definition: DSFMap.h:87
int n
std::vector< size_t > kruskal(const FactorGraph< FACTOR > &fg, const std::vector< double > &weights)
Definition: kruskal-inl.h:55
std::vector< size_t > sortedIndices(const std::vector< double > &weights)
Definition: kruskal-inl.h:36
size_t size() const
Definition: FactorGraph.h:334
Array< int, Dynamic, 1 > v
A thin wrapper around std::map that uses boost&#39;s fast_pool_allocator.
Allow for arbitrary type in DSF.
KEY find(const KEY &key) const
Given key, find the representative key for the set in which it lives.
Definition: DSFMap.h:81


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:30