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

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

void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
    if (path.getStateCount() < 3)
        return;

    // try a randomized step of connecting vertices
    bool tryMore = false;
    if (ptc == false)
        tryMore = reduceVertices(path);

    // try to collapse close-by vertices
    if (ptc == false)
        collapseCloseVertices(path);

    // try to reduce verices some more, if there is any point in doing so
    int times = 0;
    while (tryMore && ptc == false && ++times <= 5)
        tryMore = reduceVertices(path);

    // if the space is metric, we can do some additional smoothing
    if(si_->getStateSpace()->isMetricSpace())
    {
        bool tryMore = true;
        unsigned int times = 0;
        do
        {
            bool shortcut = shortcutPath(path);                             // split path segments, not just vertices
            bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false;    // Try to connect the path to a closer goal

            tryMore = shortcut || better_goal;
        } while(ptc == false && tryMore && ++times <= 5);

        // smooth the path with BSpline interpolation
        if(ptc == false)
            smoothBSpline(path, 3, path.length()/100.0);

        // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
        const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
        if (!p.second)
            OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
        else
            if (!p.first)
                OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
    }
}

/* 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);
}

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

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

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

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

bool ompl::geometric::LightningRetrieveRepair::replan(const ompl::base::State *start, const ompl::base::State *goal,
    PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
{
    // Reset problem definition
    repairProblemDef_->clearSolutionPaths();
    repairProblemDef_->clearStartStates();
    repairProblemDef_->clearGoal();

    // Reset planner
    repairPlanner_->clear();

    // Configure problem definition
    repairProblemDef_->setStartAndGoalStates(start, goal);

    // Configure planner
    repairPlanner_->setProblemDefinition(repairProblemDef_);

    // Solve
    OMPL_INFORM("LightningRetrieveRepair: Preparing to repair path");
    base::PlannerStatus lastStatus = base::PlannerStatus::UNKNOWN;
    time::point startTime = time::now();

    lastStatus = repairPlanner_->solve(ptc);

    // Results
    double planTime = time::seconds(time::now() - startTime);
    if (!lastStatus)
    {
        OMPL_INFORM("LightningRetrieveRepair: No replan solution between disconnected states found after %f seconds", planTime);
        return false;
    }

    // Check if approximate
    if (repairProblemDef_->hasApproximateSolution() || repairProblemDef_->getSolutionDifference() > std::numeric_limits<double>::epsilon())
    {
        OMPL_INFORM("LightningRetrieveRepair: Solution is approximate, not using");
        return false;
    }

    // Convert solution into a PathGeometric path
    base::PathPtr p = repairProblemDef_->getSolutionPath();
    if (!p)
    {
        OMPL_ERROR("LightningRetrieveRepair: Unable to get solution path from problem definition");
        return false;
    }

    newPathSegment = static_cast<ompl::geometric::PathGeometric&>(*p);

    // Smooth the result
    OMPL_INFORM("LightningRetrieveRepair: Simplifying solution (smoothing)...");
    time::point simplifyStart = time::now();
    std::size_t numStates = newPathSegment.getStateCount();
    psk_->simplify(newPathSegment, ptc);
    double simplifyTime = time::seconds(time::now() - simplifyStart);
    OMPL_INFORM("LightningRetrieveRepair: Path simplification took %f seconds and removed %d states",
        simplifyTime, numStates - newPathSegment.getStateCount());

    // Save the planner data for debugging purposes
    repairPlannerDatas_.push_back(ompl::base::PlannerDataPtr( new ompl::base::PlannerData(si_) ));
    repairPlanner_->getPlannerData( *repairPlannerDatas_.back() );
    repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we don't lose them

    // Return success
    OMPL_INFORM("LightningRetrieveRepair: solution found in %f seconds with %d states",
        planTime, newPathSegment.getStateCount() );

    return true;
}