LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
lazysp_selector_partition_all.h
Go to the documentation of this file.
1 
9 namespace pr_bgl
10 {
11 
27 template <class Graph, class WLazyMap>
29 {
30 public:
31  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
32  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
33  typedef typename boost::graph_traits<Graph>::edge_iterator EdgeIter;
34 
35  typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VerIndexMap;
37  typedef boost::vector_property_map<double,VerIndexMap> VerVector;
38  typedef boost::vector_property_map<double,VerPairIndexMap> VerPairVector;
39 
40  const Graph & g;
41  WLazyMap w_lazy_map;
42  const double len_ref;
43  const Vertex v_start;
44  const Vertex v_goal;
45  const bool do_fake_roots;
46 
47  // coupling temps,outputs
48  VerVector temp1;
49  VerVector temp2;
50  VerPairVector coupling_map;
51 
53  const Graph & g,
54  WLazyMap w_lazy_map, double len_ref,
55  Vertex v_start, Vertex v_goal,
56  bool do_fake_roots):
57  g(g), w_lazy_map(w_lazy_map), len_ref(len_ref),
58  v_start(v_start), v_goal(v_goal),
59  do_fake_roots(do_fake_roots),
60  temp1(num_vertices(g),get(boost::vertex_index,g)),
61  temp2(num_vertices(g),get(boost::vertex_index,g)),
62  coupling_map(
63  num_vertices(g)*num_vertices(g),
64  VerPairIndexMap(get(boost::vertex_index,g),num_vertices(g)))
65  {
66  printf("computing partition_all from scratch ...\n");
67 
68  // compute coupling from scratch
69  pr_bgl::partition_all_init(g, coupling_map);
70 
71  std::pair<EdgeIter,EdgeIter> ep=edges(g);
72  for (EdgeIter ei=ep.first; ei!=ep.second; ei++)
73  {
74  Vertex v_s = source(*ei, g);
75  Vertex v_t = target(*ei, g);
76  double weight_frac = get(w_lazy_map,*ei) / len_ref;
77 
78  if (!do_fake_roots || (v_t != v_start && v_s != v_goal))
79  {
80  // add forward edge s->t
81  partition_all_update_directed_edge(g,
82  v_s,
83  v_t,
84  weight_frac,
85  true, // is_add
86  coupling_map, temp1, temp2);
87  }
88 
89  if (!do_fake_roots || (v_s != v_start && v_t != v_goal))
90  {
91  // add backwards edge t->s
92  partition_all_update_directed_edge(g,
93  v_t,
94  v_s,
95  weight_frac,
96  true, // is_add
97  coupling_map, temp1, temp2);
98  }
99  }
100  }
101 
102  void get_to_evaluate(
103  const Graph & g,
104  const std::vector< std::pair<Edge,bool> > & path,
105  std::vector<Edge> & to_evaluate)
106  {
107  double coupling_withall = coupling_map[std::make_pair(v_start,v_goal)];
108  //printf("start-goal coupling: %e\n", coupling_withall);
109 
110  double score_best = 0.0;
111  Edge e_best;
112  bool found_best = false;
113  for (unsigned int ui=0; ui<path.size(); ui++)
114  {
115  Edge e = path[ui].first;
116  if (path[ui].second)
117  continue;
118 
119  double score;
120  if (do_fake_roots && (ui==0 || ui==path.size()-1))
121  score = 1.0;
122  else
123  {
124  // compute leave-one-out coupling score
125  double coupling_without = pr_bgl::partition_all_without_edge(g, v_start, v_goal,
126  e, get(w_lazy_map,e)/len_ref, coupling_map);
127  //printf(" path[%u] edge coupling without: %e\n", ui, coupling_without);
128  score = 1.0 - coupling_without/coupling_withall;
129  }
130 
131  // is it better?
132  if (score_best <= score)
133  {
134  e_best = e;
135  score_best = score;
136  found_best = true;
137  }
138  }
139  if (!found_best)
140  throw std::runtime_error("no best edge found!");
141  to_evaluate.push_back(e_best);
142  }
143 
144  template <class Edge, class WeightType>
145  void update_notify(Edge e, WeightType e_weight_old)
146  {
147  //printf("accommodating updated edge ...\n");
148  double e_weight_new = get(w_lazy_map, e);
149  if (e_weight_new == e_weight_old)
150  return;
151 
152  // remove in both directions (assume above check skips fake roots)
153  Vertex v_s = source(e, g);
154  Vertex v_t = target(e, g);
155 
156  double weight_frac_old = e_weight_old / len_ref;
157  if (!do_fake_roots || (v_t != v_start && v_s != v_goal))
158  partition_all_update_directed_edge(g, v_s, v_t, weight_frac_old, false, coupling_map, temp1, temp2);
159  if (!do_fake_roots || (v_s != v_start && v_t != v_goal))
160  partition_all_update_directed_edge(g, v_t, v_s, weight_frac_old, false, coupling_map, temp1, temp2);
161 
162  double weight_frac = get(w_lazy_map, e) / len_ref;
163  if (!do_fake_roots || (v_t != v_start && v_s != v_goal))
164  partition_all_update_directed_edge(g, v_s, v_t, weight_frac, true, coupling_map, temp1, temp2);
165  if (!do_fake_roots || (v_s != v_start && v_t != v_goal))
166  partition_all_update_directed_edge(g, v_t, v_s, weight_frac, true, coupling_map, temp1, temp2);
167  }
168 };
169 
173 template <class Graph, class WLazyMap>
175 {
176 public:
177  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
178  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
179 
180 
181  const Graph & g;
182  WLazyMap w_lazy_map;
183  const double len_ref;
184  const Vertex v_start;
185  const Vertex v_goal;
186  const bool do_fake_roots;
187 
189 
191  const Graph & g,
192  WLazyMap w_lazy_map, double len_ref,
193  Vertex v_start, Vertex v_goal,
194  bool do_fake_roots,
196  g(g), w_lazy_map(w_lazy_map), len_ref(len_ref),
197  v_start(v_start), v_goal(v_goal),
198  do_fake_roots(do_fake_roots),
199  solver(solver)
200  {
201  }
202 
203  void get_to_evaluate(
204  const Graph & g,
205  const std::vector< std::pair<Edge,bool> > & path,
206  std::vector<Edge> & to_evaluate)
207  {
208  double coupling_withall = solver.Z(v_start, v_goal);
209  //printf("start-goal coupling: %e\n", coupling_withall);
210 
211  double score_best = 0.0;
212  Edge e_best;
213  bool found_best = false;
214  for (unsigned int ui=0; ui<path.size(); ui++)
215  {
216  Edge e = path[ui].first;
217  if (path[ui].second)
218  continue;
219 
220  double score;
221  if (do_fake_roots && (ui==0 || ui==path.size()-1))
222  score = 1.0;
223  else
224  {
225  double weight_frac = get(w_lazy_map,e)/len_ref;
226  double coupling_without = solver.without_undirected(
227  v_start, v_goal, source(e,g), target(e,g), weight_frac);
228 
229  //printf(" path[%u] edge coupling without: %e\n", ui, coupling_without);
230  score = 1.0 - coupling_without/coupling_withall;
231  }
232 
233  // is it better?
234  if (score_best <= score)
235  {
236  e_best = e;
237  score_best = score;
238  found_best = true;
239  }
240  }
241  if (!found_best)
242  throw std::runtime_error("no best edge found!");
243  to_evaluate.push_back(e_best);
244  }
245 
246  template <class Edge, class WeightType>
247  void update_notify(Edge e, WeightType e_weight_old)
248  {
249  //printf("accommodating updated edge ...\n");
250  double e_weight_new = get(w_lazy_map, e);
251  if (e_weight_new == e_weight_old)
252  return;
253 
254  // remove in both directions (assume above check skips fake roots)
255  Vertex va = source(e, g);
256  Vertex vb = target(e, g);
257 
258  // remove in both directions
259  double weight_frac_old = e_weight_old / len_ref;
260  if (!do_fake_roots || (vb != v_start && va != v_goal))
261  solver.remove_edge(va, vb, weight_frac_old);
262  if (!do_fake_roots || (va != v_start && vb != v_goal))
263  solver.remove_edge(vb, va, weight_frac_old);
264 
265  // add in both directions
266  double weight_frac = get(w_lazy_map, e) / len_ref;
267  if (!do_fake_roots || (vb != v_start && va != v_goal))
268  solver.add_edge(va, vb, weight_frac);
269  if (!do_fake_roots || (va != v_start && vb != v_goal))
270  solver.add_edge(vb, va, weight_frac);
271  }
272 };
273 
274 } // namespace pr_bgl
Adaptor to use matrix partition_all functions as a LazySP selector.
Definition: lazysp_selector_partition_all.h:174
Adaptor to use non-matrix partition_all functions as a selector for pr_bgl::lazysp.
Definition: lazysp_selector_partition_all.h:28
Convert a master index to a row-major-order matrix index pair.
Definition: pair_index_map.h:20
Definition: partition_all.h:225