ompl/geometric/src/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 bool ompl::geometric::PathSimplifier::freeStates() const
00046 {
00047     return freeStates_;
00048 }
00049 
00050 void ompl::geometric::PathSimplifier::freeStates(bool flag)
00051 {
00052     freeStates_ = flag;
00053 }
00054 
00055 /* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
00056 void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
00057 {
00058     if (path.getStateCount() < 3)
00059         return;
00060 
00061     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00062     std::vector<base::State*> &states = path.getStates();
00063 
00064     base::State *temp1 = si->allocState();
00065     base::State *temp2 = si->allocState();
00066 
00067     for (unsigned int s = 0 ; s < maxSteps ; ++s)
00068     {
00069         path.subdivide();
00070 
00071         unsigned int i = 2, u = 0, n1 = states.size() - 1;
00072         while (i < n1)
00073         {
00074             if (si->isValid(states[i - 1]))
00075             {
00076                 si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
00077                 si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
00078                 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
00079                 if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
00080                 {
00081                     if (si->distance(states[i], temp1) > minChange)
00082                     {
00083                         si->copyState(states[i], temp1);
00084                         ++u;
00085                     }
00086                 }
00087             }
00088 
00089             i += 2;
00090         }
00091 
00092         if (u == 0)
00093             break;
00094     }
00095 
00096     si->freeState(temp1);
00097     si->freeState(temp2);
00098 }
00099 
00100 bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
00101 {
00102     if (path.getStateCount() < 3)
00103         return false;
00104 
00105     if (maxSteps == 0)
00106         maxSteps = path.getStateCount();
00107 
00108     if (maxEmptySteps == 0)
00109         maxEmptySteps = path.getStateCount();
00110 
00111     bool result = false;
00112     unsigned int nochange = 0;
00113     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00114     std::vector<base::State*> &states = path.getStates();
00115 
00116     if (si->checkMotion(states.front(), states.back()))
00117     {
00118         if (freeStates_)
00119             for (std::size_t i = 2 ; i < states.size() ; ++i)
00120                 si->freeState(states[i-1]);
00121         std::vector<base::State*> newStates(2);
00122         newStates[0] = states.front();
00123         newStates[1] = states.back();
00124         states.swap(newStates);
00125         result = true;
00126     }
00127     else
00128         for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00129         {
00130             int count = states.size();
00131             int maxN  = count - 1;
00132             int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
00133 
00134             int p1 = rng_.uniformInt(0, maxN);
00135             int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
00136             if (abs(p1 - p2) < 2)
00137             {
00138                 if (p1 < maxN - 1)
00139                     p2 = p1 + 2;
00140                 else
00141                     if (p1 > 1)
00142                         p2 = p1 - 2;
00143                     else
00144                         continue;
00145             }
00146 
00147             if (p1 > p2)
00148                 std::swap(p1, p2);
00149 
00150             if (si->checkMotion(states[p1], states[p2]))
00151             {
00152                 if (freeStates_)
00153                     for (int j = p1 + 1 ; j < p2 ; ++j)
00154                         si->freeState(states[j]);
00155                 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00156                 nochange = 0;
00157                 result = true;
00158             }
00159         }
00160     return result;
00161 }
00162 
00163 bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
00164 {
00165     if (path.getStateCount() < 3)
00166         return false;
00167 
00168     if (maxSteps == 0)
00169         maxSteps = path.getStateCount();
00170 
00171     if (maxEmptySteps == 0)
00172         maxEmptySteps = path.getStateCount();
00173 
00174     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00175     std::vector<base::State*> &states = path.getStates();
00176 
00177     std::vector<double> dists(states.size(), 0.0);
00178     for (unsigned int i = 1 ; i < dists.size() ; ++i)
00179         dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
00180     double threshold = dists.back() * snapToVertex;
00181     double rd = rangeRatio * dists.back();
00182 
00183     base::State *temp0 = si->allocState();
00184     base::State *temp1 = si->allocState();
00185     bool result = false;
00186     unsigned int nochange = 0;
00187     for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00188     {
00189         base::State *s0 = NULL;
00190         int index0 = -1;
00191         double t0 = 0.0;
00192         double p0 = rng_.uniformReal(0.0, dists.back());
00193         std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0);
00194         int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00195 
00196         if (pos0 == 0 || dists[pos0] - p0 < threshold)
00197             index0 = pos0;
00198         else
00199         {
00200             while (pos0 > 0 && p0 < dists[pos0])
00201                 --pos0;
00202             if (p0 - dists[pos0] < threshold)
00203                 index0 = pos0;
00204         }
00205 
00206         base::State *s1 = NULL;
00207         int index1 = -1;
00208         double t1 = 0.0;
00209         double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back()));
00210         pit = std::lower_bound(dists.begin(), dists.end(), p1);
00211         int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin();
00212 
00213         if (pos1 == 0 || dists[pos1] - p1 < threshold)
00214             index1 = pos1;
00215         else
00216         {
00217             while (pos1 > 0 && p1 < dists[pos1])
00218                 --pos1;
00219             if (p1 - dists[pos1] < threshold)
00220                 index1 = pos1;
00221         }
00222 
00223         if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
00224             pos0 + 1 == index1 || pos1 + 1 == index0 ||
00225             (index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
00226             continue;
00227 
00228         if (index0 >= 0)
00229             s0 = states[index0];
00230         else
00231         {
00232             t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
00233             si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
00234             s0 = temp0;
00235         }
00236 
00237         if (index1 >= 0)
00238             s1 = states[index1];
00239         else
00240         {
00241             t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
00242             si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
00243             s1 = temp1;
00244         }
00245 
00246         if (si->checkMotion(s0, s1))
00247         {
00248             if (pos0 > pos1)
00249             {
00250                 std::swap(pos0, pos1);
00251                 std::swap(index0, index1);
00252                 std::swap(s0, s1);
00253                 std::swap(t0, t1);
00254             }
00255 
00256             if (index0 < 0 && index1 < 0)
00257             {
00258                 if (pos0 + 1 == pos1)
00259                 {
00260                     si->copyState(states[pos1], s0);
00261                     states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
00262                 }
00263                 else
00264                 {
00265                     if (freeStates_)
00266                         for (int j = pos0 + 2 ; j < pos1 ; ++j)
00267                             si->freeState(states[j]);
00268                     si->copyState(states[pos0 + 1], s0);
00269                     si->copyState(states[pos1], s1);
00270                     states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
00271                 }
00272             }
00273             else
00274                 if (index0 >= 0 && index1 >= 0)
00275                 {
00276                     if (freeStates_)
00277                         for (int j = index0 + 1 ; j < index1 ; ++j)
00278                             si->freeState(states[j]);
00279                     states.erase(states.begin() + index0 + 1, states.begin() + index1);
00280                 }
00281                 else
00282                     if (index0 < 0 && index1 >= 0)
00283                     {
00284                         if (freeStates_)
00285                             for (int j = pos0 + 2 ; j < index1 ; ++j)
00286                                 si->freeState(states[j]);
00287                         si->copyState(states[pos0 + 1], s0);
00288                         states.erase(states.begin() + pos0 + 2, states.begin() + index1);
00289                     }
00290                     else
00291                         if (index0 >= 0 && index1 < 0)
00292                         {
00293                             if (freeStates_)
00294                                 for (int j = index0 + 1 ; j < pos1 ; ++j)
00295                                     si->freeState(states[j]);
00296                             si->copyState(states[pos1], s1);
00297                             states.erase(states.begin() + index0 + 1, states.begin() + pos1);
00298                         }
00299 
00300             // fix the helper variables
00301             dists.resize(states.size(), 0.0);
00302             for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
00303                 dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
00304             threshold = dists.back() * snapToVertex;
00305             rd = rangeRatio * dists.back();
00306             result = true;
00307             nochange = 0;
00308         }
00309     }
00310 
00311     si->freeState(temp1);
00312     si->freeState(temp0);
00313     return result;
00314 }
00315 
00316 bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
00317 {
00318     if (path.getStateCount() < 3)
00319         return false;
00320 
00321     if (maxSteps == 0)
00322         maxSteps = path.getStateCount();
00323 
00324     if (maxEmptySteps == 0)
00325         maxEmptySteps = path.getStateCount();
00326 
00327     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00328     std::vector<base::State*> &states = path.getStates();
00329 
00330     // compute pair-wise distances in path (construct only half the matrix)
00331     std::map<std::pair<const base::State*, const base::State*>, double> distances;
00332     for (unsigned int i = 0 ; i < states.size() ; ++i)
00333         for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00334             distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
00335 
00336     bool result = false;
00337     unsigned int nochange = 0;
00338     for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
00339     {
00340         // find closest pair of points
00341         double minDist = std::numeric_limits<double>::infinity();
00342         int p1 = -1;
00343         int p2 = -1;
00344         for (unsigned int i = 0 ; i < states.size() ; ++i)
00345             for (unsigned int j = i + 2 ; j < states.size() ; ++j)
00346             {
00347                 double d = distances[std::make_pair(states[i], states[j])];
00348                 if (d < minDist)
00349                 {
00350                     minDist = d;
00351                     p1 = i;
00352                     p2 = j;
00353                 }
00354             }
00355 
00356         if (p1 >= 0 && p2 >= 0)
00357         {
00358             if (si->checkMotion(states[p1], states[p2]))
00359             {
00360                 if (freeStates_)
00361                     for (int i = p1 + 1 ; i < p2 ; ++i)
00362                         si->freeState(states[i]);
00363                 states.erase(states.begin() + p1 + 1, states.begin() + p2);
00364                 result = true;
00365                 nochange = 0;
00366             }
00367             else
00368                 distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
00369         }
00370         else
00371             break;
00372     }
00373     return result;
00374 }
00375 
00376 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
00377 {
00378     reduceVertices(path);
00379     collapseCloseVertices(path);
00380     // BSpline and checkAndRepair code only makes sense in a metric space.
00381     if (si_->getStateSpace()->isMetricSpace())
00382     {
00383         smoothBSpline(path, 4, path.length()/100.0);
00384         const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00385         if (!p.second)
00386             OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
00387         else
00388             if (!p.first)
00389                 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00390     }
00391 }
00392 
00393 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, double maxTime)
00394 {
00395     simplify(path, base::timedPlannerTerminationCondition(maxTime));
00396 }
00397 
00398 void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
00399 {
00400     if (path.getStateCount() < 3)
00401         return;
00402 
00403     // try a randomized step of connecting vertices
00404     bool tryMore = false;
00405     if (ptc == false)
00406         tryMore = reduceVertices(path);
00407 
00408     // try to collapse close-by vertices
00409     if (ptc == false)
00410         collapseCloseVertices(path);
00411 
00412     // try to reduce verices some more, if there is any point in doing so
00413     int times = 0;
00414     while (tryMore && ptc == false && ++times <= 5)
00415         tryMore = reduceVertices(path);
00416 
00417     // if the space is metric, we can do some additional smoothing
00418     if(si_->getStateSpace()->isMetricSpace())
00419     {
00420         // run a more complex short-cut algorithm : allow splitting path segments
00421         if (ptc == false)
00422             tryMore = shortcutPath(path);
00423         else
00424             tryMore = false;
00425 
00426         // run the short-cut algorithm some more, if it makes a difference
00427         times = 0;
00428         while (tryMore && ptc == false && ++times <= 5)
00429             tryMore = shortcutPath(path);
00430 
00431         // smooth the path with BSpline interpolation
00432         if(ptc == false)
00433             smoothBSpline(path, 3, path.length()/100.0);
00434 
00435         // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
00436         const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00437         if (!p.second)
00438             OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
00439         else
00440             if (!p.first)
00441                 OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00442     }
00443 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines