SubgraphBuilder.cpp
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 
18 #include <gtsam/base/DSFVector.h>
19 #include <gtsam/base/FastMap.h>
23 #include <gtsam/linear/Errors.h>
26 #include <gtsam/base/kruskal.h>
27 
28 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
29 #include <boost/archive/text_iarchive.hpp>
30 #include <boost/archive/text_oarchive.hpp>
31 #include <boost/serialization/vector.hpp>
32 #endif
33 
34 #include <algorithm>
35 #include <cmath>
36 #include <fstream>
37 #include <iomanip>
38 #include <iostream>
39 #include <numeric> // accumulate
40 #include <queue>
41 #include <random>
42 #include <set>
43 #include <stdexcept>
44 #include <string>
45 #include <utility>
46 #include <vector>
47 
48 using std::cout;
49 using std::endl;
50 using std::ostream;
51 using std::vector;
52 
53 namespace gtsam {
54 
55 /****************************************************************************/
56 Subgraph::Subgraph(const vector<size_t> &indices) {
57  edges_.reserve(indices.size());
58  for (const size_t &index : indices) {
59  const Edge e{index, 1.0};
60  edges_.push_back(e);
61  }
62 }
63 
64 /****************************************************************************/
65 vector<size_t> Subgraph::edgeIndices() const {
66  vector<size_t> eid;
67  eid.reserve(size());
68  for (const Edge &edge : edges_) {
69  eid.push_back(edge.index);
70  }
71  return eid;
72 }
73 
74 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
75 /****************************************************************************/
76 void Subgraph::save(const std::string &fn) const {
77  std::ofstream os(fn.c_str());
78  boost::archive::text_oarchive oa(os);
79  oa << *this;
80  os.close();
81 }
82 
83 /****************************************************************************/
84 Subgraph Subgraph::load(const std::string &fn) {
85  std::ifstream is(fn.c_str());
86  boost::archive::text_iarchive ia(is);
87  Subgraph subgraph;
88  ia >> subgraph;
89  is.close();
90  return subgraph;
91 }
92 #endif
93 
94 /****************************************************************************/
95 ostream &operator<<(ostream &os, const Subgraph::Edge &edge) {
96  if (edge.weight != 1.0)
97  os << edge.index << "(" << std::setprecision(2) << edge.weight << ")";
98  else
99  os << edge.index;
100  return os;
101 }
102 
103 /****************************************************************************/
104 ostream &operator<<(ostream &os, const Subgraph &subgraph) {
105  os << "Subgraph" << endl;
106  for (const auto &e : subgraph.edges()) {
107  os << e << ", ";
108  }
109  return os;
110 }
111 
112 /*****************************************************************************/
114 
115 /***************************************************************************************/
116 void SubgraphBuilderParameters::print(ostream &os) const {
117  os << "SubgraphBuilderParameters" << endl
118  << "skeleton: " << skeletonTranslator(skeletonType) << endl
119  << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight)
120  << endl
121  << "augmentation weight: "
122  << augmentationWeightTranslator(augmentationWeight) << endl;
123 }
124 
125 /*****************************************************************************/
126 ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) {
127  p.print(os);
128  return os;
129 }
130 
131 /*****************************************************************************/
134  std::string s = src;
135  // convert to upper case
136  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
137  if (s == "NATURALCHAIN")
138  return NATURALCHAIN;
139  else if (s == "BFS")
140  return BFS;
141  else if (s == "KRUSKAL")
142  return KRUSKAL;
143  throw std::invalid_argument(
144  "SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
145  return KRUSKAL;
146 }
147 
148 /****************************************************************/
150  if (s == NATURALCHAIN)
151  return "NATURALCHAIN";
152  else if (s == BFS)
153  return "BFS";
154  else if (s == KRUSKAL)
155  return "KRUSKAL";
156  else
157  return "UNKNOWN";
158 }
159 
160 /****************************************************************/
163  std::string s = src;
164  // convert to upper case
165  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
166  if (s == "EQUAL")
167  return EQUAL;
168  else if (s == "RHS")
169  return RHS_2NORM;
170  else if (s == "LHS")
171  return LHS_FNORM;
172  else if (s == "RANDOM")
173  return RANDOM;
174  throw std::invalid_argument(
175  "SubgraphBuilderParameters::skeletonWeightTranslator undefined string " +
176  s);
177  return EQUAL;
178 }
179 
180 /****************************************************************/
182  SkeletonWeight w) {
183  if (w == EQUAL)
184  return "EQUAL";
185  else if (w == RHS_2NORM)
186  return "RHS";
187  else if (w == LHS_FNORM)
188  return "LHS";
189  else if (w == RANDOM)
190  return "RANDOM";
191  else
192  return "UNKNOWN";
193 }
194 
195 /****************************************************************/
198  const std::string &src) {
199  std::string s = src;
200  // convert to upper case
201  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
202  if (s == "SKELETON") return SKELETON;
203  // else if (s == "STRETCH") return STRETCH;
204  // else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
205  throw std::invalid_argument(
206  "SubgraphBuilder::Parameters::augmentationWeightTranslator undefined "
207  "string " +
208  s);
209  return SKELETON;
210 }
211 
212 /****************************************************************/
215  if (w == SKELETON) return "SKELETON";
216  // else if ( w == STRETCH ) return "STRETCH";
217  // else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
218  else
219  return "UNKNOWN";
220 }
221 
222 /****************************************************************/
224  const vector<double> &weights) const {
225  const SubgraphBuilderParameters &p = parameters_;
226  switch (p.skeletonType) {
228  return natural_chain(gfg);
229  break;
231  return bfs(gfg);
232  break;
234  return kruskal(gfg, weights);
235  break;
236  default:
237  std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
238  break;
239  }
240  return vector<size_t>();
241 }
242 
243 /****************************************************************/
244 vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
245  vector<size_t> unaryFactorIndices;
246  size_t index = 0;
247  for (const auto &factor : gfg) {
248  if (factor->size() == 1) {
249  unaryFactorIndices.push_back(index);
250  }
251  index++;
252  }
253  return unaryFactorIndices;
254 }
255 
256 /****************************************************************/
258  const GaussianFactorGraph &gfg) const {
259  vector<size_t> chainFactorIndices;
260  size_t index = 0;
261  for (const GaussianFactor::shared_ptr &gf : gfg) {
262  if (gf->size() == 2) {
263  const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
264  if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index);
265  }
266  index++;
267  }
268  return chainFactorIndices;
269 }
270 
271 /****************************************************************/
272 vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
273  const VariableIndex variableIndex(gfg);
274  /* start from the first key of the first factor */
275  Key seed = gfg[0]->keys()[0];
276 
277  const size_t n = variableIndex.size();
278 
279  /* each vertex has self as the predecessor */
280  vector<size_t> bfsFactorIndices;
281  bfsFactorIndices.reserve(n - 1);
282 
283  /* Initialize */
284  std::queue<size_t> q;
285  q.push(seed);
286 
287  std::set<size_t> flags;
288  flags.insert(seed);
289 
290  /* traversal */
291  while (!q.empty()) {
292  const size_t head = q.front();
293  q.pop();
294  for (const size_t index : variableIndex[head]) {
295  const GaussianFactor &gf = *gfg[index];
296  for (const size_t key : gf.keys()) {
297  if (flags.count(key) == 0) {
298  q.push(key);
299  flags.insert(key);
300  bfsFactorIndices.push_back(index);
301  }
302  }
303  }
304  }
305  return bfsFactorIndices;
306 }
307 
308 /****************************************************************/
310  const vector<double> &weights) const {
311  return utils::kruskal(gfg, weights);
312 }
313 
314 /****************************************************************/
315 vector<size_t> SubgraphBuilder::sample(const vector<double> &weights,
316  const size_t t) const {
317  std::mt19937 rng(42); // TODO(frank): allow us to use a different seed
318  WeightedSampler<std::mt19937> sampler(&rng);
319  return sampler.sampleWithoutReplacement(t, weights);
320 }
321 
322 /****************************************************************/
324  const auto &p = parameters_;
325  const auto inverse_ordering = Ordering::Natural(gfg);
326  const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
327  const size_t n = inverse_ordering.size(), m = gfg.size();
328 
329  // Make sure the subgraph preconditioner does not include more than half of
330  // the edges beyond the spanning tree, or we might as well solve the whole
331  // thing.
332  size_t numExtraEdges = n * p.augmentationFactor;
333  const size_t numRemaining = m - (n - 1);
334  numExtraEdges = std::min(numExtraEdges, numRemaining / 2);
335 
336  // Calculate weights
337  vector<double> weights = this->weights(gfg);
338 
339  // Build spanning tree.
340  const vector<size_t> tree = buildTree(gfg, weights);
341  if (tree.size() != n - 1) {
342  throw std::runtime_error(
343  "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
344  }
345 
346  // Downweight the tree edges to zero.
347  for (const size_t index : tree) {
348  weights[index] = 0.0;
349  }
350 
351  /* decide how many edges to augment */
352  vector<size_t> offTree = sample(weights, numExtraEdges);
353 
354  vector<size_t> subgraph = unary(gfg);
355  subgraph.insert(subgraph.end(), tree.begin(), tree.end());
356  subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
357 
358  return Subgraph(subgraph);
359 }
360 
361 /****************************************************************/
363  const GaussianFactorGraph &gfg) const {
364 
365  const size_t m = gfg.size();
366  Weights weight;
367  weight.reserve(m);
368 
369  for (const GaussianFactor::shared_ptr &gf : gfg) {
370  switch (parameters_.skeletonWeight) {
372  weight.push_back(1.0);
373  break;
376  std::dynamic_pointer_cast<JacobianFactor>(gf)) {
377  weight.push_back(jf->getb().norm());
378  } else if (HessianFactor::shared_ptr hf =
379  std::dynamic_pointer_cast<HessianFactor>(gf)) {
380  weight.push_back(hf->linearTerm().norm());
381  }
382  } break;
385  std::dynamic_pointer_cast<JacobianFactor>(gf)) {
386  weight.push_back(std::sqrt(jf->getA().squaredNorm()));
387  } else if (HessianFactor::shared_ptr hf =
388  std::dynamic_pointer_cast<HessianFactor>(gf)) {
389  weight.push_back(std::sqrt(hf->information().squaredNorm()));
390  }
391  } break;
392 
394  weight.push_back(std::rand() % 100 + 1.0);
395  break;
396 
397  default:
398  throw std::invalid_argument(
399  "SubgraphBuilder::weights: undefined weight scheme ");
400  break;
401  }
402  }
403  return weight;
404 }
405 
406 /*****************************************************************************/
408  const Subgraph &subgraph,
409  const bool clone) {
410  GaussianFactorGraph subgraphFactors;
411  subgraphFactors.reserve(subgraph.size());
412  for (const auto &e : subgraph) {
413  const auto factor = gfg[e.index];
414  subgraphFactors.push_back(clone ? factor->clone() : factor);
415  }
416  return subgraphFactors;
417 }
418 
419 /**************************************************************************************************/
420 std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
421  const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
422  // Get the subgraph by calling cheaper method
423  auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
424 
425  // Now, copy all factors then set subGraph factors to zero
426  GaussianFactorGraph remaining = factorGraph;
427 
428  for (const auto &e : subgraph) {
429  remaining.remove(e.index);
430  }
431 
432  return {subgraphFactors, remaining};
433 }
434 
435 /*****************************************************************************/
436 
437 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
enum gtsam::SubgraphBuilderParameters::Skeleton skeletonType
Matrix3f m
const Edges & edges() const
std::vector< size_t > sample(const std::vector< double > &weights, const size_t t) const
vector of errors
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:78
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
#define min(a, b)
Definition: datatypes.h:19
std::vector< size_t > buildTree(const GaussianFactorGraph &gfg, const std::vector< double > &weights) const
std::vector< size_t > kruskal(const GaussianFactorGraph &gfg, const std::vector< double > &weights) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
static std::mt19937 rng
int n
Fast sampling without replacement.
void remove(size_t i)
Definition: FactorGraph.h:393
std::vector< size_t > kruskal(const FactorGraph< FACTOR > &fg, const std::vector< double > &weights)
Definition: kruskal-inl.h:55
A faster implementation for DSF, which uses vector rather than btree.
size_t size() const
Definition: FactorGraph.h:334
std::vector< double > Weights
std::vector< size_t > unary(const GaussianFactorGraph &gfg) const
std::vector< size_t > natural_chain(const GaussianFactorGraph &gfg) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
Linear Factor Graph where all factors are Gaussians.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
std::shared_ptr< This > shared_ptr
shared_ptr to this class
size_t size() const
RowVector3d w
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
virtual Subgraph operator()(const GaussianFactorGraph &jfg) const
A thin wrapper around std::map that uses boost&#39;s fast_pool_allocator.
EdgeIndices edgeIndices() const
ofstream os("timeSchurFactors.csv")
Weights weights(const GaussianFactorGraph &gfg) const
std::vector< size_t > sampleWithoutReplacement(size_t numSamples, const std::vector< double > &weights)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
static AugmentationWeight augmentationWeightTranslator(const std::string &s)
float * p
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
void reserve(size_t size)
Definition: FactorGraph.h:186
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
static Skeleton skeletonTranslator(const std::string &s)
friend std::ostream & operator<<(std::ostream &os, const Subgraph &subgraph)
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Point2 t(10, 10)
std::vector< size_t > bfs(const GaussianFactorGraph &gfg) const
static SkeletonWeight skeletonWeightTranslator(const std::string &s)


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