All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PathSimplifier.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University, 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 Rice University 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/PathSimplifier.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include <algorithm>
40 #include <limits>
41 #include <cstdlib>
42 #include <cmath>
43 #include <map>
44 
45 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
46 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
47 {
48  if (path.getStateCount() < 3)
49  return;
50 
52  std::vector<base::State*> &states = path.getStates();
53 
54  base::State *temp1 = si->allocState();
55  base::State *temp2 = si->allocState();
56 
57  for (unsigned int s = 0 ; s < maxSteps ; ++s)
58  {
59  path.subdivide();
60 
61  unsigned int i = 2, u = 0, n1 = states.size() - 1;
62  while (i < n1)
63  {
64  if (si->isValid(states[i - 1]))
65  {
66  si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
67  si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
68  si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
69  if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
70  {
71  if (si->distance(states[i], temp1) > minChange)
72  {
73  si->copyState(states[i], temp1);
74  ++u;
75  }
76  }
77  }
78 
79  i += 2;
80  }
81 
82  if (u == 0)
83  break;
84  }
85 
86  si->freeState(temp1);
87  si->freeState(temp2);
88 }
89 
90 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
91 {
92  if (path.getStateCount() < 3)
93  return false;
94 
95  if (maxSteps == 0)
96  maxSteps = path.getStateCount();
97 
98  if (maxEmptySteps == 0)
99  maxEmptySteps = path.getStateCount();
100 
101  bool result = false;
102  unsigned int nochange = 0;
104  std::vector<base::State*> &states = path.getStates();
105 
106  if (si->checkMotion(states.front(), states.back()))
107  {
108  for (std::size_t i = 2 ; i < states.size() ; ++i)
109  si->freeState(states[i-1]);
110  std::vector<base::State*> newStates(2);
111  newStates[0] = states.front();
112  newStates[1] = states.back();
113  states.swap(newStates);
114  result = true;
115  }
116  else
117  for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
118  {
119  int count = states.size();
120  int maxN = count - 1;
121  int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
122 
123  int p1 = rng_.uniformInt(0, maxN);
124  int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
125  if (abs(p1 - p2) < 2)
126  {
127  if (p1 < maxN - 1)
128  p2 = p1 + 2;
129  else
130  if (p1 > 1)
131  p2 = p1 - 2;
132  else
133  continue;
134  }
135 
136  if (p1 > p2)
137  std::swap(p1, p2);
138 
139  if (si->checkMotion(states[p1], states[p2]))
140  {
141  for (int j = p1 + 1 ; j < p2 ; ++j)
142  si->freeState(states[j]);
143  states.erase(states.begin() + p1 + 1, states.begin() + p2);
144  nochange = 0;
145  result = true;
146  }
147  }
148  return result;
149 }
150 
151 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
152 {
153  if (path.getStateCount() < 3)
154  return false;
155 
156  if (maxSteps == 0)
157  maxSteps = path.getStateCount();
158 
159  if (maxEmptySteps == 0)
160  maxEmptySteps = path.getStateCount();
161 
163  std::vector<base::State*> &states = path.getStates();
164 
165  std::vector<double> dists(states.size(), 0.0);
166  for (unsigned int i = 1 ; i < dists.size() ; ++i)
167  dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
168  double threshold = dists.back() * snapToVertex;
169  double rd = rangeRatio * dists.back();
170 
171  base::State *temp0 = si->allocState();
172  base::State *temp1 = si->allocState();
173  bool result = false;
174  unsigned int nochange = 0;
175  for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
176  {
177  base::State *s0 = NULL;
178  int index0 = -1;
179  double t0 = 0.0;
180  double p0 = rng_.uniformReal(0.0, dists.back());
181  std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
182  int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
183 
184  if (pos0 == 0 || dists[pos0] - p0 < threshold)
185  index0 = pos0;
186  else
187  {
188  while (pos0 > 0 && p0 < dists[pos0])
189  --pos0;
190  if (p0 - dists[pos0] < threshold)
191  index0 = pos0;
192  }
193 
194  base::State *s1 = NULL;
195  int index1 = -1;
196  double t1 = 0.0;
197  double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
198  pit = std::lower_bound(dists.begin(), dists.end(), p1);
199  int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
200 
201  if (pos1 == 0 || dists[pos1] - p1 < threshold)
202  index1 = pos1;
203  else
204  {
205  while (pos1 > 0 && p1 < dists[pos1])
206  --pos1;
207  if (p1 - dists[pos1] < threshold)
208  index1 = pos1;
209  }
210 
211  if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
212  pos0 + 1 == index1 || pos1 + 1 == index0 ||
213  (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
214  continue;
215 
216  if (index0 >= 0)
217  s0 = states[index0];
218  else
219  {
220  t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
221  si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
222  s0 = temp0;
223  }
224 
225  if (index1 >= 0)
226  s1 = states[index1];
227  else
228  {
229  t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
230  si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
231  s1 = temp1;
232  }
233 
234  if (si->checkMotion(s0, s1))
235  {
236  if (pos0 > pos1)
237  {
238  std::swap(pos0, pos1);
239  std::swap(index0, index1);
240  std::swap(s0, s1);
241  std::swap(t0, t1);
242  }
243 
244  if (index0 < 0 && index1 < 0)
245  {
246  if (pos0 + 1 == pos1)
247  {
248  si->copyState(states[pos1], s0);
249  states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
250  }
251  else
252  {
253  for (int j = pos0 + 2 ; j < pos1 ; ++j)
254  si->freeState(states[j]);
255  si->copyState(states[pos0 + 1], s0);
256  si->copyState(states[pos1], s1);
257  states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
258  }
259  }
260  else
261  if (index0 >= 0 && index1 >= 0)
262  {
263  for (int j = index0 + 1 ; j < index1 ; ++j)
264  si->freeState(states[j]);
265  states.erase(states.begin() + index0 + 1, states.begin() + index1);
266  }
267  else
268  if (index0 < 0 && index1 >= 0)
269  {
270  for (int j = pos0 + 2 ; j < index1 ; ++j)
271  si->freeState(states[j]);
272  si->copyState(states[pos0 + 1], s0);
273  states.erase(states.begin() + pos0 + 2, states.begin() + index1);
274  }
275  else
276  if (index0 >= 0 && index1 < 0)
277  {
278  for (int j = index0 + 1 ; j < pos1 ; ++j)
279  si->freeState(states[j]);
280  si->copyState(states[pos1], s1);
281  states.erase(states.begin() + index0 + 1, states.begin() + pos1);
282  }
283 
284  // fix the helper variables
285  dists.resize(states.size(), 0.0);
286  for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
287  dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
288  threshold = dists.back() * snapToVertex;
289  rd = rangeRatio * dists.back();
290  result = true;
291  nochange = 0;
292  }
293  }
294 
295  si->freeState(temp1);
296  si->freeState(temp0);
297  return result;
298 }
299 
300 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
301 {
302  if (path.getStateCount() < 3)
303  return false;
304 
305  if (maxSteps == 0)
306  maxSteps = path.getStateCount();
307 
308  if (maxEmptySteps == 0)
309  maxEmptySteps = path.getStateCount();
310 
312  std::vector<base::State*> &states = path.getStates();
313 
314  // compute pair-wise distances in path (construct only half the matrix)
315  std::map<std::pair<const base::State*, const base::State*>, double> distances;
316  for (unsigned int i = 0 ; i < states.size() ; ++i)
317  for (unsigned int j = i + 2 ; j < states.size() ; ++j)
318  distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
319 
320  bool result = false;
321  unsigned int nochange = 0;
322  for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
323  {
324  // find closest pair of points
325  double minDist = std::numeric_limits<double>::infinity();
326  int p1 = -1;
327  int p2 = -1;
328  for (unsigned int i = 0 ; i < states.size() ; ++i)
329  for (unsigned int j = i + 2 ; j < states.size() ; ++j)
330  {
331  double d = distances[std::make_pair(states[i], states[j])];
332  if (d < minDist)
333  {
334  minDist = d;
335  p1 = i;
336  p2 = j;
337  }
338  }
339 
340  if (p1 >= 0 && p2 >= 0)
341  {
342  if (si->checkMotion(states[p1], states[p2]))
343  {
344  for (int i = p1 + 1 ; i < p2 ; ++i)
345  si->freeState(states[i]);
346  states.erase(states.begin() + p1 + 1, states.begin() + p2);
347  result = true;
348  nochange = 0;
349  }
350  else
351  distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
352  }
353  else
354  break;
355  }
356  return result;
357 }
358 
360 {
361  reduceVertices(path);
362  collapseCloseVertices(path);
363  smoothBSpline(path, 4, path.length()/100.0);
364  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
365  if (!p.second)
366  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
367  else
368  if (!p.first)
369  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
370 }
371 
373 {
374  simplify(path, base::timedPlannerTerminationCondition(maxTime));
375 }
376 
378 {
379  if (path.getStateCount() < 3)
380  return;
381 
382  // try a randomized step of connecting vertices
383  bool tryMore = false;
384  if (ptc == false)
385  tryMore = reduceVertices(path);
386 
387  // try to collapse close-by vertices
388  if (ptc == false)
389  collapseCloseVertices(path);
390 
391  // try to reduce verices some more, if there is any point in doing so
392  int times = 0;
393  while (tryMore && ptc == false && ++times <= 5)
394  tryMore = reduceVertices(path);
395 
396  // run a more complex short-cut algorithm : allow splitting path segments
397  if (ptc == false)
398  tryMore = shortcutPath(path);
399  else
400  tryMore = false;
401 
402  // run the short-cut algorithm some more, if it makes a difference
403  times = 0;
404  while (tryMore && ptc == false && ++times <= 5)
405  tryMore = shortcutPath(path);
406 
407  // smooth the path
408  if (ptc == false)
409  smoothBSpline(path, 3, path.length()/100.0);
410 
411  // we always run this
412  const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
413  if (!p.second)
414  OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
415  else
416  if (!p.first)
417  OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
418 }