From f0ac2ebcaab9c44019718a851697dcf9537c58b3 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 14 Dec 2015 17:18:06 +0100
Subject: [PATCH] tryConnectToRoadmap uses only the 7 nearest neighbors
* The number of nearest neighbor is empirical, with no particular test
---
src/manipulation-planner.cc | 8 ++++++--
1 file changed, 6 insertions(+), 2 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 18aabaa..c43cfb6 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -274,6 +274,8 @@ namespace hpp {
graph::GraphPtr_t graph = problem_.constraintGraph ();
bool connectSucceed = false;
std::size_t nbConnection = 0;
+ const std::size_t K = 7;
+ value_type distance;
for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
itn1 != nodes.end (); ++itn1) {
ConfigurationPtr_t q1 ((*itn1)->configuration ());
@@ -283,8 +285,10 @@ namespace hpp {
itcc != roadmap ()->connectedComponents ().end (); ++itcc) {
if (*itcc == (*itn1)->connectedComponent ())
continue;
- for (core::Nodes_t::const_iterator itn2 = (*itcc)->nodes().begin ();
- itn2 != (*itcc)->nodes ().end (); ++itn2) {
+ core::Nodes_t knearest = roadmap()->nearestNeighbor ()
+ ->KnearestSearch (q1, *itcc, K, distance);
+ for (core::Nodes_t::const_iterator itn2 = knearest.begin ();
+ itn2 != knearest.end (); ++itn2) {
bool _1to2 = (*itn1)->isOutNeighbor (*itn2);
bool _2to1 = (*itn1)->isInNeighbor (*itn2);
if (_1to2 && _2to1) {
--
GitLab