00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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 }