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 #include <gtsam_unstable/dllexport.h>
18 
19 #include "PartitionWorkSpace.h"
20 
21 namespace gtsam { namespace partition {
22 
23  /***************************************************
24  * 2D generic factors and their factor graph
25  ***************************************************/
27 
29  struct GenericNode2D {
32  GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
33  };
34 
36  struct GenericFactor2D {
39  int index; // the factor index in the original nonlinear factor graph
40  int weight; // the weight of the edge
41  GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
42  : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
43  GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
44  : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
45  key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
46  };
47 
49  typedef std::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
50  typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
51 
53  std::list<std::vector<size_t> > GTSAM_UNSTABLE_EXPORT findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
54  const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
55 
57  inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
58  const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) {
59  throw std::runtime_error("reduceGenericGraph 2d not implemented");
60  }
61 
63  inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
64  WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
65 
67  void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
68 
69  /***************************************************
70  * 3D generic factors and their factor graph
71  ***************************************************/
73 
74 // const int minNrConstraintsPerCamera = 7;
75 // const int minNrConstraintsPerLandmark = 2;
76 
78  struct GenericNode3D {
81  GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
82  };
83 
85  struct GenericFactor3D {
88  int index; // the index in the entire graph, 0-based
89  int weight; // the weight of the edge
91  GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
92  const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
93  : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
94  };
95 
97  typedef std::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
98  typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
99 
101  std::list<std::vector<size_t> > GTSAM_UNSTABLE_EXPORT findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
102  const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
103 
105  void GTSAM_UNSTABLE_EXPORT reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
106  const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
107 
109  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
110  WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
111 
112 
114  void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
115 
116  /***************************************************
117  * unary generic factors and their factor graph
118  ***************************************************/
122  int index; // the factor index in the original nonlinear factor graph
123  GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
124  : key(key_, type_), index(index_) {}
125  GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
126  : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
127  };
128 
130  typedef std::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
131  typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
132 
133  /***************************************************
134  * utility functions
135  ***************************************************/
136  inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
137  if (cameras1.empty() || cameras2.empty())
138  throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
139  std::set<size_t>::const_iterator it1 = cameras1.begin();
140  std::set<size_t>::const_iterator it2 = cameras2.begin();
141  while (it1 != cameras1.end() && it2 != cameras2.end()) {
142  if (*it1 == *it2)
143  return true;
144  else if (*it1 < *it2)
145  it1++;
146  else
147  it2++;
148  }
149  return false;
150  }
151 
152 }} // namespace
gtsam::partition::reduceGenericGraph
void reduceGenericGraph(const GenericGraph3D &graph, const std::vector< size_t > &cameraKeys, const std::vector< size_t > &landmarkKeys, const std::vector< int > &dictionary, GenericGraph3D &reducedGraph)
Definition: GenericGraph.cpp:353
gtsam::partition::GenericUnaryFactor::GenericUnaryFactor
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_=-1)
Definition: GenericGraph.h:125
name
Annotation for function names.
Definition: attr.h:51
gtsam::partition::WorkSpace
Definition: PartitionWorkSpace.h:19
gtsam::partition::GenericFactor2D::index
int index
Definition: GenericGraph.h:39
gtsam::partition::GenericFactor3D::GenericFactor3D
GenericFactor3D()
Definition: GenericGraph.h:90
gtsam::partition::GenericNode3DType
GenericNode3DType
Definition: GenericGraph.h:72
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
PartitionWorkSpace.h
gtsam::partition::GenericNode2D::type
GenericNode2DType type
Definition: GenericGraph.h:31
gtsam::partition::GenericFactor3D
Definition: GenericGraph.h:85
gtsam::partition::GenericNode2D::GenericNode2D
GenericNode2D(const std::size_t &index_in, const GenericNode2DType &type_in)
Definition: GenericGraph.h:32
gtsam::partition::NODE_LANDMARK_3D
@ NODE_LANDMARK_3D
Definition: GenericGraph.h:74
type
Definition: pytypes.h:1525
gtsam::partition::NODE_LANDMARK_2D
@ NODE_LANDMARK_2D
Definition: GenericGraph.h:28
gtsam::partition::GenericUnaryFactor
Definition: GenericGraph.h:120
gtsam::partition::GenericFactor3D::key1
GenericNode3D key1
Definition: GenericGraph.h:86
gtsam::partition::GenericNode2D
Definition: GenericGraph.h:29
gtsam::partition::GenericFactor3D::index
int index
Definition: GenericGraph.h:88
gtsam::partition::GenericNode2DType
GenericNode2DType
Definition: GenericGraph.h:26
gtsam::partition::NODE_POSE_3D
@ NODE_POSE_3D
Definition: GenericGraph.h:74
gtsam::partition::hasCommonCamera
bool hasCommonCamera(const std::set< size_t > &cameras1, const std::set< size_t > &cameras2)
Definition: GenericGraph.h:136
gtsam::partition::findIslands
list< vector< size_t > > findIslands(const GenericGraph2D &graph, const vector< size_t > &keys, WorkSpace &workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
Definition: GenericGraph.cpp:23
gtsam::partition::GenericFactor2D::GenericFactor2D
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:41
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
gtsam::partition::GenericNode3D::GenericNode3D
GenericNode3D(const std::size_t &index_in, const GenericNode3DType &type_in)
Definition: GenericGraph.h:81
gtsam::partition::checkSingularity
void checkSingularity(const GenericGraph3D &graph, const std::vector< size_t > &frontals, WorkSpace &workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark)
Definition: GenericGraph.cpp:407
gtsam::partition::GenericFactor3D::weight
int weight
Definition: GenericGraph.h:89
gtsam::partition::GenericFactor2D::weight
int weight
Definition: GenericGraph.h:40
gtsam::partition::sharedGenericFactor2D
std::shared_ptr< GenericFactor2D > sharedGenericFactor2D
Definition: GenericGraph.h:49
gtsam::partition::GenericUnaryFactor::index
int index
Definition: GenericGraph.h:124
gtsam::partition::GenericUnaryGraph
std::vector< sharedGenericUnaryFactor > GenericUnaryGraph
Definition: GenericGraph.h:131
gtsam
traits
Definition: SFMdata.h:40
gtsam::partition::GenericFactor2D::GenericFactor2D
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:43
gtsam::partition::GenericFactor2D
Definition: GenericGraph.h:36
gtsam::partition::GenericFactor2D::key1
GenericNode2D key1
Definition: GenericGraph.h:37
gtsam::partition::sharedGenericFactor3D
std::shared_ptr< GenericFactor3D > sharedGenericFactor3D
Definition: GenericGraph.h:97
gtsam::partition::GenericUnaryFactor::key
GenericNode2D key
Definition: GenericGraph.h:123
gtsam::partition::GenericNode3D::type
GenericNode3DType type
Definition: GenericGraph.h:80
gtsam::partition::GenericNode3D::index
std::size_t index
Definition: GenericGraph.h:79
gtsam::partition::print
void print(const GenericGraph2D &graph, const std::string name)
Definition: GenericGraph.cpp:114
gtsam::partition::GenericGraph2D
std::vector< sharedGenericFactor2D > GenericGraph2D
Definition: GenericGraph.h:50
gtsam::partition::GenericFactor3D::key2
GenericNode3D key2
Definition: GenericGraph.h:87
gtsam::partition::GenericNode3D
Definition: GenericGraph.h:78
gtsam::partition::GenericFactor2D::key2
GenericNode2D key2
Definition: GenericGraph.h:38
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::partition::GenericGraph3D
std::vector< sharedGenericFactor3D > GenericGraph3D
Definition: GenericGraph.h:98
gtsam::partition::NODE_POSE_2D
@ NODE_POSE_2D
Definition: GenericGraph.h:28
gtsam::partition::GenericFactor3D::GenericFactor3D
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:91
gtsam::partition::GenericNode2D::index
std::size_t index
Definition: GenericGraph.h:30
gtsam::partition::sharedGenericUnaryFactor
std::shared_ptr< GenericUnaryFactor > sharedGenericUnaryFactor
Definition: GenericGraph.h:130


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