Commit a5d6a23a authored by Praetorius, Simon's avatar Praetorius, Simon

kdtree corrected

parent fbc5eb23
......@@ -25,6 +25,10 @@
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
/** \file kdtree_nanoflann.h */
#ifndef KDTREE_NANOFLANN_H
#define KDTREE_NANOFLANN_H
#include <nanoflann.hpp>
......@@ -47,16 +51,20 @@ typedef std::vector<WorldVector<double> > my_vector_of_vectors_t;
* The i'th vector represents a point in the state space.
*
* \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
* \tparam num_t The type of the point coordinates (typically, double or float).
* \tparam value_type The type of the point coordinates (typically, double or float).
* \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
* \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
* \tparam index_type The type for indices in the KD-tree index (typically, size_t of int)
*/
template <class VectorOfVectorsType, typename num_t = double, int DIM = -1, class Distance = nanoflann::metric_L2_Simple, typename IndexType = size_t>
template <class VectorOfVectorsType,
typename value_type = double,
int DIM = -1,
class Distance = nanoflann::metric_L2_Simple,
typename index_type = size_t>
struct KDTreeVectorOfWorldVectorsAdaptor
{
typedef KDTreeVectorOfWorldVectorsAdaptor<VectorOfVectorsType,num_t,DIM,Distance> self_t;
typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
typedef KDTreeVectorOfWorldVectorsAdaptor<VectorOfVectorsType, value_type, DIM, Distance, index_type> self_t;
typedef typename Distance::template traits<value_type, self_t>::distance_t metric_t;
typedef KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, index_type> index_t;
index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
......@@ -82,9 +90,9 @@ struct KDTreeVectorOfWorldVectorsAdaptor
* The user can also call index->... methods as desired.
* \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
*/
inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
inline void query(const value_type *query_point, const size_t num_closest, index_type *out_indices, value_type *out_distances_sq, const int nChecks_IGNORED = 10) const
{
nanoflann::KNNResultSet<typename VectorOfVectorsType::Scalar,IndexType> resultSet(num_closest);
nanoflann::KNNResultSet<typename VectorOfVectorsType::Scalar,index_type> resultSet(num_closest);
resultSet.init(out_indices, out_distances_sq);
index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
}
......@@ -105,18 +113,18 @@ struct KDTreeVectorOfWorldVectorsAdaptor
}
// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
inline value_type kdtree_distance(const value_type *p1, const size_t idx_p2,size_t size) const
{
num_t s=0;
value_type s=0;
for (size_t i=0; i<size; i++) {
const num_t d= p1[i]-m_data[idx_p2][i];
const value_type d= p1[i]-m_data[idx_p2][i];
s+=d*d;
}
return s;
}
// Returns the dim'th component of the idx'th point in the class:
inline num_t kdtree_get_pt(const size_t idx, int dim) const {
inline value_type kdtree_get_pt(const size_t idx, int dim) const {
return m_data[idx][dim];
}
......@@ -136,4 +144,4 @@ typedef KDTreeVectorOfWorldVectorsAdaptor< my_vector_of_vectors_t, double > KD_
} // end namespace experimental
#endif
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment