1 /***********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright 2011 Jose Luis Blanco (joseluisblancoc@gmail.com).
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
19 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
20 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
22 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 *************************************************************************/
28
29 #include "../../include/nanoflann.hpp"
30 #include <cstdlib>
31 #include <iostream>
32 #include <sys/time.h>
33
34 using namespace std;
35 using namespace nanoflann;
36
37 //#define VERBOSE_OUTPUT
38
39 #ifdef VERBOSE_OUTPUT
40 # define VERB_COUT std::cout
41 #else
42 # define VERB_COUT if (0) std::cout
43 #endif
44
45
46 template <typename T>
47 struct PointCloud
48 {
49 struct Point
50 {
51 T x,y,z;
52 };
53
54 std::vector<Point> pts;
55
56 typedef PointCloud Derived; //!< In this case the dataset class is myself.
57
58 /// CRTP helper method
derivedPointCloud59 inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
60 /// CRTP helper method
derivedPointCloud61 inline Derived& derived() { return *static_cast<Derived*>(this); }
62
63 // Must return the number of data points
kdtree_get_point_countPointCloud64 inline size_t kdtree_get_point_count() const { return pts.size(); }
65
66 // Returns the dim'th component of the idx'th point in the class:
67 // Since this is inlined and the "dim" argument is typically an immediate value, the
68 // "if/else's" are actually solved at compile time.
kdtree_get_ptPointCloud69 inline float kdtree_get_pt(const size_t idx, const size_t dim) const
70 {
71 if (dim==0) return pts[idx].x;
72 else if (dim==1) return pts[idx].y;
73 else return pts[idx].z;
74 }
75
76 // Optional bounding-box computation: return false to default to a standard bbox computation loop.
77 // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
78 // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
79 template <class BBOX>
kdtree_get_bboxPointCloud80 bool kdtree_get_bbox(BBOX &bb) const { return false; }
81
82 };
83
get_time()84 double get_time()
85 {
86 struct timeval tv;
87 gettimeofday(&tv,NULL);
88 return tv.tv_sec+tv.tv_usec/1000000.0;
89 }
90
91 template <typename T>
generateRandomPointCloud(PointCloud<T> & point,const size_t N,const T max_range=10)92 void generateRandomPointCloud(PointCloud<T> &point, const size_t N, const T max_range = 10)
93 {
94 VERB_COUT << "Generating "<< N << " point cloud...";
95 const double t0=get_time();
96
97 point.pts.resize(N);
98 for (size_t i=0;i<N;i++)
99 {
100 point.pts[i].x = max_range * (rand() % 1000) / T(1000);
101 point.pts[i].y = max_range * (rand() % 1000) / T(1000);
102 point.pts[i].z = max_range * (rand() % 1000) / T(1000);
103 }
104
105 const double t1=get_time();
106 VERB_COUT << "done in "<< (t1-t0)*1e3 << " ms\n";
107 }
108
109 template <typename num_t>
perf_test(const size_t N)110 void perf_test(const size_t N)
111 {
112 PointCloud<num_t> cloud;
113
114 // Generate points:
115 generateRandomPointCloud(cloud, N);
116
117 num_t query_pt[3] = { 0.5, 0.5, 0.5};
118
119
120 // construct an randomized kd-tree index using 4 kd-trees
121 double t0=get_time();
122
123 typedef KDTreeSingleIndexAdaptor<
124 L2_Simple_Adaptor<num_t, PointCloud<num_t> > ,
125 PointCloud<num_t>,
126 3 /* dim */
127 > my_kd_tree_t;
128
129 my_kd_tree_t index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
130 index.buildIndex();
131 double t1=get_time();
132 const double At_build = t1-t0;
133 VERB_COUT << "Build Index<>: " << (t1-t0)*1e3 << " ms\n";
134
135 // do a knn search
136 t0=get_time();
137
138 const size_t num_results = 1;
139 size_t ret_index;
140 num_t out_dist_sqr;
141 nanoflann::KNNResultSet<num_t> resultSet(num_results);
142 resultSet.init(&ret_index, &out_dist_sqr );
143 index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
144 //index.knnSearch(query, indices, dists, num_results, nanoflann::SearchParams(10));
145
146 t1=get_time();
147 const double At_search = t1-t0;
148 VERB_COUT << "knnSearch(nn="<<num_results<<"): " << (t1-t0)*1e3 << " ms\n";
149 VERB_COUT << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << endl;
150
151 // Output for stats generation:
152 const double At_mat = 0;
153
154 cout
155 << N << "\t "
156 << At_mat << "\t "
157 << At_build << "\t "
158 << At_search << "\n";
159
160 }
161
162
main(int argc,char ** argv)163 int main(int argc, char** argv)
164 {
165 // Number of points
166 size_t Ns[] = {1e3, 5e3, 1e4, 5e4, 1e5, 2e5, 5e5, 7e5, 1e6, 2e6, 5e6};
167 // And repetitions for each point cloud size:
168 size_t nReps[] = {1e4, 1e3, 100, 100, 50, 50, 50, 20, 20, 10, 10};
169 for (size_t i=0;i<sizeof(Ns)/sizeof(Ns[0]);i++)
170 {
171 cerr << " ==== N:" << Ns[i] << " ===\n"; cerr.flush();
172
173 for (size_t repets=0;repets<nReps[i];repets++)
174 perf_test<float>(Ns[i]);
175 }
176
177 return 0;
178 }
179
180