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