GenericGraph.h
Go to the documentation of this file.
1 /*
2  * GenericGraph.h
3  *
4  * Created on: Nov 22, 2010
5  * Author: nikai
6  * Description: generic graph types used in partitioning
7  */
8 
9 #pragma once
10 
11 #include <set>
12 #include <list>
13 #include <vector>
14 #include <stdexcept>
15 #include <string>
16 #include <memory>
17 
18 #include "PartitionWorkSpace.h"
19 
20 namespace gtsam { namespace partition {
21 
22  /***************************************************
23  * 2D generic factors and their factor graph
24  ***************************************************/
26 
28  struct GenericNode2D {
31  GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
32  };
33 
35  struct GenericFactor2D {
38  int index; // the factor index in the original nonlinear factor graph
39  int weight; // the weight of the edge
40  GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
41  : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
42  GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
43  : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
44  key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
45  };
46 
48  typedef std::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
49  typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
50 
52  std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
53  const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
54 
56  inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
57  const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) {
58  throw std::runtime_error("reduceGenericGraph 2d not implemented");
59  }
60 
62  inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
63  WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
64 
66  void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
67 
68  /***************************************************
69  * 3D generic factors and their factor graph
70  ***************************************************/
72 
73 // const int minNrConstraintsPerCamera = 7;
74 // const int minNrConstraintsPerLandmark = 2;
75 
77  struct GenericNode3D {
80  GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
81  };
82 
84  struct GenericFactor3D {
87  int index; // the index in the entire graph, 0-based
88  int weight; // the weight of the edge
89  GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
90  GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
91  const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
92  : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
93  };
94 
96  typedef std::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
97  typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
98 
100  std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
101  const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
102 
104  void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
105  const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
106 
108  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
109  WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
110 
111 
113  void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
114 
115  /***************************************************
116  * unary generic factors and their factor graph
117  ***************************************************/
121  int index; // the factor index in the original nonlinear factor graph
122  GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
123  : key(key_, type_), index(index_) {}
124  GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
125  : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
126  };
127 
129  typedef std::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
130  typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
131 
132  /***************************************************
133  * utility functions
134  ***************************************************/
135  inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
136  if (cameras1.empty() || cameras2.empty())
137  throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
138  std::set<size_t>::const_iterator it1 = cameras1.begin();
139  std::set<size_t>::const_iterator it2 = cameras2.begin();
140  while (it1 != cameras1.end() && it2 != cameras2.end()) {
141  if (*it1 == *it2)
142  return true;
143  else if (*it1 < *it2)
144  it1++;
145  else
146  it2++;
147  }
148  return false;
149  }
150 
151 }} // namespace
list< vector< size_t > > findIslands(const GenericGraph2D &graph, const vector< size_t > &keys, WorkSpace &workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
void checkSingularity(const GenericGraph3D &graph, const std::vector< size_t > &frontals, WorkSpace &workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark)
bool hasCommonCamera(const std::set< size_t > &cameras1, const std::set< size_t > &cameras2)
Definition: GenericGraph.h:135
GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_=-1, const int weight_=1)
Definition: GenericGraph.h:40
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_=-1)
Definition: GenericGraph.h:122
std::shared_ptr< GenericFactor2D > sharedGenericFactor2D
Definition: GenericGraph.h:48
std::shared_ptr< GenericFactor3D > sharedGenericFactor3D
Definition: GenericGraph.h:96
std::vector< sharedGenericUnaryFactor > GenericUnaryGraph
Definition: GenericGraph.h:130
NonlinearFactorGraph graph
void print(const GenericGraph2D &graph, const std::string name)
std::vector< sharedGenericFactor2D > GenericGraph2D
Definition: GenericGraph.h:49
std::shared_ptr< GenericUnaryFactor > sharedGenericUnaryFactor
Definition: GenericGraph.h:129
std::vector< sharedGenericFactor3D > GenericGraph3D
Definition: GenericGraph.h:97
GenericUnaryFactor(const size_t key_, const char type_, const int index_=-1)
Definition: GenericGraph.h:124
void reduceGenericGraph(const GenericGraph3D &graph, const std::vector< size_t > &cameraKeys, const std::vector< size_t > &landmarkKeys, const std::vector< int > &dictionary, GenericGraph3D &reducedGraph)
GenericNode3D(const std::size_t &index_in, const GenericNode3DType &type_in)
Definition: GenericGraph.h:80
traits
Definition: chartTesting.h:28
GenericFactor3D(const size_t index1, const size_t index2, const int index_=-1, const GenericNode3DType type1=NODE_POSE_3D, const GenericNode3DType type2=NODE_LANDMARK_3D, const int weight_=1)
Definition: GenericGraph.h:90
GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_=-1, const int weight_=1)
Definition: GenericGraph.h:42
Annotation for function names.
Definition: attr.h:48
const KeyVector keys
GenericNode2D(const std::size_t &index_in, const GenericNode2DType &type_in)
Definition: GenericGraph.h:31


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