Skip to content
Snippets Groups Projects
Commit fdd60305 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Remove knn

parent 4f520109
No related branches found
No related tags found
No related merge requests found
......@@ -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_));
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
/*
* 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
/*
* 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
......@@ -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>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment