diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4cd31fc92fdaa4a08bbc0e10732d4859c7188c6e..cd9636ee450bb897df0eeef991e5a5ad3b33a86a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -115,7 +115,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/broadphase/broadphase_bruteforce.h
include/hpp/fcl/broadphase/morton.h
include/hpp/fcl/broadphase/hash.h
- include/hpp/fcl/learning/classifier.h
include/hpp/fcl/shape/geometric_shapes_utility.h
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
include/hpp/fcl/shape/geometric_shapes.h
@@ -126,12 +125,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/collision_node.h
include/hpp/fcl/collision_func_matrix.h
include/hpp/fcl/distance.h
- include/hpp/fcl/knn/greedy_kcenters.h
- include/hpp/fcl/knn/nearest_neighbors_GNAT.h
- include/hpp/fcl/knn/nearest_neighbors_flann.h
- include/hpp/fcl/knn/nearest_neighbors_sqrtapprox.h
- include/hpp/fcl/knn/nearest_neighbors_linear.h
- include/hpp/fcl/knn/nearest_neighbors.h
include/hpp/fcl/continuous_collision.h
include/hpp/fcl/math/vec_nf.h
include/hpp/fcl/math/matrix_3f.h
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index 5df9d173dc6707a9279d3983bf04ec0f41145005..3401843a4b5f854cb29695dd563f4834baa70f83 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -40,8 +40,6 @@
#define FCL_COLLISION_DATA_H
#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/knn/nearest_neighbors.h>
-
#include <hpp/fcl/math/vec_3f.h>
#include <vector>
@@ -520,54 +518,6 @@ struct ContinuousCollisionResult
}
};
-
-enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL};
-
-enum KNNSolverType {KNN_LINEAR, KNN_GNAT, KNN_SQRTAPPROX};
-
-
-struct PenetrationDepthRequest
-{
- void* classifier;
-
- NearestNeighbors<Transform3f>::DistanceFunction distance_func;
-
- /// @brief KNN solver type
- KNNSolverType knn_solver_type;
-
- /// @brief PD algorithm type
- PenetrationDepthType pd_type;
-
- /// @brief gjk solver type
- GJKSolverType gjk_solver_type;
-
- std::vector<Transform3f> contact_vectors;
-
- PenetrationDepthRequest(void* classifier_,
- NearestNeighbors<Transform3f>::DistanceFunction distance_func_,
- KNNSolverType knn_solver_type_ = KNN_LINEAR,
- PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL,
- GJKSolverType gjk_solver_type_ = GST_LIBCCD) : classifier(classifier_),
- distance_func(distance_func_),
- knn_solver_type(knn_solver_type_),
- pd_type(pd_type_),
- gjk_solver_type(gjk_solver_type_)
- {
- }
-};
-
-struct PenetrationDepthResult
-{
- /// @brief penetration depth value
- FCL_REAL pd_value;
-
- /// @brief the transform where the collision is resolved
- Transform3f resolved_tf;
-};
-
-
-
-
}
#endif
diff --git a/include/hpp/fcl/knn/greedy_kcenters.h b/include/hpp/fcl/knn/greedy_kcenters.h
deleted file mode 100644
index 38ea95495c35f461d4ca34dc136fd5744fe23da3..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/greedy_kcenters.h
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2015, Rice University.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Rice University nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Mark Moll */
-
-#ifndef FCL_KNN_GREEDY_KCENTERS_H
-#define FCL_KNN_GREEDY_KCENTERS_H
-
-
-#include <hpp/fcl/math/sampling.h>
-
-namespace fcl
-{
-/// @brief An instance of this class can be used to greedily select a given
-/// number of representatives from a set of data points that are all far
-/// apart from each other.
-template<typename _T>
-class GreedyKCenters
-{
-public:
- /// @brief The definition of a distance function
- typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
-
- GreedyKCenters(void)
- {
- }
-
- virtual ~GreedyKCenters(void)
- {
- }
-
- /// @brief Set the distance function to use
- void setDistanceFunction(const DistanceFunction &distFun)
- {
- distFun_ = distFun;
- }
-
- /// @brief Get the distance function used
- const DistanceFunction& getDistanceFunction(void) const
- {
- return distFun_;
- }
-
- /// @brief Greedy algorithm for selecting k centers
- /// @param data a vector of data points
- /// @param k the desired number of centers
- /// @param centers a vector of length k containing the indices into data of the k centers
- /// @param dists a 2-dimensional array such that dists[i][j] is the distance between data[i] and data[center[j]]
- void kcenters(const std::vector<_T>& data, unsigned int k,
- std::vector<unsigned int>& centers, std::vector<std::vector<double> >& dists)
- {
- // array containing the minimum distance between each data point
- // and the centers computed so far
- std::vector<double> minDist(data.size(), std::numeric_limits<double>::infinity());
-
- centers.clear();
- centers.reserve(k);
- dists.resize(data.size(), std::vector<double>(k));
- // first center is picked randomly
- centers.push_back(rng_.uniformInt(0, data.size() - 1));
- for (unsigned i=1; i<k; ++i)
- {
- unsigned ind;
- const _T& center = data[centers[i - 1]];
- double maxDist = -std::numeric_limits<double>::infinity();
- for (unsigned j=0; j<data.size(); ++j)
- {
- if ((dists[j][i-1] = distFun_(data[j], center)) < minDist[j])
- minDist[j] = dists[j][i - 1];
- // the j-th center is the one furthest away from center 0,..,j-1
- if (minDist[j] > maxDist)
- {
- ind = j;
- maxDist = minDist[j];
- }
- }
- // no more centers available
- if (maxDist < std::numeric_limits<double>::epsilon()) break;
- centers.push_back(ind);
- }
-
- const _T& center = data[centers.back()];
- unsigned i = centers.size() - 1;
- for (unsigned j = 0; j < data.size(); ++j)
- dists[j][i] = distFun_(data[j], center);
- }
-
-protected:
- /// @brief The used distance function
- DistanceFunction distFun_;
-
- /// Random number generator used to select first center
- RNG rng_;
-};
-}
-
-#endif
diff --git a/include/hpp/fcl/knn/nearest_neighbors.h b/include/hpp/fcl/knn/nearest_neighbors.h
deleted file mode 100644
index fccd9daefb6b0e4fe8d72b6b51df9e70b7695395..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/nearest_neighbors.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Ioan Sucan */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_H
-#define FCL_KNN_NEAREST_NEIGHBORS_H
-
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
-
-namespace fcl
-{
-/// @brief Abstract representation of a container that can perform nearest neighbors queries
-template<typename _T>
-class NearestNeighbors
-{
-public:
-
- /// @brief The definition of a distance function
- typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
-
- NearestNeighbors(void)
- {
- }
-
- virtual ~NearestNeighbors(void)
- {
- }
-
- /// @brief Set the distance function to use
- virtual void setDistanceFunction(const DistanceFunction &distFun)
- {
- distFun_ = distFun;
- }
-
- /// @brief Get the distance function used
- const DistanceFunction& getDistanceFunction(void) const
- {
- return distFun_;
- }
-
- /// @brief Clear the datastructure
- virtual void clear(void) = 0;
-
- /// @brief Add an element to the datastructure
- virtual void add(const _T &data) = 0;
-
- /// @brief Add a vector of points
- virtual void add(const std::vector<_T> &data)
- {
- for (typename std::vector<_T>::const_iterator elt = data.begin() ; elt != data.end() ; ++elt)
- add(*elt);
- }
-
- /// @brief Remove an element from the datastructure
- virtual bool remove(const _T &data) = 0;
-
- /// @brief Get the nearest neighbor of a point
- virtual _T nearest(const _T &data) const = 0;
-
- /// @brief Get the k-nearest neighbors of a point
- virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const = 0;
-
- /// @brief Get the nearest neighbors of a point, within a specified radius
- virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const = 0;
-
- /// @brief Get the number of elements in the datastructure
- virtual std::size_t size(void) const = 0;
-
- /// @brief Get all the elements in the datastructure
- virtual void list(std::vector<_T> &data) const = 0;
-
-protected:
-
- /// @brief The used distance function
- DistanceFunction distFun_;
-
-};
-}
-
-
-#endif
diff --git a/include/hpp/fcl/knn/nearest_neighbors_GNAT.h b/include/hpp/fcl/knn/nearest_neighbors_GNAT.h
deleted file mode 100644
index 4d3c6ac1aecd978c8c82f641e0b772ea27bfe7e3..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/nearest_neighbors_GNAT.h
+++ /dev/null
@@ -1,696 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2015, Rice University.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Rice University nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Mark Moll */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_GNAT_H
-#define FCL_KNN_NEAREST_NEIGHBORS_GNAT_H
-
-#include <hpp/fcl/knn/nearest_neighbors.h>
-#include <hpp/fcl/knn/greedy_kcenters.h>
-#include <hpp/fcl/exception.h>
-#include <boost/unordered_set.hpp>
-#include <queue>
-#include <algorithm>
-
-
-namespace fcl
-{
-
-/** \brief Geometric Near-neighbor Access Tree (GNAT), a data
- structure for nearest neighbor search.
-
- See:
- S. Brin, "Near neighbor search in large metric spaces," in Proc. 21st
- Conf. on Very Large Databases (VLDB), pp. 574�V584, 1995.
-
-*/
-template<typename _T>
-class NearestNeighborsGNAT : public NearestNeighbors<_T>
-{
-protected:
- /// \cond IGNORE
- // internally, we use a priority queue for nearest neighbors, paired
- // with their distance to the query point
- typedef std::pair<const _T*,double> DataDist;
- struct DataDistCompare
- {
- bool operator()(const DataDist& d0, const DataDist& d1)
- {
- return d0.second < d1.second;
- }
- };
- typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
-
- // another internal data structure is a priority queue of nodes to
- // check next for possible nearest neighbors
- class Node;
- typedef std::pair<Node*,double> NodeDist;
- struct NodeDistCompare
- {
- bool operator()(const NodeDist& n0, const NodeDist& n1) const
- {
- return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
- }
- };
- typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
- /// \endcond
-
-public:
- NearestNeighborsGNAT(unsigned int degree = 4, unsigned int minDegree = 2,
- unsigned int maxDegree = 6, unsigned int maxNumPtsPerLeaf = 50,
- unsigned int removedCacheSize = 50, bool rebalancing = false)
- : NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
- minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
- maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
- rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
- removedCacheSize_(removedCacheSize)
- {
- }
-
- virtual ~NearestNeighborsGNAT(void)
- {
- if (tree_)
- delete tree_;
- }
- /// \brief Set the distance function to use
- virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
- {
- NearestNeighbors<_T>::setDistanceFunction(distFun);
- pivotSelector_.setDistanceFunction(distFun);
- if (tree_)
- rebuildDataStructure();
- }
- virtual void clear(void)
- {
- if (tree_)
- {
- delete tree_;
- tree_ = NULL;
- }
- size_ = 0;
- removed_.clear();
- if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
- rebuildSize_ = maxNumPtsPerLeaf_ * degree_;
- }
-
- virtual void add(const _T &data)
- {
- if (tree_)
- {
- if (isRemoved(data))
- rebuildDataStructure();
- tree_->add(*this, data);
- }
- else
- {
- tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
- size_ = 1;
- }
- }
- virtual void add(const std::vector<_T> &data)
- {
- if (tree_)
- NearestNeighbors<_T>::add(data);
- else if (data.size()>0)
- {
- tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
- for (unsigned int i=1; i<data.size(); ++i)
- tree_->data_.push_back(data[i]);
- if (tree_->needToSplit(*this))
- tree_->split(*this);
- }
- size_ += data.size();
- }
- /// \brief Rebuild the internal data structure.
- void rebuildDataStructure()
- {
- std::vector<_T> lst;
- list(lst);
- clear();
- add(lst);
- }
- /// \brief Remove data from the tree.
- /// The element won't actually be removed immediately, but just marked
- /// for removal in the removed_ cache. When the cache is full, the tree
- /// will be rebuilt and the elements marked for removal will actually
- /// be removed.
- virtual bool remove(const _T &data)
- {
- if (!size_) return false;
- NearQueue nbhQueue;
- // find data in tree
- bool isPivot = nearestKInternal(data, 1, nbhQueue);
- if (*nbhQueue.top().first != data)
- return false;
- removed_.insert(nbhQueue.top().first);
- size_--;
- // if we removed a pivot or if the capacity of removed elements
- // has been reached, we rebuild the entire GNAT
- if (isPivot || removed_.size()>=removedCacheSize_)
- rebuildDataStructure();
- return true;
- }
-
- virtual _T nearest(const _T &data) const
- {
- if (size_)
- {
- std::vector<_T> nbh;
- nearestK(data, 1, nbh);
- if (!nbh.empty()) return nbh[0];
- }
- throw Exception("No elements found in nearest neighbors data structure");
- }
-
- virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
- {
- nbh.clear();
- if (k == 0) return;
- if (size_)
- {
- NearQueue nbhQueue;
- nearestKInternal(data, k, nbhQueue);
- postprocessNearest(nbhQueue, nbh);
- }
- }
-
- virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
- {
- nbh.clear();
- if (size_)
- {
- NearQueue nbhQueue;
- nearestRInternal(data, radius, nbhQueue);
- postprocessNearest(nbhQueue, nbh);
- }
- }
-
- virtual std::size_t size(void) const
- {
- return size_;
- }
-
- virtual void list(std::vector<_T> &data) const
- {
- data.clear();
- data.reserve(size());
- if (tree_)
- tree_->list(*this, data);
- }
-
- /// \brief Print a GNAT structure (mostly useful for debugging purposes).
- friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
- {
- if (gnat.tree_)
- {
- out << *gnat.tree_;
- if (!gnat.removed_.empty())
- {
- out << "Elements marked for removal:\n";
- for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
- it != gnat.removed_.end(); it++)
- out << **it << '\t';
- out << std::endl;
- }
- }
- return out;
- }
-
- // for debugging purposes
- void integrityCheck()
- {
- std::vector<_T> lst;
- boost::unordered_set<const _T*> tmp;
- // get all elements, including those marked for removal
- removed_.swap(tmp);
- list(lst);
- // check if every element marked for removal is also in the tree
- for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
- {
- unsigned int i;
- for (i=0; i<lst.size(); ++i)
- if (lst[i]==**it)
- break;
- if (i == lst.size())
- {
- // an element marked for removal is not actually in the tree
- std::cout << "***** FAIL!! ******\n" << *this << '\n';
- for (unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<'\t';
- std::cout<<std::endl;
- }
- assert(i != lst.size());
- }
- // restore
- removed_.swap(tmp);
- // get elements in the tree with elements marked for removal purged from the list
- list(lst);
- if (lst.size() != size_)
- std::cout << "#########################################\n" << *this << std::endl;
- assert(lst.size() == size_);
- }
-protected:
- typedef NearestNeighborsGNAT<_T> GNAT;
-
- /// Return true iff data has been marked for removal.
- bool isRemoved(const _T& data) const
- {
- return !removed_.empty() && removed_.find(&data) != removed_.end();
- }
-
- /// \brief Return in nbhQueue the k nearest neighbors of data.
- /// For k=1, return true if the nearest neighbor is a pivot.
- /// (which is important during removal; removing pivots is a
- /// special case).
- bool nearestKInternal(const _T &data, std::size_t k, NearQueue& nbhQueue) const
- {
- bool isPivot;
- double dist;
- NodeDist nodeDist;
- NodeQueue nodeQueue;
-
- isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data,
- NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
- tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
- while (nodeQueue.size() > 0)
- {
- dist = nbhQueue.top().second; // note the difference with nearestRInternal
- nodeDist = nodeQueue.top();
- nodeQueue.pop();
- if (nbhQueue.size() == k &&
- (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
- nodeDist.second < nodeDist.first->minRadius_ - dist))
- break;
- nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
- }
- return isPivot;
- }
- /// \brief Return in nbhQueue the elements that are within distance radius of data.
- void nearestRInternal(const _T &data, double radius, NearQueue& nbhQueue) const
- {
- double dist = radius; // note the difference with nearestKInternal
- NodeQueue nodeQueue;
- NodeDist nodeDist;
-
- tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
- NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
- tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
- while (nodeQueue.size() > 0)
- {
- nodeDist = nodeQueue.top();
- nodeQueue.pop();
- if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
- nodeDist.second < nodeDist.first->minRadius_ - dist)
- break;
- nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
- }
- }
- /// \brief Convert the internal data structure used for storing neighbors
- /// to the vector that NearestNeighbor API requires.
- void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
- {
- typename std::vector<_T>::reverse_iterator it;
- nbh.resize(nbhQueue.size());
- for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
- *it = *nbhQueue.top().first;
- }
-
- /// The class used internally to define the GNAT.
- class Node
- {
- public:
- /// \brief Construct a node of given degree with at most
- /// \e capacity data elements and wit given pivot.
- Node(int degree, int capacity, const _T& pivot)
- : degree_(degree), pivot_(pivot),
- minRadius_(std::numeric_limits<double>::infinity()),
- maxRadius_(-minRadius_), minRange_(degree, minRadius_),
- maxRange_(degree, maxRadius_)
- {
- // The "+1" is needed because we add an element before we check whether to split
- data_.reserve(capacity+1);
- }
-
- ~Node()
- {
- for (unsigned int i=0; i<children_.size(); ++i)
- delete children_[i];
- }
-
- /// \brief Update minRadius_ and maxRadius_, given that an element
- /// was added with distance dist to the pivot.
- void updateRadius(double dist)
- {
- if (minRadius_ > dist)
- minRadius_ = dist;
- if (maxRadius_ < dist)
- maxRadius_ = dist;
- }
- /// \brief Update minRange_[i] and maxRange_[i], given that an
- /// element was added to the i-th child of the parent that has
- /// distance dist to this Node's pivot.
- void updateRange(unsigned int i, double dist)
- {
- if (minRange_[i] > dist)
- minRange_[i] = dist;
- if (maxRange_[i] < dist)
- maxRange_[i] = dist;
- }
- /// Add an element to the tree rooted at this node.
- void add(GNAT& gnat, const _T& data)
- {
- if (children_.size()==0)
- {
- data_.push_back(data);
- gnat.size_++;
- if (needToSplit(gnat))
- {
- if (gnat.removed_.size() > 0)
- gnat.rebuildDataStructure();
- else if (gnat.size_ >= gnat.rebuildSize_)
- {
- gnat.rebuildSize_ <<= 1;
- gnat.rebuildDataStructure();
- }
- else
- split(gnat);
- }
- }
- else
- {
- std::vector<double> dist(children_.size());
- double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
- int minInd = 0;
-
- for (unsigned int i=1; i<children_.size(); ++i)
- if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
- {
- minDist = dist[i];
- minInd = i;
- }
- for (unsigned int i=0; i<children_.size(); ++i)
- children_[i]->updateRange(minInd, dist[i]);
- children_[minInd]->updateRadius(minDist);
- children_[minInd]->add(gnat, data);
- }
- }
- /// Return true iff the node needs to be split into child nodes.
- bool needToSplit(const GNAT& gnat) const
- {
- unsigned int sz = data_.size();
- return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
- }
- /// \brief The split operation finds pivot elements for the child
- /// nodes and moves each data element of this node to the appropriate
- /// child node.
- void split(GNAT& gnat)
- {
- std::vector<std::vector<double> > dists;
- std::vector<unsigned int> pivots;
-
- children_.reserve(degree_);
- gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
- for(unsigned int i=0; i<pivots.size(); i++)
- children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
- degree_ = pivots.size(); // in case fewer than degree_ pivots were found
- for (unsigned int j=0; j<data_.size(); ++j)
- {
- unsigned int k = 0;
- for (unsigned int i=1; i<degree_; ++i)
- if (dists[j][i] < dists[j][k])
- k = i;
- Node* child = children_[k];
- if (j != pivots[k])
- {
- child->data_.push_back(data_[j]);
- child->updateRadius(dists[j][k]);
- }
- for (unsigned int i=0; i<degree_; ++i)
- children_[i]->updateRange(k, dists[j][i]);
- }
-
- for (unsigned int i=0; i<degree_; ++i)
- {
- // make sure degree lies between minDegree_ and maxDegree_
- children_[i]->degree_ = std::min(std::max(
- degree_ * (unsigned int)(children_[i]->data_.size() / data_.size()),
- gnat.minDegree_), gnat.maxDegree_);
- // singleton
- if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
- children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
- }
- // this does more than clear(); it also sets capacity to 0 and frees the memory
- std::vector<_T> tmp;
- data_.swap(tmp);
- // check if new leaves need to be split
- for (unsigned int i=0; i<degree_; ++i)
- if (children_[i]->needToSplit(gnat))
- children_[i]->split(gnat);
- }
-
- /// Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh.
- bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
- {
- if (nbh.size() < k)
- {
- nbh.push(std::make_pair(&data, dist));
- return true;
- }
- else if (dist < nbh.top().second ||
- (dist < std::numeric_limits<double>::epsilon() && data==key))
- {
- nbh.pop();
- nbh.push(std::make_pair(&data, dist));
- return true;
- }
- return false;
- }
-
- /// \brief Compute the k nearest neighbors of data in the tree.
- /// For k=1, isPivot is true if the nearest neighbor is a pivot
- /// (which is important during removal; removing pivots is a
- /// special case). The nodeQueue, which contains other Nodes
- /// that need to be checked for nearest neighbors, is updated.
- void nearestK(const GNAT& gnat, const _T &data, std::size_t k,
- NearQueue& nbh, NodeQueue& nodeQueue, bool& isPivot) const
- {
- for (unsigned int i=0; i<data_.size(); ++i)
- if (!gnat.isRemoved(data_[i]))
- {
- if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
- isPivot = false;
- }
- if (children_.size() > 0)
- {
- double dist;
- Node* child;
- std::vector<double> distToPivot(children_.size());
- std::vector<int> permutation(children_.size());
-
- for (unsigned int i=0; i<permutation.size(); ++i)
- permutation[i] = i;
- std::random_shuffle(permutation.begin(), permutation.end());
-
- for (unsigned int i=0; i<children_.size(); ++i)
- if (permutation[i] >= 0)
- {
- child = children_[permutation[i]];
- distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
- if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
- isPivot = true;
- if (nbh.size()==k)
- {
- dist = nbh.top().second; // note difference with nearestR
- for (unsigned int j=0; j<children_.size(); ++j)
- if (permutation[j] >=0 && i != j &&
- (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
- distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
- permutation[j] = -1;
- }
- }
-
- dist = nbh.top().second;
- for (unsigned int i=0; i<children_.size(); ++i)
- if (permutation[i] >= 0)
- {
- child = children_[permutation[i]];
- if (nbh.size()<k ||
- (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
- distToPivot[permutation[i]] + dist >= child->minRadius_))
- nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
- }
- }
- }
- /// Insert data in nbh if it is a near neighbor.
- void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
- {
- if (dist <= r)
- nbh.push(std::make_pair(&data, dist));
- }
- /// \brief Return all elements that are within distance r in nbh.
- /// The nodeQueue, which contains other Nodes that need to
- /// be checked for nearest neighbors, is updated.
- void nearestR(const GNAT& gnat, const _T &data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
- {
- double dist = r; //note difference with nearestK
-
- for (unsigned int i=0; i<data_.size(); ++i)
- if (!gnat.isRemoved(data_[i]))
- insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
- if (children_.size() > 0)
- {
- Node* child;
- std::vector<double> distToPivot(children_.size());
- std::vector<int> permutation(children_.size());
-
- for (unsigned int i=0; i<permutation.size(); ++i)
- permutation[i] = i;
- std::random_shuffle(permutation.begin(), permutation.end());
-
- for (unsigned int i=0; i<children_.size(); ++i)
- if (permutation[i] >= 0)
- {
- child = children_[permutation[i]];
- distToPivot[i] = gnat.distFun_(data, child->pivot_);
- insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
- for (unsigned int j=0; j<children_.size(); ++j)
- if (permutation[j] >=0 && i != j &&
- (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
- distToPivot[i] + dist < child->minRange_[permutation[j]]))
- permutation[j] = -1;
- }
-
- for (unsigned int i=0; i<children_.size(); ++i)
- if (permutation[i] >= 0)
- {
- child = children_[permutation[i]];
- if (distToPivot[i] - dist <= child->maxRadius_ &&
- distToPivot[i] + dist >= child->minRadius_)
- nodeQueue.push(std::make_pair(child, distToPivot[i]));
- }
- }
- }
-
- void list(const GNAT& gnat, std::vector<_T> &data) const
- {
- if (!gnat.isRemoved(pivot_))
- data.push_back(pivot_);
- for (unsigned int i=0; i<data_.size(); ++i)
- if(!gnat.isRemoved(data_[i]))
- data.push_back(data_[i]);
- for (unsigned int i=0; i<children_.size(); ++i)
- children_[i]->list(gnat, data);
- }
-
- friend std::ostream& operator<<(std::ostream& out, const Node& node)
- {
- out << "\ndegree:\t" << node.degree_;
- out << "\nminRadius:\t" << node.minRadius_;
- out << "\nmaxRadius:\t" << node.maxRadius_;
- out << "\nminRange:\t";
- for (unsigned int i=0; i<node.minRange_.size(); ++i)
- out << node.minRange_[i] << '\t';
- out << "\nmaxRange: ";
- for (unsigned int i=0; i<node.maxRange_.size(); ++i)
- out << node.maxRange_[i] << '\t';
- out << "\npivot:\t" << node.pivot_;
- out << "\ndata: ";
- for (unsigned int i=0; i<node.data_.size(); ++i)
- out << node.data_[i] << '\t';
- out << "\nthis:\t" << &node;
- out << "\nchildren:\n";
- for (unsigned int i=0; i<node.children_.size(); ++i)
- out << node.children_[i] << '\t';
- out << '\n';
- for (unsigned int i=0; i<node.children_.size(); ++i)
- out << *node.children_[i] << '\n';
- return out;
- }
-
- /// Number of child nodes
- unsigned int degree_;
- /// Data element stored in this Node
- const _T pivot_;
- /// Minimum distance between the pivot element and the elements stored in data_
- double minRadius_;
- /// Maximum distance between the pivot element and the elements stored in data_
- double maxRadius_;
- /// \brief The i-th element in minRange_ is the minimum distance between the
- /// pivot and any data_ element in the i-th child node of this node's parent.
- std::vector<double> minRange_;
- /// \brief The i-th element in maxRange_ is the maximum distance between the
- /// pivot and any data_ element in the i-th child node of this node's parent.
- std::vector<double> maxRange_;
- /// \brief The data elements stored in this node (in addition to the pivot
- /// element). An internal node has no elements stored in data_.
- std::vector<_T> data_;
- /// \brief The child nodes of this node. By definition, only internal nodes
- /// have child nodes.
- std::vector<Node*> children_;
- };
-
- /// \brief The data structure containing the elements stored in this structure.
- Node* tree_;
- /// The desired degree of each node.
- unsigned int degree_;
- /// \brief After splitting a Node, each child Node has degree equal to
- /// the default degree times the fraction of data elements from the
- /// original node that got assigned to that child Node. However, its
- /// degree can be no less than minDegree_.
- unsigned int minDegree_;
- /// \brief After splitting a Node, each child Node has degree equal to
- /// the default degree times the fraction of data elements from the
- /// original node that got assigned to that child Node. However, its
- /// degree can be no larger than maxDegree_.
- unsigned int maxDegree_;
- /// \brief Maximum number of elements allowed to be stored in a Node before
- /// it needs to be split into several nodes.
- unsigned int maxNumPtsPerLeaf_;
- /// \brief Number of elements stored in the tree.
- std::size_t size_;
- /// \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and
- /// automatically rebalanced), and rebuildSize_ will be doubled.
- std::size_t rebuildSize_;
- /// \brief Maximum number of removed elements that can be stored in the
- /// removed_ cache. If the cache is full, the tree will be rebuilt with
- /// the elements in removed_ actually removed from the tree.
- std::size_t removedCacheSize_;
- /// \brief The data structure used to split data into subtrees.
- GreedyKCenters<_T> pivotSelector_;
- /// \brief Cache of removed elements.
- boost::unordered_set<const _T*> removed_;
-};
-}
-
-#endif
diff --git a/include/hpp/fcl/knn/nearest_neighbors_flann.h b/include/hpp/fcl/knn/nearest_neighbors_flann.h
deleted file mode 100644
index c222c7e86689dbe763cccde923ca10e9eaabb30d..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/nearest_neighbors_flann.h
+++ /dev/null
@@ -1,350 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011-2015, Rice University.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Rice University nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Mark Moll */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_FLANN_H
-#define FCL_KNN_NEAREST_NEIGHBORS_FLANN_H
-
-#include <hpp/fcl/config-fcl.hh>
-#if FCL_HAVE_FLANN == 0
-# error FLANN is not available. Please use a different NearestNeighbors data structure
-#else
-
-#include <hpp/fcl/knn/nearest_neighbors.h>
-#include <flann/flann.hpp>
-#include <hpp/fcl/exception.h>
-
-namespace fcl
-{
-/** \brief Wrapper class to allow FLANN access to the
- NearestNeighbors::distFun_ callback function
-*/
-template<typename _T>
-class FLANNDistance
-{
-public:
- typedef _T ElementType;
- typedef double ResultType;
-
- FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction& distFun)
- : distFun_(distFun)
- {
- }
-
- template <typename Iterator1, typename Iterator2>
- ResultType operator()(Iterator1 a, Iterator2 b,
- size_t /*size*/, ResultType /*worst_dist*/ = -1) const
- {
- return distFun_(*a, *b);
- }
-protected:
- const typename NearestNeighbors<_T>::DistanceFunction& distFun_;
-};
-
-/** \brief Wrapper class for nearest neighbor data structures in the
- FLANN library.
-
- See:
- M. Muja and D.G. Lowe, "Fast Approximate Nearest Neighbors with
- Automatic Algorithm Configuration", in International Conference
- on Computer Vision Theory and Applications (VISAPP'09), 2009.
- http://people.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN
-*/
-template<typename _T, typename _Dist = FLANNDistance<_T> >
-class NearestNeighborsFLANN : public NearestNeighbors<_T>
-{
-public:
-
- NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> ¶ms)
- : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
- {
- }
-
- virtual ~NearestNeighborsFLANN(void)
- {
- if (index_)
- delete index_;
- }
-
- virtual void clear(void)
- {
- if (index_)
- {
- delete index_;
- index_ = NULL;
- }
- data_.clear();
- }
-
- virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
- {
- NearestNeighbors<_T>::setDistanceFunction(distFun);
- rebuildIndex();
- }
-
- virtual void add(const _T &data)
- {
- bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
-
- if (rebuild)
- rebuildIndex(2 * data_.capacity());
-
- data_.push_back(data);
- const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
-
- if (index_)
- index_->addPoints(mat, std::numeric_limits<float>::max()/size());
- else
- createIndex(mat);
- }
- virtual void add(const std::vector<_T> &data)
- {
- unsigned int oldSize = data_.size();
- unsigned int newSize = oldSize + data.size();
- bool rebuild = index_ && (newSize > data_.capacity());
-
- if (rebuild)
- rebuildIndex(std::max(2 * oldSize, newSize));
-
- if (index_)
- {
- std::copy(data.begin(), data.end(), data_.begin() + oldSize);
- const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
- index_->addPoints(mat, std::numeric_limits<float>::max()/size());
- }
- else
- {
- data_ = data;
- const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
- createIndex(mat);
- }
- }
- virtual bool remove(const _T& data)
- {
- if (!index_) return false;
- _T& elt = const_cast<_T&>(data);
- const flann::Matrix<_T> query(&elt, 1, dimension_);
- std::vector<std::vector<size_t> > indices(1);
- std::vector<std::vector<double> > dists(1);
- index_->knnSearch(query, indices, dists, 1, searchParams_);
- if (*index_->getPoint(indices[0][0]) == data)
- {
- index_->removePoint(indices[0][0]);
- rebuildIndex();
- return true;
- }
- return false;
- }
- virtual _T nearest(const _T &data) const
- {
- if (size())
- {
- _T& elt = const_cast<_T&>(data);
- const flann::Matrix<_T> query(&elt, 1, dimension_);
- std::vector<std::vector<size_t> > indices(1);
- std::vector<std::vector<double> > dists(1);
- index_->knnSearch(query, indices, dists, 1, searchParams_);
- return *index_->getPoint(indices[0][0]);
- }
- throw Exception("No elements found in nearest neighbors data structure");
- }
- virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
- {
- _T& elt = const_cast<_T&>(data);
- const flann::Matrix<_T> query(&elt, 1, dimension_);
- std::vector<std::vector<size_t> > indices;
- std::vector<std::vector<double> > dists;
- k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
- nbh.resize(k);
- for (std::size_t i = 0 ; i < k ; ++i)
- nbh[i] = *index_->getPoint(indices[0][i]);
- }
-
- virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
- {
- _T& elt = const_cast<_T&>(data);
- flann::Matrix<_T> query(&elt, 1, dimension_);
- std::vector<std::vector<size_t> > indices;
- std::vector<std::vector<double> > dists;
- int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
- nbh.resize(k);
- for (int i = 0 ; i < k ; ++i)
- nbh[i] = *index_->getPoint(indices[0][i]);
- }
-
- virtual std::size_t size(void) const
- {
- return index_ ? index_->size() : 0;
- }
-
- virtual void list(std::vector<_T> &data) const
- {
- std::size_t sz = size();
- if (sz == 0)
- {
- data.resize(0);
- return;
- }
- const _T& dummy = *index_->getPoint(0);
- int checks = searchParams_.checks;
- searchParams_.checks = size();
- nearestK(dummy, sz, data);
- searchParams_.checks = checks;
- }
-
- /// \brief Set the FLANN index parameters.
- ///
- /// The parameters determine the type of nearest neighbor
- /// data structure to be constructed.
- virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> ¶ms)
- {
- params_ = params;
- rebuildIndex();
- }
-
- /// \brief Get the FLANN parameters used to build the current index.
- virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams(void) const
- {
- return params_;
- }
-
- /// \brief Set the FLANN parameters to be used during nearest neighbor
- /// searches
- virtual void setSearchParams(const flann::SearchParams& searchParams)
- {
- searchParams_ = searchParams;
- }
-
- /// \brief Get the FLANN parameters used during nearest neighbor
- /// searches
- flann::SearchParams& getSearchParams(void)
- {
- return searchParams_;
- }
-
- /// \brief Get the FLANN parameters used during nearest neighbor
- /// searches
- const flann::SearchParams& getSearchParams(void) const
- {
- return searchParams_;
- }
-
- unsigned int getContainerSize(void) const
- {
- return dimension_;
- }
-
-protected:
-
- /// \brief Internal function to construct nearest neighbor
- /// data structure with initial elements stored in mat.
- void createIndex(const flann::Matrix<_T>& mat)
- {
- index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
- index_->buildIndex();
- }
-
- /// \brief Rebuild the nearest neighbor data structure (necessary when
- /// changing the distance function or index parameters).
- void rebuildIndex(unsigned int capacity = 0)
- {
- if (index_)
- {
- std::vector<_T> data;
- list(data);
- clear();
- if (capacity)
- data_.reserve(capacity);
- add(data);
- }
- }
-
- /// \brief vector of data stored in FLANN's index. FLANN only indexes
- /// references, so we need store the original data.
- std::vector<_T> data_;
-
- /// \brief The FLANN index (the actual index type depends on params_).
- flann::Index<_Dist>* index_;
-
- /// \brief The FLANN index parameters. This contains both the type of
- /// index and the parameters for that type.
- boost::shared_ptr<flann::IndexParams> params_;
-
- /// \brief The parameters used to seach for nearest neighbors.
- mutable flann::SearchParams searchParams_;
-
- /// \brief If each element has an array-like structure that is exposed
- /// to FLANN, then the dimension_ needs to be set to the length of
- /// this array.
- unsigned int dimension_;
-};
-
-template<>
-void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex(
- const flann::Matrix<double>& mat)
-{
- index_ = new flann::Index<flann::L2<double> >(mat, *params_);
- index_->buildIndex();
-}
-
-template<typename _T, typename _Dist = FLANNDistance<_T> >
-class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist>
-{
-public:
- NearestNeighborsFLANNLinear()
- : NearestNeighborsFLANN<_T, _Dist>(
- boost::shared_ptr<flann::LinearIndexParams>(
- new flann::LinearIndexParams()))
- {
- }
-};
-
-template<typename _T, typename _Dist = FLANNDistance<_T> >
-class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN<_T, _Dist>
-{
-public:
- NearestNeighborsFLANNHierarchicalClustering()
- : NearestNeighborsFLANN<_T, _Dist>(
- boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
- new flann::HierarchicalClusteringIndexParams()))
- {
- }
-};
-
-}
-
-#endif
-
-#endif
diff --git a/include/hpp/fcl/knn/nearest_neighbors_linear.h b/include/hpp/fcl/knn/nearest_neighbors_linear.h
deleted file mode 100644
index bc387b09d39a220fc63e975be1de3fb41e79f795..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/nearest_neighbors_linear.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Ioan Sucan */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_LINEAR_H
-#define FCL_KNN_NEAREST_NEIGHBORS_LINEAR_H
-
-#include <hpp/fcl/knn/nearest_neighbors.h>
-#include <hpp/fcl/exception.h>
-#include <algorithm>
-
-namespace fcl
-{
-/** \brief A nearest neighbors datastructure that uses linear
- search.
-
- \li Search for nearest neighbor is O(n).
- \li Search for k-nearest neighbors is O(n log(k)).
- \li Search for neighbors within a range is O(n log(n)).
- \li Adding an element to the datastructure is O(1).
- \li Removing an element from the datastructure O(n).
-*/
-template<typename _T>
-class NearestNeighborsLinear : public NearestNeighbors<_T>
-{
-public:
- NearestNeighborsLinear(void) : NearestNeighbors<_T>()
- {
- }
-
- virtual ~NearestNeighborsLinear(void)
- {
- }
-
- virtual void clear(void)
- {
- data_.clear();
- }
-
- virtual void add(const _T &data)
- {
- data_.push_back(data);
- }
-
- virtual void add(const std::vector<_T> &data)
- {
- data_.reserve(data_.size() + data.size());
- data_.insert(data_.end(), data.begin(), data.end());
- }
-
- virtual bool remove(const _T &data)
- {
- if (!data_.empty())
- for (int i = data_.size() - 1 ; i >= 0 ; --i)
- if (data_[i] == data)
- {
- data_.erase(data_.begin() + i);
- return true;
- }
- return false;
- }
-
- virtual _T nearest(const _T &data) const
- {
- const std::size_t sz = data_.size();
- std::size_t pos = sz;
- double dmin = 0.0;
- for (std::size_t i = 0 ; i < sz ; ++i)
- {
- double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
- if (pos == sz || dmin > distance)
- {
- pos = i;
- dmin = distance;
- }
- }
- if (pos != sz)
- return data_[pos];
-
- throw Exception("No elements found in nearest neighbors data structure");
- }
-
- virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
- {
- nbh = data_;
- if (nbh.size() > k)
- {
- std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
- ElemSort(data, NearestNeighbors<_T>::distFun_));
- nbh.resize(k);
- }
- else
- {
- std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
- }
- }
-
- virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
- {
- nbh.clear();
- for (std::size_t i = 0 ; i < data_.size() ; ++i)
- if (NearestNeighbors<_T>::distFun_(data_[i], data) <= radius)
- nbh.push_back(data_[i]);
- std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
- }
-
- virtual std::size_t size(void) const
- {
- return data_.size();
- }
-
- virtual void list(std::vector<_T> &data) const
- {
- data = data_;
- }
-
-protected:
-
- /** \brief The data elements stored in this structure */
- std::vector<_T> data_;
-
-private:
-
- struct ElemSort
- {
- ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
- {
- }
-
- bool operator()(const _T &a, const _T &b) const
- {
- return df_(a, e_) < df_(b, e_);
- }
-
- const _T &e_;
- const typename NearestNeighbors<_T>::DistanceFunction &df_;
- };
-
-};
-
-}
-
-#endif
diff --git a/include/hpp/fcl/knn/nearest_neighbors_sqrtapprox.h b/include/hpp/fcl/knn/nearest_neighbors_sqrtapprox.h
deleted file mode 100644
index e4748b6af68b7b1cfd42cdfec105d857571a81db..0000000000000000000000000000000000000000
--- a/include/hpp/fcl/knn/nearest_neighbors_sqrtapprox.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008-2014, Willow Garage, Inc.
- * Copyright (c) 2014-2015, Open Source Robotics Foundation
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Open Source Robotics Foundation nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/* Author: Ioan Sucan */
-
-#ifndef FCL_KNN_NEAREST_NEIGHBORS_SQRT_APPROX_H
-#define FCL_KNN_NEAREST_NEIGHBORS_SQRT_APPROX_H
-
-#include <hpp/fcl/knn/nearest_neighbors_linear.h>
-#include <hpp/fcl/exception.h>
-#include <algorithm>
-#include <cmath>
-
-namespace fcl
-{
-/** \brief A nearest neighbors datastructure that uses linear
- search. The linear search is done over sqrt(n) elements
- only. (Every sqrt(n) elements are skipped).
-
- \li Search for nearest neighbor is O(sqrt(n)).
- \li Search for k-nearest neighbors is O(n log(k)).
- \li Search for neighbors within a range is O(n log(n)).
- \li Adding an element to the datastructure is O(1).
- \li Removing an element from the datastructure O(n).
-*/
-template<typename _T>
-class NearestNeighborsSqrtApprox : public NearestNeighborsLinear<_T>
-{
-public:
- NearestNeighborsSqrtApprox(void) : NearestNeighborsLinear<_T>(), checks_(0), offset_(0)
- {
- }
-
- virtual ~NearestNeighborsSqrtApprox(void)
- {
- }
-
- virtual void clear(void)
- {
- NearestNeighborsLinear<_T>::clear();
- checks_ = 0;
- offset_ = 0;
- }
-
- virtual void add(const _T &data)
- {
- NearestNeighborsLinear<_T>::add(data);
- updateCheckCount();
- }
-
- virtual void add(const std::vector<_T> &data)
- {
- NearestNeighborsLinear<_T>::add(data);
- updateCheckCount();
- }
-
- virtual bool remove(const _T &data)
- {
- bool result = NearestNeighborsLinear<_T>::remove(data);
- if (result)
- updateCheckCount();
- return result;
- }
-
- virtual _T nearest(const _T &data) const
- {
- const std::size_t n = NearestNeighborsLinear<_T>::data_.size();
- std::size_t pos = n;
-
- if (checks_ > 0 && n > 0)
- {
- double dmin = 0.0;
- for (std::size_t j = 0 ; j < checks_ ; ++j)
- {
- std::size_t i = (j * checks_ + offset_) % n;
-
- double distance = NearestNeighbors<_T>::distFun_(NearestNeighborsLinear<_T>::data_[i], data);
- if (pos == n || dmin > distance)
- {
- pos = i;
- dmin = distance;
- }
- }
- offset_ = (offset_ + 1) % checks_;
- }
- if (pos != n)
- return NearestNeighborsLinear<_T>::data_[pos];
-
- throw Exception("No elements found in nearest neighbors data structure");
- }
-
-protected:
-
- /** \brief The maximum number of checks to perform when searching for a nearest neighbor */
- inline void updateCheckCount(void)
- {
- checks_ = 1 + (std::size_t)floor(sqrt((double)NearestNeighborsLinear<_T>::data_.size()));
- }
-
- /** \brief The number of checks to be performed when looking for a nearest neighbor */
- std::size_t checks_;
-
- /** \brief The offset to start checking at (between 0 and \e checks_) */
- mutable std::size_t offset_;
-
-};
-
-}
-
-#endif
diff --git a/test/test_fcl_simple.cpp b/test/test_fcl_simple.cpp
index 351f314f70d3f4b9b9dfffb6a40629cf094926e2..039b774bc714d0e88613b5f9aef951c6cd400d77 100644
--- a/test/test_fcl_simple.cpp
+++ b/test/test_fcl_simple.cpp
@@ -7,9 +7,6 @@
#include "fcl_resources/config.h"
#include <boost/filesystem.hpp>
#include <sstream>
-#include <hpp/fcl/math/vec_nf.h>
-#include <hpp/fcl/math/sampling.h>
-#include <hpp/fcl/knn/nearest_neighbors_GNAT.h>
#include <boost/assign/list_of.hpp>