ompl::geometric::BasicPRMmodif::Milestone* ompl::geometric::BasicPRMmodif::addMilestone(base::State *state)
{
Milestone *m = new Milestone();
m->state = state;
m->component = componentCount_;
componentSizes_[m->component] = 1;
// connect to nearest neighbors
std::vector<Milestone*> nbh;
nearestNeighbors(m, nbh);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (si_->checkMotion(m->state, nbh[i]->state))
{
m->adjacent.push_back(nbh[i]);
nbh[i]->adjacent.push_back(m);
m->costs.push_back(si_->distance(m->state, nbh[i]->state));
// std::cout << si_->distance(m->state, nbh[i]->state) << std::endl;
nbh[i]->costs.push_back(m->costs.back());
uniteComponents(m, nbh[i]);
}
// if the new milestone was no absorbed in an existing component,
// increase the number of components
if (m->component == componentCount_)
componentCount_++;
m->index = milestones_.size();
milestones_.push_back(m);
nn_->add(m);
return m;
}
ompl::geometric::LazyPRM::Vertex ompl::geometric::LazyPRM::addMilestone(base::State *state)
{
Vertex m = boost::add_vertex(g_);
stateProperty_[m] = state;
vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
unsigned long int newComponent = componentCount_++;
vertexComponentProperty_[m] = newComponent;
componentSize_[newComponent] = 1;
// Which milestones will we attempt to connect to?
const std::vector<Vertex> &neighbors = connectionStrategy_(m);
foreach (Vertex n, neighbors)
if (connectionFilter_(m, n))
{
const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
const Graph::edge_property_type properties(weight);
const Edge &e = boost::add_edge(m, n, properties, g_).first;
edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
uniteComponents(m, n);
}
nn_->add(m);
return m;
}
void FormSegmentator::analyze(QList<Component> &objects, QList<Component> &edges)
{
uniteComponents();
objects = getObjects();
edges = getEdges();
return;
}
