Commit fdd60305 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Remove knn

parent 4f520109
......@@ -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
......
......@@ -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
/*
* 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
/*
* 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
This diff is collapsed.
/*
* 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> &params)
: 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> &params)
{
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_));