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; }