1 /***********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright 2011-2016 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 <nanoflann.hpp>
30
31 #include <ctime>
32 #include <cstdlib>
33 #include <iostream>
34
35 using namespace std;
36 using namespace nanoflann;
37
38 void dump_mem_usage();
39
40 // This is an exampleof a custom data set class
41 template <typename T>
42 struct PointCloud
43 {
44 typedef T coord_t; //!< The type of each coordinate
45
46 struct Point
47 {
48 T x,y,z;
49 };
50
51 std::vector<Point> pts;
52 }; // end of PointCloud
53
54 // And this is the "dataset to kd-tree" adaptor class:
55 template <typename Derived>
56 struct PointCloudAdaptor
57 {
58 typedef typename Derived::coord_t coord_t;
59
60 const Derived &obj; //!< A const ref to the data set origin
61
62 /// The constructor that sets the data set source
PointCloudAdaptorPointCloudAdaptor63 PointCloudAdaptor(const Derived &obj_) : obj(obj_) { }
64
65 /// CRTP helper method
derivedPointCloudAdaptor66 inline const Derived& derived() const { return obj; }
67
68 // Must return the number of data points
kdtree_get_point_countPointCloudAdaptor69 inline size_t kdtree_get_point_count() const { return derived().pts.size(); }
70
71 // Returns the dim'th component of the idx'th point in the class:
72 // Since this is inlined and the "dim" argument is typically an immediate value, the
73 // "if/else's" are actually solved at compile time.
kdtree_get_ptPointCloudAdaptor74 inline coord_t kdtree_get_pt(const size_t idx, int dim) const
75 {
76 if (dim == 0) return derived().pts[idx].x;
77 else if (dim == 1) return derived().pts[idx].y;
78 else return derived().pts[idx].z;
79 }
80
81 // Optional bounding-box computation: return false to default to a standard bbox computation loop.
82 // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
83 // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
84 template <class BBOX>
kdtree_get_bboxPointCloudAdaptor85 bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
86
87 }; // end of PointCloudAdaptor
88
89
90 template <typename T>
generateRandomPointCloud(PointCloud<T> & point,const size_t N,const T max_range=10)91 void generateRandomPointCloud(PointCloud<T> &point, const size_t N, const T max_range = 10)
92 {
93 std::cout << "Generating "<< N << " point cloud...";
94 point.pts.resize(N);
95 for (size_t i = 0; i < N;i++)
96 {
97 point.pts[i].x = max_range * (rand() % 1000) / T(1000);
98 point.pts[i].y = max_range * (rand() % 1000) / T(1000);
99 point.pts[i].z = max_range * (rand() % 1000) / T(1000);
100 }
101
102 std::cout << "done\n";
103 }
104
105 template <typename num_t>
kdtree_demo(const size_t N)106 void kdtree_demo(const size_t N)
107 {
108 PointCloud<num_t> cloud;
109
110 // Generate points:
111 generateRandomPointCloud(cloud, N);
112
113 num_t query_pt[3] = { 0.5, 0.5, 0.5 };
114
115 typedef PointCloudAdaptor<PointCloud<num_t> > PC2KD;
116 const PC2KD pc2kd(cloud); // The adaptor
117
118 // construct a kd-tree index:
119 typedef KDTreeSingleIndexAdaptor<
120 L2_Simple_Adaptor<num_t, PC2KD > ,
121 PC2KD,
122 3 /* dim */
123 > my_kd_tree_t;
124
125 dump_mem_usage();
126
127 my_kd_tree_t index(3 /*dim*/, pc2kd, KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
128 index.buildIndex();
129 dump_mem_usage();
130
131 // do a knn search
132 const size_t num_results = 1;
133 size_t ret_index;
134 num_t out_dist_sqr;
135 nanoflann::KNNResultSet<num_t> resultSet(num_results);
136 resultSet.init(&ret_index, &out_dist_sqr );
137 index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
138 //index.knnSearch(query, indices, dists, num_results, mrpt_flann::SearchParams(10));
139
140 std::cout << "knnSearch(nn="<<num_results<<"): \n";
141 std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << endl;
142
143 }
144
main()145 int main()
146 {
147 // Randomize Seed
148 srand(time(NULL));
149 kdtree_demo<float>(1000000);
150 kdtree_demo<double>(1000000);
151 return 0;
152 }
153
dump_mem_usage()154 void dump_mem_usage()
155 {
156 FILE* f=fopen("/proc/self/statm","rt");
157 if (!f) return;
158 char str[300];
159 size_t n=fread(str,1,200,f);
160 str[n]=0;
161 printf("MEM: %s\n",str);
162 fclose(f);
163 }
164