LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
partition_all.h
Go to the documentation of this file.
1 
10 /* requires:
11 #include <boost/type_traits/is_base_and_derived.hpp>
12 #include <boost/type_traits/is_same.hpp>
13 */
14 
15 namespace pr_bgl
16 {
17 
18 struct partition_all_divergent_exception {};
19 
20 template <class Graph, class CouplingMap, class TempMap>
21 void partition_all_update_directed_edge(
22  const Graph & g,
23  typename boost::graph_traits<Graph>::vertex_descriptor v_a,
24  typename boost::graph_traits<Graph>::vertex_descriptor v_b,
25  double weight_frac,
26  bool is_add,
27  CouplingMap coupling_map,
28  TempMap cs_xa, TempMap cs_by)
29 {
30  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
31  typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIter;
32  double denom;
33  if (is_add)
34  {
35  // update denominator
36  denom = exp(weight_frac)
37  - boost::get(coupling_map, std::make_pair(v_b,v_a));
38  if (denom <= 0.0)
39  throw partition_all_divergent_exception();
40  //printf("adding with denom: %f\n", denom);
41  }
42  else
43  {
44  denom = - exp(weight_frac)
45  - boost::get(coupling_map, std::make_pair(v_b,v_a));
46  }
47  // temporary storge of old c values
48  for (std::pair<VertexIter,VertexIter> vp=boost::vertices(g);
49  vp.first!=vp.second; vp.first++)
50  {
51  Vertex v = *vp.first;
52  boost::put(cs_xa,v,
53  boost::get(coupling_map, std::make_pair(v,v_a)));
54  boost::put(cs_by,v,
55  boost::get(coupling_map, std::make_pair(v_b,v)));
56  }
57  // iterate over all vertex pairs, updating coupling map
58  for (std::pair<VertexIter,VertexIter> vpx=boost::vertices(g);
59  vpx.first!=vpx.second; vpx.first++)
60  {
61  Vertex v_x = *vpx.first;
62  for (std::pair<VertexIter,VertexIter> vpy=boost::vertices(g);
63  vpy.first!=vpy.second; vpy.first++)
64  {
65  Vertex v_y = *vpy.first;
66  boost::put(coupling_map, std::make_pair(v_x,v_y),
67  boost::get(coupling_map, std::make_pair(v_x,v_y))
68  + boost::get(cs_xa,v_x)*boost::get(cs_by,v_y)/denom);
69  }
70  }
71 }
72 
73 template <class Graph, class CouplingMap, class TempMap>
74 void partition_all_update_edge(
75  const Graph & g,
76  typename boost::graph_traits<Graph>::edge_descriptor edge,
77  double weight_frac,
78  bool is_add,
79  CouplingMap coupling_map,
80  TempMap cs_xa, TempMap cs_by)
81 {
82  typedef typename boost::graph_traits<Graph>::directed_category DirCat;
83  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
84 
85  // taken from mesh_graph_generator.hpp
86  BOOST_STATIC_CONSTANT(bool, is_undirected
87  = (boost::is_base_and_derived<boost::undirected_tag, DirCat>::value
88  || boost::is_same<boost::undirected_tag, DirCat>::value));
89 
90  Vertex v_s = boost::source(edge, g);
91  Vertex v_t = boost::target(edge, g);
92 
93  // add forward edge s->t
94  partition_all_update_directed_edge(g,
95  v_s,
96  v_t,
97  weight_frac,
98  is_add,
99  coupling_map, cs_xa, cs_by);
100 
101  // add backwards edge t->s
102  if (is_undirected)
103  {
104  partition_all_update_directed_edge(g,
105  v_t,
106  v_s,
107  weight_frac,
108  is_add,
109  coupling_map, cs_xa, cs_by);
110  }
111 }
112 
113 template <class Graph, class CouplingMap>
114 void partition_all_init(
115  const Graph & g,
116  CouplingMap coupling_map)
117 {
118  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
119  typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIter;
120 
121  for (std::pair<VertexIter,VertexIter> vpx=boost::vertices(g);
122  vpx.first!=vpx.second; vpx.first++)
123  {
124  Vertex v_x = *vpx.first;
125  for (std::pair<VertexIter,VertexIter> vpy=boost::vertices(g);
126  vpy.first!=vpy.second; vpy.first++)
127  {
128  Vertex v_y = *vpy.first;
129  boost::put(coupling_map, std::make_pair(v_x,v_y),
130  (v_x == v_y) ? 1.0 : 0.0);
131  }
132  }
133 }
134 
142 template <class Graph, class WeightMap, class CouplingMap, class TempMap>
143 void partition_all(
144  const Graph & g,
145  double len_ref,
146  WeightMap weight_map,
147  CouplingMap coupling_map,
148  TempMap cs_xa, TempMap cs_by)
149 {
150  typedef typename boost::graph_traits<Graph>::edge_iterator EdgeIter;
151 
152  partition_all_init(g, coupling_map);
153 
154  std::pair<EdgeIter,EdgeIter> ep=boost::edges(g);
155  for (EdgeIter ei=ep.first; ei!=ep.second; ei++)
156  {
157  partition_all_update_edge(
158  g,
159  *ei,
160  boost::get(weight_map,*ei) / len_ref,
161  true, // is_add
162  coupling_map, cs_xa, cs_by);
163  }
164 }
165 
166 template <class Graph, class CouplingMap>
167 double partition_all_without_edge(
168  const Graph & g,
169  typename boost::graph_traits<Graph>::vertex_descriptor v_x,
170  typename boost::graph_traits<Graph>::vertex_descriptor v_y,
171  typename boost::graph_traits<Graph>::edge_descriptor edge,
172  double weight_frac,
173  CouplingMap coupling_map)
174 {
175  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
176  typedef typename boost::graph_traits<Graph>::directed_category DirCat;
177 
178  // taken from mesh_graph_generator.hpp
179  BOOST_STATIC_CONSTANT(bool, is_undirected
180  = (boost::is_base_and_derived<boost::undirected_tag, DirCat>::value
181  || boost::is_same<boost::undirected_tag, DirCat>::value));
182 
183  Vertex v_s = boost::source(edge, g);
184  Vertex v_t = boost::target(edge, g);
185 
186  if (is_undirected)
187  {
188  // lets do a two-step backup
189 
190  // original (double-prime) values
191  double cpp_xy = boost::get(coupling_map, std::make_pair(v_x,v_y));
192  double cpp_xs = boost::get(coupling_map, std::make_pair(v_x,v_s));
193  double cpp_xt = boost::get(coupling_map, std::make_pair(v_x,v_t));
194  double cpp_sy = boost::get(coupling_map, std::make_pair(v_s,v_y));
195  double cpp_ss = boost::get(coupling_map, std::make_pair(v_s,v_s));
196  double cpp_st = boost::get(coupling_map, std::make_pair(v_s,v_t));
197  double cpp_ty = boost::get(coupling_map, std::make_pair(v_t,v_y));
198  double cpp_ts = boost::get(coupling_map, std::make_pair(v_t,v_s));
199  double cpp_tt = boost::get(coupling_map, std::make_pair(v_t,v_t));
200 
201  // first, remove the edge from s(a) --> t(b)
202  // calculate some intermediate results we'll need later
203  double cp_xy = cpp_xy - (cpp_xs * cpp_ty)/(exp(weight_frac) + cpp_ts);
204  double cp_xt = cpp_xt - (cpp_xs * cpp_tt)/(exp(weight_frac) + cpp_ts);
205  double cp_sy = cpp_sy - (cpp_ss * cpp_ty)/(exp(weight_frac) + cpp_ts);
206  double cp_st = cpp_st - (cpp_ss * cpp_tt)/(exp(weight_frac) + cpp_ts);
207 
208  // next, remove the edge from b(s) --> a(t)
209  double c_xy = cp_xy - (cp_xt * cp_sy)/(exp(weight_frac) + cp_st);
210  return c_xy;
211  }
212  else
213  {
214  double cp_xy = boost::get(coupling_map, std::make_pair(v_x,v_y));
215  double cp_xs = boost::get(coupling_map, std::make_pair(v_x,v_s));
216  double cp_ty = boost::get(coupling_map, std::make_pair(v_t,v_y));
217  double cp_ts = boost::get(coupling_map, std::make_pair(v_t,v_s));
218 
219  // remove the edge from s(a) --> t(b)
220  double c_xy = cp_xy - (cp_xs * cp_ty)/(exp(weight_frac) + cp_ts);
221  return c_xy;
222  }
223 }
224 
225 class partition_all_matrix
226 {
227 public:
228  boost::numeric::ublas::matrix<double> Z;
229  boost::numeric::ublas::vector<double> Z_xa;
230  boost::numeric::ublas::vector<double> Z_by;
231 
232  partition_all_matrix(size_t num_vertices):
233  Z(num_vertices,num_vertices),
234  Z_xa(num_vertices), Z_by(num_vertices)
235  {
236  Z = boost::numeric::ublas::identity_matrix<double>(num_vertices);
237  }
238 
239  inline void add_edge(size_t va, size_t vb, double weight_frac)
240  {
241  double denom = exp(weight_frac) - Z(vb, va);
242  if (denom <= 0.0)
243  throw std::runtime_error("divergent!");
244  // copy in a-th column and b-th row
245  Z_xa = column(Z, va);
246  Z_by = row(Z, vb);
247  // do outer product
248  Z += (1.0/denom) * outer_prod(Z_xa, Z_by);
249  }
250 
251  inline void remove_edge(size_t va, size_t vb, double weight_frac)
252  {
253  double denom = exp(weight_frac) + Z(vb, va);
254  // copy in a-th column and b-th row
255  Z_xa = column(Z, va);
256  Z_by = row(Z, vb);
257  // do outer product
258  Z -= (1.0/denom) * outer_prod(Z_xa, Z_by);
259  }
260 
261  inline double without_undirected(size_t vx, size_t vy, size_t va, size_t vb, double weight_frac)
262  {
263  // lets do a two-step backup
264 
265  // original (double-prime) values
266  double cpp_xy = Z(vx, vy);
267  double cpp_xs = Z(vx, va);
268  double cpp_xt = Z(vx, vb);
269  double cpp_sy = Z(va, vy);
270  double cpp_ss = Z(va, va);
271  double cpp_st = Z(va, vb);
272  double cpp_ty = Z(vb, vy);
273  double cpp_ts = Z(vb, va);
274  double cpp_tt = Z(vb, vb);
275 
276  // first, remove the edge from s(a) --> t(b)
277  // calculate some intermediate results we'll need later
278  double cp_xy = cpp_xy - (cpp_xs * cpp_ty)/(exp(weight_frac) + cpp_ts);
279  double cp_xt = cpp_xt - (cpp_xs * cpp_tt)/(exp(weight_frac) + cpp_ts);
280  double cp_sy = cpp_sy - (cpp_ss * cpp_ty)/(exp(weight_frac) + cpp_ts);
281  double cp_st = cpp_st - (cpp_ss * cpp_tt)/(exp(weight_frac) + cpp_ts);
282 
283  // next, remove the edge from b(s) --> a(t)
284  double c_xy = cp_xy - (cp_xt * cp_sy)/(exp(weight_frac) + cp_st);
285  return c_xy;
286  }
287 };
288 
289 } // namespace pr_bgl
pr_bgl::partition_all_divergent_exception
Definition: partition_all.h:18
pr_bgl::partition_all_matrix
Definition: partition_all.h:225
Generated on Sat Sep 3 2016 01:25:38 for pr_bgl by   doxygen 1.8.6 using Read the Docs