All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
PathSimplifier.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/geometric/PathSimplifier.h"
00038 #include "ompl/tools/config/MagicConstants.h"
00039 #include <algorithm>
00040 #include <limits>
00041 #include <cstdlib>
00042 #include <cmath>
00043 #include <map>
00044 
00045 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
00046 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
00047 {
00048     if (path.getStateCount() < 3)
00049         return;
00050 
00051     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00052     std::vector<base::State*> &states = path.getStates();
00053 
00054     base::State *temp1 = si->allocState();
00055     base::State *temp2 = si->allocState();
00056 
00057     for (unsigned int s = 0 ; s < maxSteps ; ++s)
00058     {
00059         path.subdivide();
00060 
00061         unsigned int i = 2, u = 0, n1 = states.size() - 1;
00062         while (i < n1)
00063         {
00064             if (si->isValid(states[i - 1]))
00065             {
00066                 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
00067                 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
00068                 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
00069                 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
00070                 {
00071                     if (si->distance(states[i], temp1) > minChange)
00072                     {
00073                         si->copyState(states[i], temp1);
00074                         ++u;
00075                     }
00076                 }
00077             }
00078 
00079             i += 2;
00080         }
00081 
00082         if (u == 0)
00083             break;
00084     }
00085 
00086     si->freeState(temp1);
00087     si->freeState(temp2);
00088 }
00089 
00090 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
00091 {
00092     if (path.getStateCount() < 3)
00093         return false;
00094 
00095     if (maxSteps == 0)
00096         maxSteps = path.getStateCount();
00097 
00098     if (maxEmptySteps == 0)
00099         maxEmptySteps = path.getStateCount();
00100 
00101     bool result = false;
00102     unsigned int nochange = 0;
00103     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00104     std::vector<base::State*> &states = path.getStates();
00105 
00106     for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00107     {
00108         int count = states.size();
00109         int maxN  = count - 1;
00110         int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
00111 
00112         int p1 = rng_.uniformInt(0, maxN);
00113         int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
00114         if (abs(p1 - p2) < 2)
00115         {
00116             if (p1 < maxN - 1)
00117                 p2 = p1 + 2;
00118             else
00119                 if (p1 > 1)
00120                     p2 = p1 - 2;
00121                 else
00122                     continue;
00123         }
00124 
00125         if (p1 > p2)
00126             std::swap(p1, p2);
00127 
00128         if (si->checkMotion(states[p1], states[p2]))
00129         {
00130             for (int j = p1 + 1 ; j < p2 ; ++j)
00131                 si->freeState(states[j]);
00132             states.erase(states.begin() + p1 + 1, states.begin() + p2);
00133             nochange = 0;
00134             result = true;
00135         }
00136     }
00137     return result;
00138 }
00139 
00140 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
00141 {
00142     if (path.getStateCount() < 3)
00143         return false;
00144 
00145     if (maxSteps == 0)
00146         maxSteps = path.getStateCount();
00147 
00148     if (maxEmptySteps == 0)
00149         maxEmptySteps = path.getStateCount();
00150 
00151     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00152     std::vector<base::State*> &states = path.getStates();
00153 
00154     std::vector<double> dists(states.size(), 0.0);
00155     for (unsigned int i = 1 ; i < dists.size() ; ++i)
00156         dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
00157     double threshold = dists.back() * snapToVertex;
00158     double rd = rangeRatio * dists.back();
00159 
00160     base::State *temp0 = si->allocState();
00161     base::State *temp1 = si->allocState();
00162     bool result = false;
00163     unsigned int nochange = 0;
00164     for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00165     {
00166         base::State *s0 = NULL;
00167         int index0 = -1;
00168         double t0 = 0.0;
00169         double p0 = rng_.uniformReal(0.0, dists.back());
00170         std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
00171         int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00172 
00173         if (pos0 == 0 || dists[pos0] - p0 < threshold)
00174             index0 = pos0;
00175         else
00176         {
00177             while (pos0 > 0 && p0 < dists[pos0])
00178                 --pos0;
00179             if (p0 - dists[pos0] < threshold)
00180                 index0 = pos0;
00181         }
00182 
00183         base::State *s1 = NULL;
00184         int index1 = -1;
00185         double t1 = 0.0;
00186         double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
00187         pit = std::lower_bound(dists.begin(), dists.end(), p1);
00188         int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00189 
00190         if (pos1 == 0 || dists[pos1] - p1 < threshold)
00191             index1 = pos1;
00192         else
00193         {
00194             while (pos1 > 0 && p1 < dists[pos1])
00195                 --pos1;
00196             if (p1 - dists[pos1] < threshold)
00197                 index1 = pos1;
00198         }
00199 
00200         if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
00201             pos0 + 1 == index1 || pos1 + 1 == index0 ||
00202             (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
00203             continue;
00204 
00205         if (index0 >= 0)
00206             s0 = states[index0];
00207         else
00208         {
00209             t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
00210             si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
00211             s0 = temp0;
00212         }
00213 
00214         if (index1 >= 0)
00215             s1 = states[index1];
00216         else
00217         {
00218             t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
00219             si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
00220             s1 = temp1;
00221         }
00222 
00223         if (si->checkMotion(s0, s1))
00224         {
00225             if (pos0 > pos1)
00226             {
00227                 std::swap(pos0, pos1);
00228                 std::swap(index0, index1);
00229                 std::swap(s0, s1);
00230                 std::swap(t0, t1);
00231             }
00232 
00233             if (index0 < 0 && index1 < 0)
00234             {
00235                 if (pos0 + 1 == pos1)
00236                 {
00237                     si->copyState(states[pos1], s0);
00238                     states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
00239                 }
00240                 else
00241                 {
00242                     for (int j = pos0 + 2 ; j < pos1 ; ++j)
00243                         si->freeState(states[j]);
00244                     si->copyState(states[pos0 + 1], s0);
00245                     si->copyState(states[pos1], s1);
00246                     states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
00247                 }
00248             }
00249             else
00250                 if (index0 >= 0 && index1 >= 0)
00251                 {
00252                     for (int j = index0 + 1 ; j < index1 ; ++j)
00253                         si->freeState(states[j]);
00254                     states.erase(states.begin() + index0 + 1, states.begin() + index1);
00255                 }
00256                 else
00257                     if (index0 < 0 && index1 >= 0)
00258                     {
00259                         for (int j = pos0 + 2 ; j < index1 ; ++j)
00260                             si->freeState(states[j]);
00261                         si->copyState(states[pos0 + 1], s0);
00262                         states.erase(states.begin() + pos0 + 2, states.begin() + index1);
00263                     }
00264                     else
00265                         if (index0 >= 0 && index1 < 0)
00266                         {
00267                             for (int j = index0 + 1 ; j < pos1 ; ++j)
00268                                 si->freeState(states[j]);
00269                             si->copyState(states[pos1], s1);
00270                             states.erase(states.begin() + index0 + 1, states.begin() + pos1);
00271                         }
00272 
00273             // fix the helper variables
00274             dists.resize(states.size(), 0.0);
00275             for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
00276                 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
00277             threshold = dists.back() * snapToVertex;
00278             rd = rangeRatio * dists.back();
00279             result = true;
00280             nochange = 0;
00281         }
00282     }
00283 
00284     si->freeState(temp1);
00285     si->freeState(temp0);
00286     return result;
00287 }
00288 
00289 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
00290 {
00291     if (path.getStateCount() < 3)
00292         return false;
00293 
00294     if (maxSteps == 0)
00295         maxSteps = path.getStateCount();
00296 
00297     if (maxEmptySteps == 0)
00298         maxEmptySteps = path.getStateCount();
00299 
00300     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00301     std::vector<base::State*> &states = path.getStates();
00302 
00303     // compute pair-wise distances in path (construct only half the matrix)
00304     std::map<std::pair<const base::State*, const base::State*>, double> distances;
00305     for (unsigned int i = 0 ; i < states.size() ; ++i)
00306         for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00307             distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
00308 
00309     bool result = false;
00310     unsigned int nochange = 0;
00311     for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
00312     {
00313         // find closest pair of points
00314         double minDist = std::numeric_limits<double>::infinity();
00315         int p1 = -1;
00316         int p2 = -1;
00317         for (unsigned int i = 0 ; i < states.size() ; ++i)
00318             for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00319             {
00320                 double d = distances[std::make_pair(states[i], states[j])];
00321                 if (d < minDist)
00322                 {
00323                     minDist = d;
00324                     p1 = i;
00325                     p2 = j;
00326                 }
00327             }
00328 
00329         if (p1 >= 0 && p2 >= 0)
00330         {
00331             if (si->checkMotion(states[p1], states[p2]))
00332             {
00333                 for (int i = p1 + 1 ; i < p2 ; ++i)
00334                     si->freeState(states[i]);
00335                 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00336                 result = true;
00337                 nochange = 0;
00338             }
00339             else
00340                 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
00341         }
00342         else
00343             break;
00344     }
00345     return result;
00346 }
00347 
00348 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
00349 {
00350     reduceVertices(path);
00351     collapseCloseVertices(path);
00352     smoothBSpline(path, 4, path.length()/100.0);
00353     const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00354     if (!p.second)
00355         msg_.warn("Solution path may slightly touch on an invalid region of the state space");
00356     else
00357         if (!p.first)
00358             msg_.debug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00359 }
00360 
00361 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime)
00362 {
00363     simplify(path, base::timedPlannerTerminationCondition(maxTime));
00364 }
00365 
00366 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
00367 {
00368     if (path.getStateCount() < 3)
00369         return;
00370 
00371     // try a randomized step of connecting vertices
00372     bool tryMore = false;
00373     if (ptc() == false)
00374         tryMore = reduceVertices(path);
00375 
00376     // try to collapse close-by vertices
00377     if (ptc() == false)
00378         collapseCloseVertices(path);
00379 
00380     // try to reduce verices some more, if there is any point in doing so
00381     int times = 0;
00382     while (tryMore && ptc() == false && ++times <= 5)
00383         tryMore = reduceVertices(path);
00384 
00385     // run a more complex short-cut algorithm : allow splitting path segments
00386     if (ptc() == false)
00387         tryMore = shortcutPath(path);
00388     else
00389         tryMore = false;
00390 
00391     // run the short-cut algorithm some more, if it makes a difference
00392     times = 0;
00393     while (tryMore && ptc() == false && ++times <= 5)
00394         tryMore = shortcutPath(path);
00395 
00396     // smooth the path
00397     if (ptc() == false)
00398         smoothBSpline(path, 3, path.length()/100.0);
00399 
00400     // we always run this
00401     const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00402     if (!p.second)
00403         msg_.warn("Solution path may slightly touch on an invalid region of the state space");
00404     else
00405         if (!p.first)
00406             msg_.debug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00407 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends