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.states.size() < 3)
00049         return;
00050 
00051     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00052     base::State *temp1 = si->allocState();
00053     base::State *temp2 = si->allocState();
00054 
00055     for (unsigned int s = 0 ; s < maxSteps ; ++s)
00056     {
00057         path.subdivide();
00058 
00059         unsigned int i = 2, u = 0, n1 = path.states.size() - 1;
00060         while (i < n1)
00061         {
00062             if (si->isValid(path.states[i - 1]))
00063             {
00064                 si->getStateSpace()->interpolate(path.states[i - 1], path.states[i], 0.5, temp1);
00065                 si->getStateSpace()->interpolate(path.states[i], path.states[i + 1], 0.5, temp2);
00066                 si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
00067                 if (si->checkMotion(path.states[i - 1], temp1) && si->checkMotion(temp1, path.states[i + 1]))
00068                 {
00069                     if (si->distance(path.states[i], temp1) > minChange)
00070                     {
00071                         si->copyState(path.states[i], temp1);
00072                         ++u;
00073                     }
00074                 }
00075             }
00076 
00077             i += 2;
00078         }
00079 
00080         if (u == 0)
00081             break;
00082     }
00083 
00084     si->freeState(temp1);
00085     si->freeState(temp2);
00086 }
00087 
00088 void ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
00089 {
00090     if (path.states.size() < 3)
00091         return;
00092 
00093     if (maxSteps == 0)
00094         maxSteps = path.states.size();
00095 
00096     if (maxEmptySteps == 0)
00097         maxEmptySteps = path.states.size();
00098 
00099     unsigned int nochange = 0;
00100     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00101 
00102     for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
00103     {
00104         int count = path.states.size();
00105         int maxN  = count - 1;
00106         int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
00107 
00108         int p1 = rng_.uniformInt(0, maxN);
00109         int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
00110         if (abs(p1 - p2) < 2)
00111         {
00112             if (p1 < maxN - 1)
00113                 p2 = p1 + 2;
00114             else
00115                 if (p1 > 1)
00116                     p2 = p1 - 2;
00117                 else
00118                     continue;
00119         }
00120 
00121         if (p1 > p2)
00122             std::swap(p1, p2);
00123 
00124         if (si->checkMotion(path.states[p1], path.states[p2]))
00125         {
00126             for (int i = p1 + 1 ; i < p2 ; ++i)
00127                 si->freeState(path.states[i]);
00128             path.states.erase(path.states.begin() + p1 + 1, path.states.begin() + p2);
00129             nochange = 0;
00130         }
00131     }
00132 }
00133 
00134 void ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
00135 {
00136     if (path.states.size() < 3)
00137         return;
00138 
00139     if (maxSteps == 0)
00140         maxSteps = path.states.size();
00141 
00142     if (maxEmptySteps == 0)
00143         maxEmptySteps = path.states.size();
00144 
00145     const base::SpaceInformationPtr &si = path.getSpaceInformation();
00146 
00147     // compute pair-wise distances in path (construct only half the matrix)
00148     std::map<std::pair<const base::State*, const base::State*>, double> distances;
00149     for (unsigned int i = 0 ; i < path.states.size() ; ++i)
00150         for (unsigned int j = i + 2 ; j < path.states.size() ; ++j)
00151             distances[std::make_pair(path.states[i], path.states[j])] = si->distance(path.states[i], path.states[j]);
00152 
00153     unsigned int nochange = 0;
00154 
00155     for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
00156     {
00157         // find closest pair of points
00158         double minDist = std::numeric_limits<double>::infinity();
00159         int p1 = -1;
00160         int p2 = -1;
00161         for (unsigned int i = 0 ; i < path.states.size() ; ++i)
00162             for (unsigned int j = i + 2 ; j < path.states.size() ; ++j)
00163             {
00164                 double d = distances[std::make_pair(path.states[i], path.states[j])];
00165                 if (d < minDist)
00166                 {
00167                     minDist = d;
00168                     p1 = i;
00169                     p2 = j;
00170                 }
00171             }
00172 
00173         if (p1 >= 0 && p2 >= 0)
00174         {
00175             if (si->checkMotion(path.states[p1], path.states[p2]))
00176             {
00177                 for (int i = p1 + 1 ; i < p2 ; ++i)
00178                     si->freeState(path.states[i]);
00179                 path.states.erase(path.states.begin() + p1 + 1, path.states.begin() + p2);
00180                 nochange = 0;
00181             }
00182             else
00183                 distances[std::make_pair(path.states[p1], path.states[p2])] = std::numeric_limits<double>::infinity();
00184         }
00185         else
00186             break;
00187     }
00188 }
00189 
00190 void ompl::geometric::PathSimplifier::simplifyMax(PathGeometric &path)
00191 {
00192     reduceVertices(path);
00193     collapseCloseVertices(path);
00194     smoothBSpline(path, 5, path.length()/100.0);
00195     const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
00196     if (!p.second)
00197         msg_.warn("Solution path may slightly touch on an invalid region of the state space");
00198     else
00199         if (!p.first)
00200             msg_.debug("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
00201 }