void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapCost,
std::vector<int> &indexP, std::vector<int> &indexQ) const
{
std::vector<std::vector<double> > C(p.getStateCount());
std::vector<std::vector<char> > T(p.getStateCount());
for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
{
C[i].resize(q.getStateCount(), 0.0);
T[i].resize(q.getStateCount(), '\0');
for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
{
// as far as I can tell, there is a bug in the algorithm as presented in the paper
// so I am doing things slightly differently ...
double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
if (match <= up && match <= left)
{
C[i][j] = match;
T[i][j] = 'm';
}
else
if (up <= match && up <= left)
{
C[i][j] = up;
T[i][j] = 'u';
}
else
{
C[i][j] = left;
T[i][j] = 'l';
}
}
}
// construct the sequences with gaps (only index positions)
int m = p.getStateCount() - 1;
int n = q.getStateCount() - 1;
indexP.clear();
indexQ.clear();
indexP.reserve(std::max(n,m));
indexQ.reserve(indexP.size());
while (n >= 0 && m >= 0)
{
if (T[m][n] == 'm')
{
indexP.push_back(m);
indexQ.push_back(n);
--m; --n;
}
else
if (T[m][n] == 'u')
{
indexP.push_back(m);
indexQ.push_back(-1);
--m;
}
else
{
indexP.push_back(-1);
indexQ.push_back(n);
--n;
}
}
while (n >= 0)
{
indexP.push_back(-1);
indexQ.push_back(n);
--n;
}
while (m >= 0)
{
indexP.push_back(m);
indexQ.push_back(-1);
--m;
}
}
bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
unsigned int samplingAttempts, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 2)
return false;
if (!gsr_)
{
OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
return false;
}
unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
unsigned int failedTries = 0;
bool betterGoal = false;
const base::StateSpacePtr& ss = si_->getStateSpace();
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* temp = si_->allocState();
base::State* tempGoal = si_->allocState();
while(!ptc && failedTries++ < maxGoals && !betterGoal)
{
gsr_->sampleGoal(tempGoal);
// Goal state is not compatible with the start state
if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
continue;
unsigned int numSamples = 0;
while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
{
// sample a state within rangeRatio
double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0), dists.back()); // Sample a random point within rd of the end of the path
std::vector<double>::iterator end = std::lower_bound(dists.begin(), dists.end(), t);
std::vector<double>::iterator start = end;
while(start != dists.begin() && *start >= t)
start -= 1;
unsigned int startIndex = start - dists.begin();
unsigned int endIndex = end - dists.begin();
// Snap the random point to the nearest vertex, if within the threshold
if (t - (*start) < threshold) // snap to the starting waypoint
endIndex = startIndex;
if ((*end) - t < threshold) // snap to the ending waypoint
startIndex = endIndex;
// Compute the state value and the accumulated cost up to that state
double costToCome = dists[startIndex];
base::State* state;
if (startIndex == endIndex)
{
state = states[startIndex];
}
else
{
double tSeg = (t - (*start)) / (*end - *start);
ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
state = temp;
costToCome += si_->distance(states[startIndex], state);
}
double costToGo = si_->distance(state, tempGoal);
double candidateCost = costToCome + costToGo;
// Make sure we improve before attempting validation
if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() && si_->checkMotion(state, tempGoal))
{
// insert the new states
if (startIndex == endIndex)
{
// new intermediate state
si_->copyState(states[startIndex], state);
// new goal state
si_->copyState(states[startIndex+1], tempGoal);
if (freeStates_)
{
for(size_t i = startIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + startIndex + 2, states.end());
}
else
{
// overwriting the end of the segment with the new state
si_->copyState(states[endIndex], state);
if (endIndex == states.size()-1)
{
path.append(tempGoal);
}
else
{
// adding goal new goal state
si_->copyState(states[endIndex + 1], tempGoal);
if (freeStates_)
{
for(size_t i = endIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + endIndex + 2, states.end());
}
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = std::max(1u, startIndex); j < dists.size() ; ++j)
dists[j] = dists[j-1] + si_->distance(states[j-1], states[j]);
betterGoal = true;
}
}
}
si_->freeState(temp);
si_->freeState(tempGoal);
return betterGoal;
}