1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
*************************************************************************/
#pragma once
#include "nanoflann.hpp"
#include <vector>
// ===== This example shows how to use nanoflann with these types of containers:
// =======
// typedef std::vector<std::vector<double> > my_vector_of_vectors_t;
// typedef std::vector<Eigen::VectorXd> my_vector_of_vectors_t; // This
// requires #include <Eigen/Dense>
// =====================================================================================
/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the
* storage. 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
* 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)
*/
template <class VectorOfVectorsType, typename num_t = double, int DIM = -1,
class Distance = nanoflann::metric_L2, typename IndexType = size_t>
struct KDTreeVectorOfVectorsAdaptor {
typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType, num_t, DIM,
Distance>
self_t;
typedef
typename Distance::template traits<num_t, self_t>::distance_t metric_t;
typedef nanoflann::KDTreeSingleIndexAdaptor<metric_t, self_t, DIM, IndexType>
index_t;
index_t *index; //! The kd-tree index for the user to call its methods as
//! usual with any other FLANN index.
/// Constructor: takes a const ref to the vector of vectors object with the
/// data points
KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */,
const VectorOfVectorsType &mat,
const int leaf_max_size = 10)
: m_data(mat) {
assert(mat.size() != 0 && mat[0].size() != 0);
const size_t dims = mat[0].size();
if (DIM > 0 && static_cast<int>(dims) != DIM)
throw std::runtime_error(
"Data set dimensionality does not match the 'DIM' template argument");
index =
new index_t(static_cast<int>(dims), *this /* adaptor */,
nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
index->buildIndex();
}
~KDTreeVectorOfVectorsAdaptor() { delete index; }
const VectorOfVectorsType &m_data;
/** Query for the \a num_closest closest points to a given point (entered as
* query_point[0:dim-1]). Note that this is a short-cut method for
* index->findNeighbors(). 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 {
nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
resultSet.init(out_indices, out_distances_sq);
index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
}
/** @name Interface expected by KDTreeSingleIndexAdaptor
* @{ */
const self_t &derived() const { return *this; }
self_t &derived() { return *this; }
// Must return the number of data points
inline size_t kdtree_get_point_count() const { return m_data.size(); }
// Returns the dim'th component of the idx'th point in the class:
inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
return m_data[idx][dim];
}
// Optional bounding-box computation: return false to default to a standard
// bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in
// "bb" so it can be avoided to redo it again. Look at bb.size() to find out
// the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX> bool kdtree_get_bbox(BBOX & /*bb*/) const {
return false;
}
/** @} */
}; // end of KDTreeVectorOfVectorsAdaptor
|
