示例#1
0
int GenSOM::trainSingle(const multi_img::Pixel &input, int iter, int max)
{
	// adjust learning rate and radius
	// note that they are _decreasing_ -> start * (end/start)^(iter%)
	double learnRate = config.learnStart * std::pow(
				config.learnEnd / config.learnStart,
				(double)iter/(double)max);
	double sigma = config.sigmaStart * std::pow(
				config.sigmaEnd / config.sigmaStart,
				(double)iter/(double)max);

	// find best matching unit to given input vector
	size_t index = findBMU(input).index;

	// increase winning count of neuron
	//m_bmuMap(pos) += 1.0;

	int updates = updateNeighborhood(index, input, sigma, learnRate);

	return updates;
}
示例#2
0
文件: FMT.cpp 项目: RickOne16/ompl
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
    if (lastGoalMotion_) {
        OMPL_INFORM("solve() called before clear(); returning previous solution");
        traceSolutionPathThroughTree(lastGoalMotion_);
        OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
        return base::PlannerStatus(true, false);
    }
    else if (Open_.size() > 0)
    {
        OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
        clear();
    }

    checkValidity();
    base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
    Motion *initMotion = nullptr;

    if (!goal)
    {
        OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
        return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
    }

    // Add start states to V (nn_) and Open
    while (const base::State *st = pis_.nextStart())
    {
        initMotion = new Motion(si_);
        si_->copyState(initMotion->getState(), st);
        Open_.insert(initMotion);
        initMotion->setSetType(Motion::SET_OPEN);
        initMotion->setCost(opt_->initialCost(initMotion->getState()));
        nn_->add(initMotion); // V <-- {x_init}
    }

    if (!initMotion)
    {
        OMPL_ERROR("Start state undefined");
        return base::PlannerStatus::INVALID_START;
    }

    // Sample N free states in the configuration space
    if (!sampler_)
        sampler_ = si_->allocStateSampler();
    sampleFree(ptc);
    assureGoalIsSampled(goal);
    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    // Calculate the nearest neighbor search radius
    /// \todo Create a PRM-like connection strategy
    if (nearestK_)
    {
        NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
                        (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
                        log((double)nn_->size()));
        OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
    }
    else
    {
        NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
        OMPL_DEBUG("Using radius of %f", NNr_);
    }

    // Execute the planner, and return early if the planner returns a failure
    bool plannerSuccess = false;
    bool successfulExpansion = false;
    Motion *z = initMotion; // z <-- xinit
    saveNeighborhood(z);

    while (!ptc)
    {
        if ((plannerSuccess = goal->isSatisfied(z->getState())))
            break;

        successfulExpansion = expandTreeFromNode(&z);

        if (!extendedFMT_ && !successfulExpansion)
            break;
        else if (extendedFMT_ && !successfulExpansion)
        {
            //Apply RRT*-like connections: sample and connect samples to tree
            std::vector<Motion*>       nbh;
            std::vector<base::Cost>    costs;
            std::vector<base::Cost>    incCosts;
            std::vector<std::size_t>   sortedCostIndices;

            // our functor for sorting nearest neighbors
            CostIndexCompare compareFn(costs, *opt_);

            Motion *m = new Motion(si_);
            while (!ptc && Open_.empty())
            {
                sampler_->sampleUniform(m->getState());

                if (!si_->isValid(m->getState()))
                    continue;

                if (nearestK_)
                    nn_->nearestK(m, NNk_, nbh);
                else
                    nn_->nearestR(m, NNr_, nbh);

                // Get neighbours in the tree.
                std::vector<Motion*> yNear;
                yNear.reserve(nbh.size());
                for (std::size_t j = 0; j < nbh.size(); ++j)
                {
                    if (nbh[j]->getSetType() == Motion::SET_CLOSED)
                    {
                        if (nearestK_)
                        {
                            // Only include neighbors that are mutually k-nearest
                            // Relies on NN datastructure returning k-nearest in sorted order
                            const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
                            const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());

                            if (opt_->isCostBetterThan(worstCost, connCost))
                                continue;
                            else
                                yNear.push_back(nbh[j]);
                        }
                        else
                            yNear.push_back(nbh[j]);
                    }
                }

                // Sample again if the new sample does not connect to the tree.
                if (yNear.empty())
                    continue;

                // cache for distance computations
                //
                // Our cost caches only increase in size, so they're only
                // resized if they can't fit the current neighborhood
                if (costs.size() < yNear.size())
                {
                    costs.resize(yNear.size());
                    incCosts.resize(yNear.size());
                    sortedCostIndices.resize(yNear.size());
                }

                // Finding the nearest neighbor to connect to
                // By default, neighborhood states are sorted by cost, and collision checking
                // is performed in increasing order of cost
                //
                // calculate all costs and distances
                for (std::size_t i = 0 ; i < yNear.size(); ++i)
                {
                    incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
                    costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
                }

                // sort the nodes
                //
                // we're using index-value pairs so that we can get at
                // original, unsorted indices
                for (std::size_t i = 0; i < yNear.size(); ++i)
                    sortedCostIndices[i] = i;
                std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
                          compareFn);

               // collision check until a valid motion is found
               for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
                    i != sortedCostIndices.begin() + yNear.size();
                    ++i)
               {
                   if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
                   {
                       m->setParent(yNear[*i]);
                       yNear[*i]->getChildren().push_back(m);
                       const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
                       m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
                       m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
                       m->setSetType(Motion::SET_OPEN);

                       nn_->add(m);
                       saveNeighborhood(m);
                       updateNeighborhood(m,nbh);

                       Open_.insert(m);
                       z = m;
                       break;
                   }
               }
            } // while (!ptc && Open_.empty())
        } // else if (extendedFMT_ && !successfulExpansion)
    } // While not at goal

    if (plannerSuccess)
    {
        // Return the path to z, since by definition of planner success, z is in the goal region
        lastGoalMotion_ = z;
        traceSolutionPathThroughTree(lastGoalMotion_);

        OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());

        return base::PlannerStatus(true, false);
    } // if plannerSuccess
    else
    {
        // Planner terminated without accomplishing goal
        return base::PlannerStatus(false, false);
    }
}