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
21namespace gtsam { namespace partition {
22
23 /***************************************************
24 * 2D generic factors and their factor graph
25 ***************************************************/
26 enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
27
28 /** the index of the node and the type of the node */
29 struct GenericNode2D {
30 std::size_t index;
31 GenericNode2DType type;
32 GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
33 };
34
35 /** a factor always involves two nodes/variables for now */
36 struct GenericFactor2D {
37 GenericNode2D key1;
38 GenericNode2D key2;
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
48 /** graph is a collection of factors */
49 typedef std::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
50 typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
51
52 /** merge nodes in DSF using constraints captured by the given graph */
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
56 /** eliminate the sensors from generic graph */
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
62 /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
63 inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
64 WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
65
66 /** print the graph **/
67 void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
68
69 /***************************************************
70 * 3D generic factors and their factor graph
71 ***************************************************/
72 enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
73
74// const int minNrConstraintsPerCamera = 7;
75// const int minNrConstraintsPerLandmark = 2;
76
77 /** the index of the node and the type of the node */
78 struct GenericNode3D {
79 std::size_t index;
80 GenericNode3DType type;
81 GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
82 };
83
84 /** a factor always involves two nodes/variables for now */
85 struct GenericFactor3D {
86 GenericNode3D key1;
87 GenericNode3D key2;
88 int index; // the index in the entire graph, 0-based
89 int weight; // the weight of the edge
90 GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
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
96 /** graph is a collection of factors */
97 typedef std::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
98 typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
99
100 /** merge nodes in DSF using constraints captured by the given graph */
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
104 /** eliminate the sensors from generic graph */
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
108 /** check whether the 3D graph is singular (under constrained) */
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
113 /** print the graph **/
114 void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
115
116 /***************************************************
117 * unary generic factors and their factor graph
118 ***************************************************/
119 /** a factor involves a single variable */
120 struct GenericUnaryFactor {
121 GenericNode2D key;
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
129 /** graph is a collection of factors */
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
153