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