C++ PathGeometric::getState方法代码示例

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
                {
//.........这里部分代码省略.........