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 #include <utility>
40 
41 namespace ompl
42 {
43  namespace magic
44  {
46  static const double GAP_COST_FRACTION = 0.05;
47  }
48 }
49 
51  : si_(std::move(si)), stateProperty_(boost::get(vertex_state_t(), g_)), name_("PathHybridization")
52 {
53  root_ = boost::add_vertex(g_);
54  stateProperty_[root_] = nullptr;
55  goal_ = boost::add_vertex(g_);
56  stateProperty_[goal_] = nullptr;
57 }
58 
59 ompl::geometric::PathHybridization::~PathHybridization() = default;
60 
62 {
63  hpath_.reset();
64  paths_.clear();
65 
66  g_.clear();
67  root_ = boost::add_vertex(g_);
68  stateProperty_[root_] = nullptr;
69  goal_ = boost::add_vertex(g_);
70  stateProperty_[goal_] = nullptr;
71 }
72 
73 void ompl::geometric::PathHybridization::print(std::ostream &out) const
74 {
75  out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
76  int i = 1;
77  for (auto it = paths_.begin(); it != paths_.end(); ++it, ++i)
78  out << " path " << i << " of length " << it->length_ << std::endl;
79  if (hpath_)
80  out << "Hybridized path of length " << hpath_->length() << std::endl;
81 }
82 
84 {
85  return name_;
86 }
87 
89 {
90  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
91  boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
92  if (prev[goal_] != goal_)
93  {
94  auto h(std::make_shared<PathGeometric>(si_));
95  for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
96  h->append(stateProperty_[pos]);
97  h->reverse();
98  hpath_ = h;
99  }
100 }
101 
103 {
104  return hpath_;
105 }
106 
107 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
108 {
109  auto *p = dynamic_cast<PathGeometric *>(pp.get());
110  if (!p)
111  {
112  OMPL_ERROR("Path hybridization only works for geometric paths");
113  return 0;
114  }
115 
116  if (p->getSpaceInformation() != si_)
117  {
118  OMPL_ERROR("Paths for hybridization must be from the same space information");
119  return 0;
120  }
121 
122  // skip empty paths
123  if (p->getStateCount() == 0)
124  return 0;
125 
126  PathInfo pi(pp);
127 
128  // if this path was previously included in the hybridization, skip it
129  if (paths_.find(pi) != paths_.end())
130  return 0;
131 
132  // the number of connection attempts
133  unsigned int nattempts = 0;
134 
135  // start from virtual root
136  Vertex v0 = boost::add_vertex(g_);
137  stateProperty_[v0] = pi.states_[0];
138  pi.vertices_.push_back(v0);
139 
140  // add all the vertices of the path, and the edges between them, to the HGraph
141  // also compute the path length for future use (just for computational savings)
142  const HGraph::edge_property_type prop0(0.0);
143  boost::add_edge(root_, v0, prop0, g_);
144  double length = 0.0;
145  for (std::size_t j = 1; j < pi.states_.size(); ++j)
146  {
147  Vertex v1 = boost::add_vertex(g_);
148  stateProperty_[v1] = pi.states_[j];
149  double weight = si_->distance(pi.states_[j - 1], pi.states_[j]);
150  const HGraph::edge_property_type properties(weight);
151  boost::add_edge(v0, v1, properties, g_);
152  length += weight;
153  pi.vertices_.push_back(v1);
154  v0 = v1;
155  }
156 
157  // connect to virtual goal
158  boost::add_edge(v0, goal_, prop0, g_);
159  pi.length_ = length;
160 
161  // find matches with previously added paths
162  for (const auto &path : paths_)
163  {
164  const auto *q = static_cast<const PathGeometric *>(path.path_.get());
165  std::vector<int> indexP, indexQ;
166  matchPaths(*p, *q, (pi.length_ + path.length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
167 
168  if (matchAcrossGaps)
169  {
170  int lastP = -1;
171  int lastQ = -1;
172  int gapStartP = -1;
173  int gapStartQ = -1;
174  bool gapP = false;
175  bool gapQ = false;
176  for (std::size_t i = 0; i < indexP.size(); ++i)
177  {
178  // a gap is found in p
179  if (indexP[i] < 0)
180  {
181  // remember this as the beginning of the gap, if needed
182  if (!gapP)
183  gapStartP = i;
184  // mark the fact we are now in a gap on p
185  gapP = true;
186  }
187  else
188  {
189  // check if a gap just ended;
190  // if it did, try to match the endpoint with the elements in q
191  if (gapP)
192  for (std::size_t j = gapStartP; j < i; ++j)
193  {
194  attemptNewEdge(pi, path, indexP[i], indexQ[j]);
195  ++nattempts;
196  }
197  // remember the last non-negative index in p
198  lastP = i;
199  gapP = false;
200  }
201  if (indexQ[i] < 0)
202  {
203  if (!gapQ)
204  gapStartQ = i;
205  gapQ = true;
206  }
207  else
208  {
209  if (gapQ)
210  for (std::size_t j = gapStartQ; j < i; ++j)
211  {
212  attemptNewEdge(pi, path, indexP[j], indexQ[i]);
213  ++nattempts;
214  }
215  lastQ = i;
216  gapQ = false;
217  }
218 
219  // try to match corresponding index values and gep beginnings
220  if (lastP >= 0 && lastQ >= 0)
221  {
222  attemptNewEdge(pi, path, indexP[lastP], indexQ[lastQ]);
223  ++nattempts;
224  }
225  }
226  }
227  else
228  {
229  // attempt new edge only when states align
230  for (std::size_t i = 0; i < indexP.size(); ++i)
231  if (indexP[i] >= 0 && indexQ[i] >= 0)
232  {
233  attemptNewEdge(pi, path, indexP[i], indexQ[i]);
234  ++nattempts;
235  }
236  }
237  }
238 
239  // remember this path is part of the hybridization
240  paths_.insert(pi);
241  return nattempts;
242 }
243 
244 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
245 {
246  if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
247  {
248  double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
249  const HGraph::edge_property_type properties(weight);
250  boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
251  }
252 }
253 
255 {
256  return paths_.size();
257 }
258 
260  std::vector<int> &indexP, std::vector<int> &indexQ) const
261 {
262  std::vector<std::vector<double>> C(p.getStateCount());
263  std::vector<std::vector<char>> T(p.getStateCount());
264 
265  for (std::size_t i = 0; i < p.getStateCount(); ++i)
266  {
267  C[i].resize(q.getStateCount(), 0.0);
268  T[i].resize(q.getStateCount(), '\0');
269  for (std::size_t j = 0; j < q.getStateCount(); ++j)
270  {
271  // as far as I can tell, there is a bug in the algorithm as presented in the paper
272  // so I am doing things slightly differently ...
273  double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
274  double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
275  double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
276  if (match <= up && match <= left)
277  {
278  C[i][j] = match;
279  T[i][j] = 'm';
280  }
281  else if (up <= match && up <= left)
282  {
283  C[i][j] = up;
284  T[i][j] = 'u';
285  }
286  else
287  {
288  C[i][j] = left;
289  T[i][j] = 'l';
290  }
291  }
292  }
293  // construct the sequences with gaps (only index positions)
294  int m = p.getStateCount() - 1;
295  int n = q.getStateCount() - 1;
296 
297  indexP.clear();
298  indexQ.clear();
299  indexP.reserve(std::max(n, m));
300  indexQ.reserve(indexP.size());
301 
302  while (n >= 0 && m >= 0)
303  {
304  if (T[m][n] == 'm')
305  {
306  indexP.push_back(m);
307  indexQ.push_back(n);
308  --m;
309  --n;
310  }
311  else if (T[m][n] == 'u')
312  {
313  indexP.push_back(m);
314  indexQ.push_back(-1);
315  --m;
316  }
317  else
318  {
319  indexP.push_back(-1);
320  indexQ.push_back(n);
321  --n;
322  }
323  }
324  while (n >= 0)
325  {
326  indexP.push_back(-1);
327  indexQ.push_back(n);
328  --n;
329  }
330  while (m >= 0)
331  {
332  indexP.push_back(m);
333  indexQ.push_back(-1);
334  --m;
335  }
336 }
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized...
void clear()
Clear all the stored paths.
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
STL namespace.
const std::string & getName() const
Get the name of the algorithm.
void computeHybridPath()
Run Dijkstra&#39;s algorithm to find out the shortest path among the mixed ones.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
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.
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...
Definition of a geometric path.
Definition: PathGeometric.h:60
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on...
A shared pointer wrapper for ompl::base::Path.