bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
// compute pair-wise distances in path (construct only half the matrix)
std::map<std::pair<const base::State*, const base::State*>, double> distances;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
bool result = false;
unsigned int nochange = 0;
for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
{
// find closest pair of points
double minDist = std::numeric_limits<double>::infinity();
int p1 = -1;
int p2 = -1;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
{
double d = distances[std::make_pair(states[i], states[j])];
if (d < minDist)
{
minDist = d;
p1 = i;
p2 = j;
}
}
if (p1 >= 0 && p2 >= 0)
{
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int i = p1 + 1 ; i < p2 ; ++i)
si->freeState(states[i]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
result = true;
nochange = 0;
}
else
distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
}
else
break;
}
return result;
}
/* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
{
if (path.getStateCount() < 3)
return;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
base::State *temp1 = si->allocState();
base::State *temp2 = si->allocState();
for (unsigned int s = 0 ; s < maxSteps ; ++s)
{
path.subdivide();
unsigned int i = 2, u = 0, n1 = states.size() - 1;
while (i < n1)
{
if (si->isValid(states[i - 1]))
{
si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
{
if (si->distance(states[i], temp1) > minChange)
{
si->copyState(states[i], temp1);
++u;
}
}
}
i += 2;
}
if (u == 0)
break;
}
si->freeState(temp1);
si->freeState(temp2);
}
unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
{
PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
if (!p)
{
OMPL_ERROR("Path hybridization only works for geometric paths");
return 0;
}
if (p->getSpaceInformation() != si_)
{
OMPL_ERROR("Paths for hybridization must be from the same space information");
return 0;
}
// skip empty paths
if (p->getStateCount() == 0)
return 0;
PathInfo pi(pp);
// if this path was previously included in the hybridization, skip it
if (paths_.find(pi) != paths_.end())
return 0;
// the number of connection attempts
unsigned int nattempts = 0;
// start from virtual root
Vertex v0 = boost::add_vertex(g_);
stateProperty_[v0] = pi.states_[0];
pi.vertices_.push_back(v0);
// add all the vertices of the path, and the edges between them, to the HGraph
// also compute the path length for future use (just for computational savings)
const HGraph::edge_property_type prop0(0.0);
boost::add_edge(root_, v0, prop0, g_);
double length = 0.0;
for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
{
Vertex v1 = boost::add_vertex(g_);
stateProperty_[v1] = pi.states_[j];
double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
const HGraph::edge_property_type properties(weight);
boost::add_edge(v0, v1, properties, g_);
length += weight;
pi.vertices_.push_back(v1);
v0 = v1;
}
// connect to virtual goal
boost::add_edge(v0, goal_, prop0, g_);
pi.length_ = length;
// find matches with previously added paths
for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
{
const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
std::vector<int> indexP, indexQ;
matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
if (matchAcrossGaps)
{
int lastP = -1;
int lastQ = -1;
int gapStartP = -1;
int gapStartQ = -1;
bool gapP = false;
bool gapQ = false;
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
{
// a gap is found in p
if (indexP[i] < 0)
{
// remember this as the beginning of the gap, if needed
if (!gapP)
gapStartP = i;
// mark the fact we are now in a gap on p
gapP = true;
}
else
{
// check if a gap just ended;
// if it did, try to match the endpoint with the elements in q
if (gapP)
for (std::size_t j = gapStartP ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
++nattempts;
}
// remember the last non-negative index in p
lastP = i;
gapP = false;
}
if (indexQ[i] < 0)
{
if (!gapQ)
gapStartQ = i;
gapQ = true;
}
else
{
if (gapQ)
for (std::size_t j = gapStartQ ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
++nattempts;
}
lastQ = i;
gapQ = false;
}
// try to match corresponding index values and gep beginnings
if (lastP >= 0 && lastQ >= 0)
{
attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
++nattempts;
}
}
}
else
{
// attempt new edge only when states align
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
if (indexP[i] >= 0 && indexQ[i] >= 0)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
++nattempts;
}
}
}
// remember this path is part of the hybridization
paths_.insert(pi);
return nattempts;
}
bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
// dists[i] contains the cumulative length of the path up to and including state i
std::vector<double> dists(states.size(), 0.0);
for (unsigned int i = 1 ; i < dists.size() ; ++i)
dists[i] = dists[i - 1] + si->distance(states[i-1], states[i]);
// Sampled states closer than 'threshold' distance to any existing state in the path
// are snapped to the close state
double threshold = dists.back() * snapToVertex;
// The range (distance) of a single connection that will be attempted
double rd = rangeRatio * dists.back();
base::State *temp0 = si->allocState();
base::State *temp1 = si->allocState();
bool result = false;
unsigned int nochange = 0;
// Attempt shortcutting maxSteps times or when no improvement is found after
// maxEmptySteps attempts, whichever comes first
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
// Sample a random point anywhere along the path
base::State *s0 = nullptr;
int index0 = -1;
double t0 = 0.0;
double p0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0); // find the NEXT waypoint after the random point
int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos0 == 0 || dists[pos0] - p0 < threshold) // snap to the NEXT waypoint
index0 = pos0;
else
{
while (pos0 > 0 && p0 < dists[pos0])
--pos0;
if (p0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
index0 = pos0;
}
// Sample a random point within rd distance of the previously sampled point
base::State *s1 = nullptr;
int index1 = -1;
double t1 = 0.0;
double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back())); // sample a random point (p1) near p0
pit = std::lower_bound(dists.begin(), dists.end(), p1); // find the NEXT waypoint after the random point
int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos1 == 0 || dists[pos1] - p1 < threshold) // snap to the NEXT waypoint
index1 = pos1;
else
{
while (pos1 > 0 && p1 < dists[pos1])
--pos1;
if (p1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
index1 = pos1;
}
// Don't waste time on points that are on the same path segment
if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
pos0 + 1 == index1 || pos1 + 1 == index0 ||
(index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
continue;
// Get the state pointer for p0
if (index0 >= 0)
s0 = states[index0];
else
{
t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
s0 = temp0;
}
// Get the state pointer for p1
if (index1 >= 0)
s1 = states[index1];
else
{
t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
s1 = temp1;
}
// Check for validity between s0 and s1
if (si->checkMotion(s0, s1))
{
if (pos0 > pos1)
{
std::swap(pos0, pos1);
std::swap(index0, index1);
std::swap(s0, s1);
std::swap(t0, t1);
}
// Modify the path with the new, shorter result
if (index0 < 0 && index1 < 0)
{
if (pos0 + 1 == pos1)
{
si->copyState(states[pos1], s0);
states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
}
else
{
if (freeStates_)
for (int j = pos0 + 2 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
si->copyState(states[pos1], s1);
states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
}
}
else
if (index0 >= 0 && index1 >= 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < index1 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + index0 + 1, states.begin() + index1);
}
else
if (index0 < 0 && index1 >= 0)
{
if (freeStates_)
for (int j = pos0 + 2 ; j < index1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
states.erase(states.begin() + pos0 + 2, states.begin() + index1);
}
else
if (index0 >= 0 && index1 < 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos1], s1);
states.erase(states.begin() + index0 + 1, states.begin() + pos1);
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
threshold = dists.back() * snapToVertex;
rd = rangeRatio * dists.back();
result = true;
nochange = 0;
}
}
si->freeState(temp1);
si->freeState(temp0);
return result;
}
bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
bool result = false;
unsigned int nochange = 0;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
if (si->checkMotion(states.front(), states.back()))
{
if (freeStates_)
for (std::size_t i = 2 ; i < states.size() ; ++i)
si->freeState(states[i-1]);
std::vector<base::State*> newStates(2);
newStates[0] = states.front();
newStates[1] = states.back();
states.swap(newStates);
result = true;
}
else
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
int count = states.size();
int maxN = count - 1;
int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
int p1 = rng_.uniformInt(0, maxN);
int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
if (abs(p1 - p2) < 2)
{
if (p1 < maxN - 1)
p2 = p1 + 2;
else
if (p1 > 1)
p2 = p1 - 2;
else
continue;
}
if (p1 > p2)
std::swap(p1, p2);
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int j = p1 + 1 ; j < p2 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
nochange = 0;
result = true;
}
}
return result;
}