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&#39;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.