gtsam  4.1.0
gtsam
graph-inl.h
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 
52  typedef typename SGraph<KEY>::Vertex SVertex;
53 
54  SGraph<KEY> g;
55  SVertex root;
56  std::map<KEY, SVertex> key2vertex;
57  boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
58 
59  // breadth first visit on the graph
60  std::list<KEY> keys;
61  ordering_key_visitor<KEY> vis(keys);
62  boost::breadth_first_search(g, root, boost::visitor(vis));
63  return keys;
64 }
65 
66 /* ************************************************************************* */
67 template<class G, class F, class KEY>
68 SDGraph<KEY> toBoostGraph(const G& graph) {
69  // convert the factor graph to boost graph
70  SDGraph<KEY> g;
71  typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
72  std::map<KEY, BoostVertex> key2vertex;
73  typename G::const_iterator itFactor;
74 
75  // Loop over the factors
76  for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
77 
78  // Ignore factors that are not binary
79  if ((*itFactor)->keys().size() != 2)
80  continue;
81 
82  // Cast the factor to the user-specified factor type F
83  boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
84  // Ignore factors that are not of type F
85  if (!factor) continue;
86 
87  // Retrieve the 2 keys (nodes) the factor (edge) is incident on
88  KEY key1 = factor->keys()[0];
89  KEY key2 = factor->keys()[1];
90 
91  BoostVertex v1, v2;
92 
93  // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
94  if (key2vertex.find(key1) == key2vertex.end()) {
95  v1 = add_vertex(key1, g);
96  key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
97  } else
98  v1 = key2vertex[key1];
99 
100  // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
101  if (key2vertex.find(key2) == key2vertex.end()) {
102  v2 = add_vertex(key2, g);
103  key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
104  } else
105  v2 = key2vertex[key2];
106 
107  // Add an edge with weight 1.0
108  boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
109  boost::add_edge(v1, v2, edge_property, g);
110  }
111 
112  return g;
113 }
114 
115 /* ************************************************************************* */
116 template<class G, class V, class KEY>
117 boost::tuple<G, V, std::map<KEY,V> >
119 
120  G g;
121  std::map<KEY, V> key2vertex;
122  V v1, v2, root;
123  bool foundRoot = false;
124  for(auto child_parent: p_map) {
125  KEY child, parent;
126  std::tie(child,parent) = child_parent;
127  if (key2vertex.find(child) == key2vertex.end()) {
128  v1 = add_vertex(child, g);
129  key2vertex.insert(std::make_pair(child, v1));
130  } else
131  v1 = key2vertex[child];
132 
133  if (key2vertex.find(parent) == key2vertex.end()) {
134  v2 = add_vertex(parent, g);
135  key2vertex.insert(std::make_pair(parent, v2));
136  } else
137  v2 = key2vertex[parent];
138 
139  if (child==parent) {
140  root = v1;
141  foundRoot = true;
142  } else
143  boost::add_edge(v2, v1, g); // edge is from parent to child
144  }
145 
146  if (!foundRoot)
147  throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
148  else
149  return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
150 }
151 
152 /* ************************************************************************* */
153 template <class V, class POSE, class KEY>
154 class compose_key_visitor : public boost::default_bfs_visitor {
155 
156 private:
157  boost::shared_ptr<Values> config_;
158 
159 public:
160 
161  compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
162 
163  template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
164  KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
165  KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
166  POSE relativePose = boost::get(boost::edge_weight, g, edge);
167  config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
168  }
169 
170 };
171 
172 /* ************************************************************************* */
173 template<class G, class Factor, class POSE, class KEY>
174 boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
175  const POSE& rootPose) {
176 
177  //TODO: change edge_weight_t to edge_pose_t
178  typedef typename boost::adjacency_list<
179  boost::vecS, boost::vecS, boost::directedS,
180  boost::property<boost::vertex_name_t, KEY>,
181  boost::property<boost::edge_weight_t, POSE> > PoseGraph;
182  typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
183  typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
184 
185  PoseGraph g;
186  PoseVertex root;
187  std::map<KEY, PoseVertex> key2vertex;
188  boost::tie(g, root, key2vertex) =
189  predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
190 
191  // attach the relative poses to the edges
192  PoseEdge edge12, edge21;
193  bool found1, found2;
194  for(typename G::sharedFactor nl_factor: graph) {
195 
196  if (nl_factor->keys().size() > 2)
197  throw std::invalid_argument("composePoses: only support factors with at most two keys");
198 
199  // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
200  boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
201  if (!factor) continue;
202 
203  KEY key1 = factor->key1();
204  KEY key2 = factor->key2();
205 
206  PoseVertex v1 = key2vertex.find(key1)->second;
207  PoseVertex v2 = key2vertex.find(key2)->second;
208 
209  POSE l1Xl2 = factor->measured();
210  boost::tie(edge12, found1) = boost::edge(v1, v2, g);
211  boost::tie(edge21, found2) = boost::edge(v2, v1, g);
212  if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
213  if (!found1 && !found2) continue;
214  if (found1)
215  boost::put(boost::edge_weight, g, edge12, l1Xl2);
216  else if (found2)
217  boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse());
218  }
219 
220  // compose poses
221  boost::shared_ptr<Values> config(new Values);
222  KEY rootKey = boost::get(boost::vertex_name, g, root);
223  config->insert(rootKey, rootPose);
225  boost::breadth_first_search(g, root, boost::visitor(vis));
226 
227  return config;
228 }
229 
230 /* ************************************************************************* */
231 template<class G, class KEY, class FACTOR2>
233 
234  // Convert to a graph that boost understands
235  SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
236 
237  // find minimum spanning tree
238  std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
239  prim_minimum_spanning_tree(g, &p_map[0]);
240 
241  // convert edge to string pairs
242  PredecessorMap<KEY> tree;
243  typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
244  for(const typename SDGraph<KEY>::Vertex& vi: p_map){
245  KEY key = boost::get(boost::vertex_name, g, *itVertex);
246  KEY parent = boost::get(boost::vertex_name, g, vi);
247  tree.insert(key, parent);
248  itVertex++;
249  }
250  return tree;
251 }
252 
253 /* ************************************************************************* */
254 template<class G, class KEY, class FACTOR2>
255 void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
256 
257  typedef typename G::sharedFactor F ;
258 
259  for(const F& factor: g)
260  {
261  if (factor->keys().size() > 2)
262  throw(std::invalid_argument("split: only support factors with at most two keys"));
263 
264  if (factor->keys().size() == 1) {
265  Ab1.push_back(factor);
266  continue;
267  }
268 
269  boost::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
270  FACTOR2>(factor);
271  if (!factor2) continue;
272 
273  KEY key1 = factor2->key1();
274  KEY key2 = factor2->key2();
275  // if the tree contains the key
276  if ((tree.find(key1) != tree.end() &&
277  tree.find(key1)->second.compare(key2) == 0) ||
278  (tree.find(key2) != tree.end() &&
279  tree.find(key2)->second.compare(key1)== 0) )
280  Ab1.push_back(factor2);
281  else
282  Ab2.push_back(factor2);
283  }
284 }
285 
286 }
gtsam::PredecessorMap
Map from variable key to parent key.
Definition: graph.h:58
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
find the minimum spanning tree using boost graph library
Definition: graph-inl.h:232
gtsam::compose_key_visitor
Definition: graph-inl.h:154
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Build takes a predecessor map, and builds a directed graph corresponding to the tree.
Definition: graph-inl.h:118
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Compose the poses by following the chain specified by the spanning tree.
Definition: graph-inl.h:174
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition: graph-inl.h:255
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Generate a list of keys from a spanning tree represented by its predecessor map.
Definition: graph-inl.h:50
gtsam::toBoostGraph
SDGraph< KEY > toBoostGraph(const G &graph)
Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
Definition: graph-inl.h:68
gtsam::SGraph
Definition: graph.h:47
gtsam::ordering_key_visitor
Definition: graph-inl.h:38
gtsam::SDGraph
SDGraph is undirected graph with variable keys and double edge weights.
Definition: graph.h:40
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
graph.h
Graph algorithm using boost library.
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
convenience insert so we can pass ints for TypedSymbol keys
Definition: graph.h:61