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