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 // Comment-out for using random points:
38 #define REAL_DATASET_FILE "scan_071_points.dat"
39
40 //#define VERBOSE_OUTPUT
41
42 #ifdef VERBOSE_OUTPUT
43 # define VERB_COUT std::cout
44 #else
45 # define VERB_COUT if (0) std::cout
46 #endif
47
48
49 template <typename T>
50 struct PointCloud
51 {
52 struct Point
53 {
54 T x,y,z;
55 };
56
57 std::vector<Point> pts;
58
59 typedef PointCloud Derived; //!< In this case the dataset class is myself.
60
61 /// CRTP helper method
derivedPointCloud62 inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
63 /// CRTP helper method
derivedPointCloud64 inline Derived& derived() { return *static_cast<Derived*>(this); }
65
66 // Must return the number of data points
kdtree_get_point_countPointCloud67 inline size_t kdtree_get_point_count() const { return pts.size(); }
68
69 // Returns the dim'th component of the idx'th point in the class:
70 // Since this is inlined and the "dim" argument is typically an immediate value, the
71 // "if/else's" are actually solved at compile time.
kdtree_get_ptPointCloud72 inline T kdtree_get_pt(const size_t idx, const size_t dim) const
73 {
74 if (dim==0) return pts[idx].x;
75 else if (dim==1) return pts[idx].y;
76 else return pts[idx].z;
77 }
78
79 // Optional bounding-box computation: return false to default to a standard bbox computation loop.
80 // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
81 // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
82 template <class BBOX>
kdtree_get_bboxPointCloud83 bool kdtree_get_bbox(BBOX &bb) const { return false; }
84
85 // Default ctor.
PointCloudPointCloud86 PointCloud() {}
87
88 // load dataset in Freiburg 3D scans format:
PointCloudPointCloud89 PointCloud(const char* sFil)
90 {
91 FILE *f = fopen(sFil,"rt");
92 if (!f) throw std::runtime_error("can't open dataset file!");
93 pts.clear();
94
95 char str[300];
96 while (fgets(str,sizeof(str),f))
97 {
98 float x,y,z;
99 if (sscanf(str,"%*f %*f %*f %f %f %f\n",&x,&y,&z)==3)
100 {
101 pts.resize(pts.size()+1);
102 pts.rbegin()->x=x;
103 pts.rbegin()->y=y;
104 pts.rbegin()->z=z;
105 }
106 }
107 fprintf(stderr,"Read dataset: %u points\n", static_cast<unsigned int>(pts.size()));
108
109 fclose(f);
110 }
111 };
112
get_time()113 double get_time()
114 {
115 struct timeval tv;
116 gettimeofday(&tv,NULL);
117 return tv.tv_sec+tv.tv_usec/1000000.0;
118 }
119
my_random(const double min,const double max)120 double my_random(const double min,const double max)
121 {
122 return min+(max-min)*(rand() % 1000) / 1000.0;
123 }
124
125 template <typename T>
generateRandomPointCloud(PointCloud<T> & point,const size_t N,const T max_range=10)126 void generateRandomPointCloud(PointCloud<T> &point, const size_t N, const T max_range = 10)
127 {
128 VERB_COUT << "Generating "<< N << " point cloud...";
129 const double t0=get_time();
130
131 point.pts.resize(N);
132 for (size_t i=0;i<N;i++)
133 {
134 point.pts[i].x = max_range * (rand() % 1000) / T(1000);
135 point.pts[i].y = max_range * (rand() % 1000) / T(1000);
136 point.pts[i].z = max_range * (rand() % 1000) / T(1000);
137 }
138
139 const double t1=get_time();
140 VERB_COUT << "done in "<< (t1-t0)*1e3 << " ms\n";
141 }
142
143 // Load dataset only once:
144 #ifdef REAL_DATASET_FILE
145 PointCloud<float> cloud(REAL_DATASET_FILE);
146 #endif
147
148 template <typename num_t>
perf_test(const size_t N,const size_t max_leaf_elements)149 void perf_test(const size_t N, const size_t max_leaf_elements)
150 {
151 #ifndef REAL_DATASET_FILE
152 // Generate random points:
153 PointCloud<num_t> cloud;
154 generateRandomPointCloud(cloud, N);
155 num_t query_pt[3] = { 0.5, 0.5, 0.5};
156 #else
157 // Sample dataset is [-40,40]x[-40,40]x[0,15]: Query at random:
158 num_t query_pt[3] = { my_random(-40.0,40.0), my_random(-40.0,40.0), my_random(0,10)};
159 #endif
160
161 // construct an randomized kd-tree index using 4 kd-trees
162 double t0=get_time();
163
164 typedef KDTreeSingleIndexAdaptor<
165 L2_Simple_Adaptor<num_t, PointCloud<num_t> > ,
166 PointCloud<num_t>,
167 3 /* dim */
168 > my_kd_tree_t;
169
170 my_kd_tree_t index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(max_leaf_elements) );
171 index.buildIndex();
172 double t1=get_time();
173 const double At_build = t1-t0;
174 VERB_COUT << "Build Index<>: " << (t1-t0)*1e3 << " ms\n";
175
176 // do a knn search
177 t0=get_time();
178
179 const size_t num_results = 1;
180 size_t ret_index;
181 num_t out_dist_sqr;
182 nanoflann::KNNResultSet<num_t> resultSet(num_results);
183 resultSet.init(&ret_index, &out_dist_sqr );
184 index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
185 //index.knnSearch(query, indices, dists, num_results, nanoflann::SearchParams(10));
186
187 t1=get_time();
188 const double At_search = t1-t0;
189 VERB_COUT << "knnSearch(nn="<<num_results<<"): " << (t1-t0)*1e3 << " ms\n";
190 VERB_COUT << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << endl;
191
192 // Output for stats generation:
193 cout
194 << cloud.pts.size() << "\t "
195 << max_leaf_elements << "\t "
196 << At_build << "\t "
197 << At_search << "\n";
198
199 }
200
main(int argc,char ** argv)201 int main(int argc, char** argv)
202 {
203 // Max. number of elements per tree leaf:
204 size_t MaxLeafs[] = { 1, 2, 3, 4, 5, 10, 20, 50, 100, 500,1000, 10000};
205 for (size_t i=0;i<sizeof(MaxLeafs)/sizeof(MaxLeafs[0]);i++)
206 {
207 const size_t nPts = 1e5;
208 const size_t nReps = 200;
209 const size_t maxLeaf = MaxLeafs[i];
210
211 cerr << " ==== nPts:" << nPts << " === Max.Leaf: " << maxLeaf << "\n"; cerr.flush();
212 for (size_t repets=0;repets<nReps;repets++)
213 perf_test<float>(nPts, maxLeaf);
214 }
215
216 return 0;
217 }
218
219