Commit 9a213984 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

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.
parent 445a196e
......@@ -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
......
......@@ -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);
......
......@@ -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)
......
......@@ -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
......
//
// 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
......@@ -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
......@@ -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
......@@ -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,
......
......@@ -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>
......
......@@ -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;
}
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment