Create a abstract class NearestNeighbor for KDTree

  - two implementations: nearestNeighbor::KDTree and nearestNeighbor::Basic.
  - update test accordingly.
  - use nearestNeighbor::Basic by default since nearestNeighbor::KDtree fails
    with benchmark 2014/09/27/pr2_in_iai_kitchen.

include/hpp/core/fwd.hh

@@ -150,7 +150,6 @@ namespace hpp {
     typedef model::vectorIn_t vectorIn_t;
     typedef model::vectorOut_t vectorOut_t;
     typedef boost::shared_ptr <WeighedDistance> WeighedDistancePtr_t;
-    typedef KDTree* KDTreePtr_t;
     typedef std::map <std::string, DifferentiableFunctionPtr_t>
     DifferentiableFunctionMap_t;
     // Collision pairs
@@ -165,6 +164,14 @@ namespace hpp {
       typedef boost::shared_ptr <Progressive> ProgressivePtr_t;
     } // namespace continuousCollisionChecking
 
+    class NearestNeighbor;
+    typedef NearestNeighbor* NearestNeighborPtr_t;
+    namespace nearestNeighbor {
+      class Basic;
+      class KDTree;
+      typedef KDTree* KDTreePtr_t;
+      typedef Basic* BasicPtr_t;
+    } // namespace nearestNeighbor
   } // namespace core
 } // namespace hpp
 

include/hpp/core/roadmap.hh

@@ -22,7 +22,6 @@
 # include <iostream>
 # include <hpp/core/fwd.hh>
 # include <hpp/core/config.hh>
-# include <hpp/core/k-d-tree.hh>
 
 namespace hpp {
   namespace core {
@@ -170,7 +169,7 @@ namespace hpp {
       Edges_t edges_;
       NodePtr_t initNode_;
       Nodes_t goalNodes_;
-      KDTreePtr_t kdTree_;
+      NearestNeighborPtr_t nearestNeighbor_;
 
     }; // class Roadmap
     std::ostream& operator<< (std::ostream& os, const Roadmap& r);

src/CMakeLists.txt

@@ -37,6 +37,9 @@ SET(${LIBRARY_NAME}_SOURCES
   extracted-path.hh
   joint-bound-validation.cc
   nearest-neighbor.hh
+  nearest-neighbor/basic.hh
+  nearest-neighbor/k-d-tree.cc
+  nearest-neighbor/k-d-tree.hh
   node.cc
   path.cc
   path-planner.cc
@@ -48,8 +51,6 @@ SET(${LIBRARY_NAME}_SOURCES
   roadmap.cc
   straight-path.cc
   weighed-distance.cc
-  k-d-tree.cc
-  k-d-tree.hh
   )
 
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)

src/nearest-neighbor.hh

@@ -19,114 +19,28 @@
 #ifndef HPP_CORE_NEAREST_NEIGHBOR_HH
 # define HPP_CORE_NEAREST_NEIGHBOR_HH
 
-# include <limits>
 # include <hpp/core/fwd.hh>
-# include <hpp/core/distance.hh>
 
 namespace hpp {
   namespace core {
-    HPP_PREDEF_CLASS (NearestNeighbor);
-    typedef boost::shared_ptr <NearestNeighbor> NearestNeighborPtr_t;
-
     /// Optimization of the nearest neighbor search
     class NearestNeighbor
     {
     public:
+      virtual void clear () = 0;
+      virtual void addNode (const NodePtr_t& node) = 0;
+      virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
+			       const ConnectedComponentPtr_t&
+				connectedComponent,
+			       value_type& distance) = 0;
 
-      NearestNeighbor(const DistancePtr_t& distance) : distance_ (distance)
-      {
-      }
-
-      ~NearestNeighbor()
-      {
-      }
-
-      // void distance (const DistancePtr_t& distance)
-      // {
-      // 	distance_ = distance;
-      // }
-
-      DistancePtr_t distance () const
-      {
-	return distance_;
-      }
-
-      void clear ()
-      {
-	nodes_.clear ();
-      }
-
-      void add (const NodePtr_t& node)
-      {
-	nodes_.push_back (node);
-      }
-
-      void merge (const NearestNeighborPtr_t& other)
-      {
-	nodes_.splice (nodes_.end (), other->nodes_);
-      }
-
-      bool remove (const NodePtr_t& node)
-      {
-	Nodes_t::iterator itConfig =
-	  std::find (nodes_.begin (), nodes_.end (), node);
-	if (itConfig == nodes_.end ()) {
-	  throw std::runtime_error ("Attempt to remove a node that is"
-				    " not in nearest neighbor data-structure.");
-	}
-	nodes_.erase (itConfig);
-      }
-
-      NodePtr_t nearest (const ConfigurationPtr_t& configuration,
-			 value_type& minDistance)
-      {
-	assert (!nodes_.empty ());
-	minDistance = std::numeric_limits <value_type>::infinity ();
-	Nodes_t::iterator itClosest = nodes_.end ();
-	for (Nodes_t::iterator itNode = nodes_.begin ();
-	     itNode != nodes_.end (); itNode ++) {
-	  value_type distance = (*distance_) (*configuration,
-					  *((*itNode)->configuration ()));
-	  if (distance < minDistance) {
-	    minDistance = distance;
-	    itClosest = itNode;
-	  }
-	}
-	assert (itClosest != nodes_.end ());
-	return *itClosest;
-      }
+      // merge two connected components in the whole tree
+      virtual void merge (ConnectedComponentPtr_t cc1,
+			  ConnectedComponentPtr_t cc2) = 0;
 
-      ///  Return the k nearest neighbors in sorted order
-      void nearest (const ConfigurationPtr_t& configuration, std::size_t k,
-		    Nodes_t& nearestNeighbors) const
-      {
-	Nodes_t copy;
-	std::copy (nodes_.begin (), nodes_.end (), copy.begin ());
-	nearestNeighbors.clear ();
-	while (nearestNeighbors.size () < k) {
-	  value_type minDistance = std::numeric_limits <value_type>::infinity ();
-	  Nodes_t::iterator itClosest = copy.end ();
-	  for (Nodes_t::iterator itNode = copy.begin ();
-	       itNode != copy.end (); itNode ++) {
-	    value_type distance = (*distance_) (*configuration,
-					    *((*itNode)->configuration ()));
-	    if (distance < minDistance) {
-	      minDistance = distance;
-	      itClosest = itNode;
-	    }
-	  }
-	  if (itClosest != copy.end ()) {
-	    nearestNeighbors.push_back (*itClosest);
-	    copy.erase (itClosest);
-	  } else {
-	    k = 0; // if k is more than number of configs, stop
-	  }
-	}
-      }
+      // Get distance function
+      virtual DistancePtr_t distance () const = 0;
 
-    private:
-      Nodes_t nodes_;
-      const DistancePtr_t& distance_;
     }; // class NearestNeighbor
   } // namespace core
 } // namespace hpp

src/nearest-neighbor/basic.hh

+//
+// Copyright (c) 2014 CNRS
+// Authors: Florent Lamiraux
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_CORE_NEAREST_NEIGHBOR_BASIC_HH
+# define HPP_CORE_NEAREST_NEIGHBOR_BASIC_HH
+
+# include <limits>
+# include <hpp/core/fwd.hh>
+# include <hpp/core/distance.hh>
+# include "../src/nearest-neighbor.hh"
+
+namespace hpp {
+  namespace core {
+    namespace nearestNeighbor {
+    /// Optimization of the nearest neighbor search
+    class Basic : public NearestNeighbor
+    {
+    public:
+
+      Basic(const DistancePtr_t& distance) : distance_ (distance)
+      {
+      }
+
+      ~Basic()
+      {
+      }
+
+      virtual void clear ()
+      {
+      }
+
+      void addNode (const NodePtr_t&)
+      {
+      }
+
+      virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
+			       const ConnectedComponentPtr_t&
+				connectedComponent,
+			       value_type& distance)
+      {
+	NodePtr_t result;
+	distance = std::numeric_limits <value_type>::infinity ();
+	for (Nodes_t::const_iterator itNode =
+	       connectedComponent->nodes ().begin ();
+	     itNode != connectedComponent->nodes ().end (); ++itNode) {
+	  value_type d = (*distance_) (*(*itNode)->configuration (),
+				       *configuration);
+	  if (d < distance) {
+	    distance = d;
+	    result = *itNode;
+	  }
+	}
+	assert (result);
+	return result;
+      }
+
+      virtual void merge (ConnectedComponentPtr_t, ConnectedComponentPtr_t)
+      {
+      }
+
+      // Get distance function
+      virtual DistancePtr_t distance () const
+      {
+	return distance_;
+      }
+
+    private:
+      const DistancePtr_t& distance_;
+    }; // class Basic
+    } // namespace nearestNeighbor
+  } // namespace core
+} // namespace hpp
+
+#endif // HPP_CORE_NEAREST_NEIGHBOR_BASIC_HH

src/k-d-tree.cc → src/nearest-neighbor/k-d-tree.cc

@@ -16,22 +16,22 @@
 // hpp-core  If not, see
 // <http://www.gnu.org/licenses/>.
 
-# include <hpp/util/debug.hh>
-# include <hpp/core/distance.hh>
-# include <hpp/core/node.hh>
-# include <hpp/model/joint.hh>
-# include <hpp/model/joint-configuration.hh>
-# include <hpp/model/device.hh>
-# include <hpp/core/weighed-distance.hh>
-#include "k-d-tree.hh"
 #include <iostream>
 #include <fstream>
+#include <hpp/util/debug.hh>
+#include <hpp/core/distance.hh>
+#include <hpp/core/node.hh>
+#include <hpp/model/joint.hh>
+#include <hpp/model/joint-configuration.hh>
+#include <hpp/model/device.hh>
+#include <hpp/core/weighed-distance.hh>
+#include "../src/nearest-neighbor/k-d-tree.hh"
 
 using namespace std;
 
 namespace hpp {
   namespace core {
-
+    namespace nearestNeighbor {
     // Constructor with the mother tree node (same bounds)
     KDTree::KDTree (const KDTreePtr_t mother, size_type splitDim) :
       robot_(mother->robot_),
@@ -255,6 +255,7 @@ namespace hpp {
       minDistance = std::numeric_limits <value_type>::infinity ();
       this->search (boxDistance, minDistance, configuration,
 		    connectedComponent, nearest);
+      assert (nearest);
       return nearest;
     }
 
@@ -330,5 +331,6 @@ namespace hpp {
 	supChild_->merge(cc1, cc2);
       }
     }
-  }
-}
+    } // namespace nearestNeighbor
+  } // namespace core
+} // namespace hpp

src/k-d-tree.hh → src/nearest-neighbor/k-d-tree.hh

@@ -16,22 +16,22 @@
 // hpp-core  If not, see
 // <http://www.gnu.org/licenses/>.
 
-#ifndef HPP_CORE_K_D_TREE_HH
-# define HPP_CORE_K_D_TREE_HH
+#ifndef HPP_CORE_NEAREST_NEIGHBOR_K_D_TREE_HH
+# define HPP_CORE_NEAREST_NEIGHBOR_K_D_TREE_HH
 
 # include <hpp/core/distance.hh>
 # include <hpp/core/node.hh>
 # include <hpp/model/joint.hh>
 # include <hpp/model/joint-configuration.hh>
 # include <hpp/model/device.hh>
-# include <hpp/core/fwd.hh>
+# include "../src/nearest-neighbor.hh"
 
 namespace hpp {
   namespace core {
+    namespace nearestNeighbor {
     // Built an k-dimentional tree for the nearest neighbour research
-    class KDTree
+    class KDTree : public NearestNeighbor
     {
-      // typedef KDTree* KDTreePtr_t;
     public:
 
       //constructor
@@ -43,19 +43,24 @@ namespace hpp {
       ~KDTree();
 
       // add a configuration in the KDTree
-      void addNode(const NodePtr_t& node);
+      virtual void addNode(const NodePtr_t& node);
 
       // Clear all the nodes in the KDTree
-      void clear();
+      virtual void clear();
 
       // search nearest node
-      NodePtr_t search(const ConfigurationPtr_t& configuration,
-		       const ConnectedComponentPtr_t& connectedComponent,
-		       value_type& minDistance);
+      virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
+			        const ConnectedComponentPtr_t&
+				connectedComponent,
+				value_type& minDistance);
 
       // merge two connected components in the whole tree
       void merge(ConnectedComponentPtr_t cc1, ConnectedComponentPtr_t cc2);
-
+      // Get distance function
+      virtual DistancePtr_t distance () const
+      {
+	return distance_;
+      }
     private:
       DevicePtr_t robot_;
       int dim_;
@@ -103,7 +108,8 @@ namespace hpp {
 		  NodePtr_t& nearest);
 
 
-    };
-  }
-}
-#endif // HPP_CORE_K_D_TREE_HH
+    }; // class KDTree
+    } // namespace nearestNeighbor
+  } // namespace core
+} // namespace hpp
+#endif // HPP_CORE_NEAREST_NEIGHBOR_K_D_TREE_HH

src/roadmap.cc

@@ -24,7 +24,7 @@
 #include <hpp/core/node.hh>
 #include <hpp/core/path.hh>
 #include <hpp/core/roadmap.hh>
-#include <hpp/core/k-d-tree.hh>
+#include <../src/nearest-neighbor/basic.hh>
 
 namespace hpp {
   namespace core {
@@ -37,10 +37,10 @@ namespace hpp {
       return RoadmapPtr_t (ptr);
     }
 
-    Roadmap::Roadmap (const DistancePtr_t& distance, const DevicePtr_t& robot) :
+    Roadmap::Roadmap (const DistancePtr_t& distance, const DevicePtr_t&) :
       distance_ (distance), connectedComponents_ (), nodes_ (), edges_ (),
       initNode_ (), goalNodes_ (),
-      kdTree_ (new KDTree (robot, distance, 30))
+      nearestNeighbor_ (new nearestNeighbor::Basic (distance))
     {
     }
 
@@ -70,7 +70,7 @@ namespace hpp {
 
       goalNodes_.clear ();
       initNode_ = 0x0;
-      kdTree_->clear();
+      nearestNeighbor_->clear();
     }
 
     NodePtr_t Roadmap::addNode (const ConfigurationPtr_t& configuration)
@@ -108,7 +108,7 @@ namespace hpp {
       // The new node needs to be registered in the connected
       // component.
       connectedComponent->addNode (node);
-      kdTree_->addNode(node);
+      nearestNeighbor_->addNode(node);
       return node;
     }
 
@@ -145,7 +145,7 @@ namespace hpp {
 	   itcc != connectedComponents_.end (); itcc++) {
 	value_type distance;
 	NodePtr_t node;
-	node = kdTree_->search(configuration, *itcc, distance);
+	node = nearestNeighbor_->search(configuration, *itcc, distance);
 	if (distance < minDistance) {
 	  minDistance = distance;
 	  closest = node;
@@ -162,7 +162,7 @@ namespace hpp {
       assert (connectedComponent);
       assert (connectedComponent->nodes ().size () != 0);
       NodePtr_t closest =
-	kdTree_->search(configuration, connectedComponent, minDistance);
+	nearestNeighbor_->search(configuration, connectedComponent, minDistance);
       return closest;
     }
     
@@ -196,7 +196,7 @@ namespace hpp {
     {
       connectedComponents_.insert (node->connectedComponent ());
       node->connectedComponent ()->addNode (node);
-      kdTree_->addNode(node);
+      nearestNeighbor_->addNode(node);
     }
 
     void Roadmap::connect (const ConnectedComponentPtr_t& cc1,

tests/roadmap-1.cc

@@ -23,7 +23,6 @@
 #include <hpp/model/joint.hh>
 #include <hpp/core/fwd.hh>
 #include <hpp/core/roadmap.hh>
-#include <hpp/core/k-d-tree.hh>
 #include <hpp/core/weighed-distance.hh>
 #include "hpp/core/basic-configuration-shooter.hh"
 #include <hpp/core/connected-component.hh>

tests/test-kdTree.cc

@@ -26,19 +26,17 @@
 #include <hpp/util/debug.hh>
 #include <hpp/model/device.hh>
 #include <hpp/model/joint.hh>
+#include <hpp/model/configuration.hh>
 #include <hpp/core/fwd.hh>
-//#include <hpp/model/fwd.hh>
 #include <hpp/core/roadmap.hh>
-#include "../src/nearest-neighbor.hh"
-#include <hpp/core/k-d-tree.hh>
 #include <hpp/core/weighed-distance.hh>
 #include "hpp/core/basic-configuration-shooter.hh"
 #include <hpp/core/connected-component.hh>
 #include <hpp/core/node.hh>
-#include "../src/node.cc"
-#include "../src/k-d-tree.cc"
-#include "../src/weighed-distance.cc"
+#include <hpp/core/steering-method-straight.hh>
 #include <hpp/model/joint-configuration.hh>
+#include "../src/nearest-neighbor/basic.hh"
+#include "../src/nearest-neighbor/k-d-tree.hh"
 
 
 #define BOOST_TEST_MODULE kdTree
@@ -72,28 +70,26 @@ BOOST_AUTO_TEST_CASE (kdTree) {
   so3Joint->addChildJoint (so2Joint);
 
   // Build Distance, nearestNeighbor, KDTree
-  WeighedDistancePtr_t weighedDistance = WeighedDistance::create(robot);
-  for ( int i =0 ; i<3 ; i++ ) weighedDistance->setWeight(i,1);
-  DistancePtr_t distance = weighedDistance;
+  WeighedDistancePtr_t distance = WeighedDistance::create(robot);
   BasicConfigurationShooter confShoot(robot);
-  KDTree kdTree(robot,distance,30);
-  typedef std::map <ConnectedComponentPtr_t, NearestNeighborPtr_t>
-    NearetNeighborMap_t;
-  NearetNeighborMap_t nearestNeighbor;
+  nearestNeighbor::KDTree kdTree(robot,distance,30);
+  nearestNeighbor::Basic basic (distance);
+  SteeringMethodPtr_t sm (new SteeringMethodStraight (robot));
 
   // Add 4 connectedComponents with 2000 nodes each
   ConfigurationPtr_t configuration;
   NodePtr_t node;
-  ConnectedComponentPtr_t connectedComponent[4];
+  NodePtr_t rootNode [4];
+  RoadmapPtr_t roadmap = Roadmap::create (distance, robot);
   for ( int i=0 ; i<4 ; i++ ) {
-    connectedComponent[i] = ConnectedComponent::create();
-    nearestNeighbor[connectedComponent[i]] =
-      NearestNeighborPtr_t (new NearestNeighbor (distance));
-    for ( int j=0 ; j<2000 ; j++ ) {
+    configuration = confShoot.shoot();
+    rootNode [i] = roadmap->addNode (configuration);
+    for (int j=1 ; j<200 ; j++) {
       configuration = confShoot.shoot();
-      node = NodePtr_t(new Node(configuration, connectedComponent[i]));
-      nearestNeighbor[ (connectedComponent[i]) ]->add(node);
-      kdTree.addNode(node);
+      PathPtr_t path = (*sm) (*(rootNode [i]->configuration ()),
+			      *configuration);
+      node = roadmap->addNodeAndEdges (rootNode [i], configuration, path);
+      basic.addNode (node);
     }
   }
 
@@ -107,11 +103,15 @@ BOOST_AUTO_TEST_CASE (kdTree) {
     for ( int i=0 ; i<4 ; i++ ) {
       minDistance1 = std::numeric_limits<value_type>::infinity ();
       minDistance2 = std::numeric_limits<value_type>::infinity ();
-      node1 = nearestNeighbor[connectedComponent[i]]
-	->nearest(configuration,minDistance1);
-      node2 = kdTree.search(configuration, connectedComponent[i], minDistance2);
+      node1 = basic.search (configuration, rootNode [i]->connectedComponent (),
+			    minDistance1);
+      node2 = roadmap->nearestNode (configuration,
+				    rootNode [i]->connectedComponent (),
+				    minDistance2);
       BOOST_CHECK( node1 == node2 );
       BOOST_CHECK( minDistance1 == minDistance2 );
+      std::cout << displayConfig (*(node1->configuration ())) << std::endl;
+      std::cout << minDistance1 << std::endl;
     }
   }
 }