graph-inl.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, 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 
12 /*
13  * @file graph-inl.h
14  * @brief Graph algorithm using boost library
15  * @author Kai Ni
16  */
17 
18 #pragma once
19 
20 #include <stdexcept>
21 #ifdef __GNUC__
22 #pragma GCC diagnostic push
23 #pragma GCC diagnostic ignored "-Wunused-variable"
24 //#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
25 #endif
26 #include <boost/graph/breadth_first_search.hpp>
27 #ifdef __GNUC__
28 #pragma GCC diagnostic pop
29 #endif
30 #include <boost/graph/prim_minimum_spanning_tree.hpp>
31 
32 #include <gtsam/inference/graph.h>
33 
34 namespace gtsam {
35 
36 /* ************************************************************************* */
37 template <class KEY>
38 class ordering_key_visitor : public boost::default_bfs_visitor {
39 public:
40  ordering_key_visitor(std::list<KEY>& ordering_in) : ordering_(ordering_in) {}
41  template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
42  KEY key = boost::get(boost::vertex_name, g, v);
43  ordering_.push_front(key);
44  }
45  std::list<KEY>& ordering_;
46 };
47 
48 /* ************************************************************************* */
49 template<class KEY>
50 std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map) {
51  typedef typename SGraph<KEY>::Vertex SVertex;
52  const auto [g, root, key2vertex] = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
53 
54  // breadth first visit on the graph
55  std::list<KEY> keys;
56  ordering_key_visitor<KEY> vis(keys);
57  boost::breadth_first_search(g, root, boost::visitor(vis));
58  return keys;
59 }
60 
61 /* ************************************************************************* */
62 template<class G, class F, class KEY>
64  // convert the factor graph to boost graph
66  typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
67  std::map<KEY, BoostVertex> key2vertex;
68  typename G::const_iterator itFactor;
69 
70  // Loop over the factors
71  for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
72 
73  // Ignore factors that are not binary
74  if ((*itFactor)->keys().size() != 2)
75  continue;
76 
77  // Cast the factor to the user-specified factor type F
78  std::shared_ptr<F> factor = std::dynamic_pointer_cast<F>(*itFactor);
79  // Ignore factors that are not of type F
80  if (!factor) continue;
81 
82  // Retrieve the 2 keys (nodes) the factor (edge) is incident on
83  KEY key1 = factor->keys()[0];
84  KEY key2 = factor->keys()[1];
85 
86  BoostVertex v1, v2;
87 
88  // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
89  if (key2vertex.find(key1) == key2vertex.end()) {
90  v1 = add_vertex(key1, g);
91  key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
92  } else
93  v1 = key2vertex[key1];
94 
95  // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
96  if (key2vertex.find(key2) == key2vertex.end()) {
97  v2 = add_vertex(key2, g);
98  key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
99  } else
100  v2 = key2vertex[key2];
101 
102  // Add an edge with weight 1.0
103  boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
104  boost::add_edge(v1, v2, edge_property, g);
105  }
106 
107  return g;
108 }
109 
110 /* ************************************************************************* */
111 template<class G, class V, class KEY>
112 std::tuple<G, V, std::map<KEY,V> >
114 
115  G g;
116  std::map<KEY, V> key2vertex;
117  V v1, v2, root;
118  bool foundRoot = false;
119  for(const auto& [child, parent]: p_map) {
120  if (key2vertex.find(child) == key2vertex.end()) {
121  v1 = add_vertex(child, g);
122  key2vertex.emplace(child, v1);
123  } else
124  v1 = key2vertex[child];
125 
126  if (key2vertex.find(parent) == key2vertex.end()) {
127  v2 = add_vertex(parent, g);
128  key2vertex.emplace(parent, v2);
129  } else
130  v2 = key2vertex[parent];
131 
132  if (child==parent) {
133  root = v1;
134  foundRoot = true;
135  } else
136  boost::add_edge(v2, v1, g); // edge is from parent to child
137  }
138 
139  if (!foundRoot)
140  throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
141  else
142  return std::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
143 }
144 
145 /* ************************************************************************* */
146 template <class V, class POSE, class KEY>
147 class compose_key_visitor : public boost::default_bfs_visitor {
148 
149 private:
150  std::shared_ptr<Values> config_;
151 
152 public:
153 
154  compose_key_visitor(std::shared_ptr<Values> config_in) {config_ = config_in;}
155 
156  template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
157  KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
158  KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
159  POSE relativePose = boost::get(boost::edge_weight, g, edge);
160  config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
161  }
162 
163 };
164 
165 /* ************************************************************************* */
166 template<class G, class Factor, class POSE, class KEY>
167 std::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
168  const POSE& rootPose) {
169 
170  //TODO: change edge_weight_t to edge_pose_t
171  typedef typename boost::adjacency_list<
172  boost::vecS, boost::vecS, boost::directedS,
173  boost::property<boost::vertex_name_t, KEY>,
174  boost::property<boost::edge_weight_t, POSE> > PoseGraph;
175  typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
176 
177  PoseGraph g;
178  PoseVertex root;
179  std::map<KEY, PoseVertex> key2vertex;
180  std::tie(g, root, key2vertex) =
181  predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
182 
183  // attach the relative poses to the edges
184  for(typename G::sharedFactor nl_factor: graph) {
185 
186  if (nl_factor->keys().size() > 2)
187  throw std::invalid_argument("composePoses: only support factors with at most two keys");
188 
189  // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
190  std::shared_ptr<Factor> factor = std::dynamic_pointer_cast<Factor>(nl_factor);
191  if (!factor) continue;
192 
193  KEY key1 = factor->key1();
194  KEY key2 = factor->key2();
195 
196  PoseVertex v1 = key2vertex.find(key1)->second;
197  PoseVertex v2 = key2vertex.find(key2)->second;
198 
199  POSE l1Xl2 = factor->measured();
200  const auto [edge12, found1] = boost::edge(v1, v2, g);
201  const auto [edge21, found2] = boost::edge(v2, v1, g);
202  if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
203  if (!found1 && !found2) continue;
204  if (found1)
205  boost::put(boost::edge_weight, g, edge12, l1Xl2);
206  else if (found2)
207  boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse());
208  }
209 
210  // compose poses
211  std::shared_ptr<Values> config(new Values);
212  KEY rootKey = boost::get(boost::vertex_name, g, root);
213  config->insert(rootKey, rootPose);
215  boost::breadth_first_search(g, root, boost::visitor(vis));
216 
217  return config;
218 }
219 
220 /* ************************************************************************* */
221 template<class G, class KEY, class FACTOR2>
223 
224  // Convert to a graph that boost understands
225  SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
226 
227  // find minimum spanning tree
228  std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
229  prim_minimum_spanning_tree(g, &p_map[0]);
230 
231  // convert edge to string pairs
233  typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
234  for(const typename SDGraph<KEY>::Vertex& vi: p_map){
235  KEY key = boost::get(boost::vertex_name, g, *itVertex);
236  KEY parent = boost::get(boost::vertex_name, g, vi);
237  tree.insert(key, parent);
238  itVertex++;
239  }
240  return tree;
241 }
242 
243 /* ************************************************************************* */
244 template<class G, class KEY, class FACTOR2>
245 void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
246 
247  typedef typename G::sharedFactor F ;
248 
249  for(const F& factor: g)
250  {
251  if (factor->keys().size() > 2)
252  throw(std::invalid_argument("split: only support factors with at most two keys"));
253 
254  if (factor->keys().size() == 1) {
255  Ab1.push_back(factor);
256  continue;
257  }
258 
259  std::shared_ptr<FACTOR2> factor2 = std::dynamic_pointer_cast<
260  FACTOR2>(factor);
261  if (!factor2) continue;
262 
263  KEY key1 = factor2->key1();
264  KEY key2 = factor2->key2();
265  // if the tree contains the key
266  if ((tree.find(key1) != tree.end() &&
267  tree.find(key1)->second.compare(key2) == 0) ||
268  (tree.find(key2) != tree.end() &&
269  tree.find(key2)->second.compare(key1)== 0) )
270  Ab1.push_back(factor2);
271  else
272  Ab2.push_back(factor2);
273  }
274 }
275 
276 }
const gtsam::Symbol key('X', 0)
std::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Definition: graph-inl.h:167
void insert(const KEY &key, const KEY &parent)
Definition: graph.h:61
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
Definition: graph-inl.h:222
Key F(std::uint64_t j)
Vector v2
ordering_key_visitor(std::list< KEY > &ordering_in)
Definition: graph-inl.h:40
std::shared_ptr< Values > config_
Definition: graph-inl.h:150
Vector v1
JacobiRotation< float > G
void tree_edge(Edge edge, const Graph &g) const
Definition: graph-inl.h:156
NonlinearFactorGraph graph
void discover_vertex(Vertex v, const Graph &g) const
Definition: graph-inl.h:41
void g(const string &key, int i)
Definition: testBTree.cpp:41
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Definition: graph-inl.h:245
SDGraph< KEY > toBoostGraph(const G &graph)
Definition: graph-inl.h:63
const Symbol key1('v', 1)
std::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Definition: graph-inl.h:113
boost::graph_traits< SDGraph< KEY > >::vertex_descriptor Vertex
Definition: graph.h:42
Array< int, Dynamic, 1 > v
compose_key_visitor(std::shared_ptr< Values > config_in)
Definition: graph-inl.h:154
traits
Definition: chartTesting.h:28
Graph algorithm using boost library.
std::list< KEY > & ordering_
Definition: graph-inl.h:45
boost::graph_traits< SGraph< KEY > >::vertex_descriptor Vertex
Definition: graph.h:49
const KeyVector keys
Container::iterator get(Container &c, Position position)
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Definition: graph-inl.h:50
const Symbol key2('v', 2)


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