PathHybridization.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/geometric/PathHybridization.h"
38 #include <boost/graph/dijkstra_shortest_paths.hpp>
39 
40 namespace ompl
41 {
42  namespace magic
43  {
45  static const double GAP_COST_FRACTION = 0.05;
46  }
47 }
48 
50  si_(si),
51  stateProperty_(boost::get(vertex_state_t(), g_)),
52  name_("PathHybridization")
53 {
54  root_ = boost::add_vertex(g_);
55  stateProperty_[root_] = NULL;
56  goal_ = boost::add_vertex(g_);
57  stateProperty_[goal_] = NULL;
58 }
59 
60 ompl::geometric::PathHybridization::~PathHybridization()
61 {
62 }
63 
65 {
66  hpath_.reset();
67  paths_.clear();
68 
69  g_.clear();
70  root_ = boost::add_vertex(g_);
71  stateProperty_[root_] = NULL;
72  goal_ = boost::add_vertex(g_);
73  stateProperty_[goal_] = NULL;
74 }
75 
76 void ompl::geometric::PathHybridization::print(std::ostream &out) const
77 {
78  out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
79  int i = 1;
80  for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i)
81  out << " path " << i << " of length " << it->length_ << std::endl;
82  if (hpath_)
83  out << "Hybridized path of length " << hpath_->length() << std::endl;
84 }
85 
87 {
88  return name_;
89 }
90 
92 {
93  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
94  boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
95  if (prev[goal_] != goal_)
96  {
97  PathGeometric *h = new PathGeometric(si_);
98  for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
99  h->append(stateProperty_[pos]);
100  h->reverse();
101  hpath_.reset(h);
102  }
103 }
104 
106 {
107  return hpath_;
108 }
109 
110 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
111 {
112  PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
113  if (!p)
114  {
115  OMPL_ERROR("Path hybridization only works for geometric paths");
116  return 0;
117  }
118 
119  if (p->getSpaceInformation() != si_)
120  {
121  OMPL_ERROR("Paths for hybridization must be from the same space information");
122  return 0;
123  }
124 
125  // skip empty paths
126  if (p->getStateCount() == 0)
127  return 0;
128 
129  PathInfo pi(pp);
130 
131  // if this path was previously included in the hybridization, skip it
132  if (paths_.find(pi) != paths_.end())
133  return 0;
134 
135  // the number of connection attempts
136  unsigned int nattempts = 0;
137 
138  // start from virtual root
139  Vertex v0 = boost::add_vertex(g_);
140  stateProperty_[v0] = pi.states_[0];
141  pi.vertices_.push_back(v0);
142 
143  // add all the vertices of the path, and the edges between them, to the HGraph
144  // also compute the path length for future use (just for computational savings)
145  const HGraph::edge_property_type prop0(0.0);
146  boost::add_edge(root_, v0, prop0, g_);
147  double length = 0.0;
148  for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
149  {
150  Vertex v1 = boost::add_vertex(g_);
151  stateProperty_[v1] = pi.states_[j];
152  double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
153  const HGraph::edge_property_type properties(weight);
154  boost::add_edge(v0, v1, properties, g_);
155  length += weight;
156  pi.vertices_.push_back(v1);
157  v0 = v1;
158  }
159 
160  // connect to virtual goal
161  boost::add_edge(v0, goal_, prop0, g_);
162  pi.length_ = length;
163 
164  // find matches with previously added paths
165  for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
166  {
167  const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
168  std::vector<int> indexP, indexQ;
169  matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
170 
171  if (matchAcrossGaps)
172  {
173  int lastP = -1;
174  int lastQ = -1;
175  int gapStartP = -1;
176  int gapStartQ = -1;
177  bool gapP = false;
178  bool gapQ = false;
179  for (std::size_t i = 0 ; i < indexP.size() ; ++i)
180  {
181  // a gap is found in p
182  if (indexP[i] < 0)
183  {
184  // remember this as the beginning of the gap, if needed
185  if (!gapP)
186  gapStartP = i;
187  // mark the fact we are now in a gap on p
188  gapP = true;
189  }
190  else
191  {
192  // check if a gap just ended;
193  // if it did, try to match the endpoint with the elements in q
194  if (gapP)
195  for (std::size_t j = gapStartP ; j < i ; ++j)
196  {
197  attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
198  ++nattempts;
199  }
200  // remember the last non-negative index in p
201  lastP = i;
202  gapP = false;
203  }
204  if (indexQ[i] < 0)
205  {
206  if (!gapQ)
207  gapStartQ = i;
208  gapQ = true;
209  }
210  else
211  {
212  if (gapQ)
213  for (std::size_t j = gapStartQ ; j < i ; ++j)
214  {
215  attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
216  ++nattempts;
217  }
218  lastQ = i;
219  gapQ = false;
220  }
221 
222  // try to match corresponding index values and gep beginnings
223  if (lastP >= 0 && lastQ >= 0)
224  {
225  attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
226  ++nattempts;
227  }
228  }
229  }
230  else
231  {
232  // attempt new edge only when states align
233  for (std::size_t i = 0 ; i < indexP.size() ; ++i)
234  if (indexP[i] >= 0 && indexQ[i] >= 0)
235  {
236  attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
237  ++nattempts;
238  }
239  }
240  }
241 
242  // remember this path is part of the hybridization
243  paths_.insert(pi);
244  return nattempts;
245 }
246 
247 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
248 {
249  if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
250  {
251  double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
252  const HGraph::edge_property_type properties(weight);
253  boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
254  }
255 }
256 
258 {
259  return paths_.size();
260 }
261 
263  std::vector<int> &indexP, std::vector<int> &indexQ) const
264 {
265  std::vector<std::vector<double> > C(p.getStateCount());
266  std::vector<std::vector<char> > T(p.getStateCount());
267 
268  for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
269  {
270  C[i].resize(q.getStateCount(), 0.0);
271  T[i].resize(q.getStateCount(), '\0');
272  for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
273  {
274  // as far as I can tell, there is a bug in the algorithm as presented in the paper
275  // so I am doing things slightly differently ...
276  double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
277  double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
278  double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
279  if (match <= up && match <= left)
280  {
281  C[i][j] = match;
282  T[i][j] = 'm';
283  }
284  else
285  if (up <= match && up <= left)
286  {
287  C[i][j] = up;
288  T[i][j] = 'u';
289  }
290  else
291  {
292  C[i][j] = left;
293  T[i][j] = 'l';
294  }
295  }
296  }
297  // construct the sequences with gaps (only index positions)
298  int m = p.getStateCount() - 1;
299  int n = q.getStateCount() - 1;
300 
301  indexP.clear();
302  indexQ.clear();
303  indexP.reserve(std::max(n,m));
304  indexQ.reserve(indexP.size());
305 
306  while (n >= 0 && m >= 0)
307  {
308  if (T[m][n] == 'm')
309  {
310  indexP.push_back(m);
311  indexQ.push_back(n);
312  --m; --n;
313  }
314  else
315  if (T[m][n] == 'u')
316  {
317  indexP.push_back(m);
318  indexQ.push_back(-1);
319  --m;
320  }
321  else
322  {
323  indexP.push_back(-1);
324  indexQ.push_back(n);
325  --n;
326  }
327  }
328  while (n >= 0)
329  {
330  indexP.push_back(-1);
331  indexQ.push_back(n);
332  --n;
333  }
334  while (m >= 0)
335  {
336  indexP.push_back(m);
337  indexQ.push_back(-1);
338  --m;
339  }
340 }
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized...
const std::string & getName() const
Get the name of the algorithm.
void clear()
Clear all the stored paths.
void computeHybridPath()
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::State * getState(unsigned int index)
Get the state located at index along the path.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(const base::SpaceInformationPtr &si)
The constructor needs to know about the space information of the paths it will operate on...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
const SpaceInformationPtr & getSpaceInformation() const
Get the space information associated to this class.
Definition: Path.h:82
Main namespace. Contains everything in this library.
Definition: Cost.h:42
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost, std::vector< int > &indexP, std::vector< int > &indexQ) const
Given two geometric paths p and q, compute the alignment of the paths using dynamic programming in an...
void reverse()
Reverse the path.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
Definition: PathGeometric.h:60
A boost shared pointer wrapper for ompl::base::Path.