@@ -79,7 +79,7 @@ size_t nanoflann_neighbors(std::vector<scalar_t>& queries, std::vector<scalar_t>
7979 // CLoud variable
8080 PointCloud<scalar_t > pcd;
8181 pcd.set (supports, dim);
82- // Cloud query
82+ // Cloud query
8383 PointCloud<scalar_t >* pcd_query = new PointCloud<scalar_t >();
8484 (*pcd_query).set (queries, dim);
8585
@@ -95,7 +95,6 @@ size_t nanoflann_neighbors(std::vector<scalar_t>& queries, std::vector<scalar_t>
9595 index = new my_kd_tree_t (dim, pcd, tree_params);
9696 index->buildIndex ();
9797 // Search neigbors indices
98- // ***********************
9998
10099 // Search params
101100 nanoflann::SearchParams search_params;
@@ -137,7 +136,7 @@ size_t nanoflann_neighbors(std::vector<scalar_t>& queries, std::vector<scalar_t>
137136 size_t n_queries = (*pcd_query).pts .size ();
138137 size_t actual_threads = std::min ((long long )n_threads, (long long )n_queries);
139138
140- std::thread* tid[ actual_threads] ;
139+ std::vector<std:: thread*> tid ( actual_threads) ;
141140
142141 size_t start, end;
143142 size_t length;
@@ -147,17 +146,8 @@ size_t nanoflann_neighbors(std::vector<scalar_t>& queries, std::vector<scalar_t>
147146 else {
148147 auto res = std::lldiv ((long long )n_queries, (long long )n_threads);
149148 length = (size_t )res.quot ;
150- /*
151- if (res.rem == 0) {
152- length = res.quot;
153- }
154- else {
155- length =
156- }
157- */
158149 }
159150 for (size_t t = 0 ; t < actual_threads; t++) {
160- // sem->wait();
161151 start = t*length;
162152 if (t == actual_threads-1 ) {
163153 end = n_queries;
@@ -233,12 +223,10 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
233223 double radius, int dim, int64_t max_num){
234224
235225
236- // Initiate variables
237- // ******************
238- // indices
226+ // indices
239227 size_t i0 = 0 ;
240228
241- // Square radius
229+ // Square radius
242230 const scalar_t r2 = static_cast <scalar_t >(radius*radius);
243231
244232 // Counting vector
@@ -257,7 +245,6 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
257245 eps = 0 ;
258246 }
259247 // Nanoflann related variables
260- // ***************************
261248
262249 // CLoud variable
263250 PointCloud<scalar_t > current_cloud;
@@ -271,21 +258,20 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
271258 // KDTree type definition
272259 typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Adaptor<scalar_t , PointCloud<scalar_t > > , PointCloud<scalar_t >> my_kd_tree_t ;
273260
274- // Pointer to trees
261+ // Pointer to trees
275262 my_kd_tree_t * index;
276263 // Build KDTree for the first batch element
277264 current_cloud.set_batch (supports, sum_sb, s_batches[b], dim);
278265 index = new my_kd_tree_t (dim, current_cloud, tree_params);
279266 index->buildIndex ();
280- // Search neigbors indices
281- // ***********************
282- // Search params
267+ // Search neigbors indices
268+ // Search params
283269 nanoflann::SearchParams search_params;
284270 search_params.sorted = true ;
285271
286272 for (auto & p : query_pcd.pts ){
287273 auto p0 = *p;
288- // Check if we changed batch
274+ // Check if we changed batch
289275
290276 scalar_t * query_pt = new scalar_t [dim];
291277 std::copy (p0.begin (), p0.end (), query_pt);
@@ -295,19 +281,19 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
295281 sum_sb += s_batches[b];
296282 b++;
297283
298- // Change the points
284+ // Change the points
299285 current_cloud.pts .clear ();
300286 current_cloud.set_batch (supports, sum_sb, s_batches[b], dim);
301- // Build KDTree of the current element of the batch
287+ // Build KDTree of the current element of the batch
302288 delete index;
303289 index = new my_kd_tree_t (dim, current_cloud, tree_params);
304290 index->buildIndex ();
305291 }
306- // Initial guess of neighbors size
292+ // Initial guess of neighbors size
307293 all_inds_dists[i0].reserve (max_count);
308- // Find neighbors
294+ // Find neighbors
309295 size_t nMatches = index->radiusSearch (query_pt, r2+eps, all_inds_dists[i0], search_params);
310- // Update max count
296+ // Update max count
311297
312298 std::vector<std::pair<size_t , float > > indices_dists;
313299 nanoflann::RadiusResultSet<float ,size_t > resultSet (r2, indices_dists);
@@ -316,14 +302,17 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
316302
317303 if (nMatches > max_count)
318304 max_count = nMatches;
319- // Increment query idx
305+ // Increment query idx
320306 i0++;
321307 }
308+
309+
310+
322311 // how many neighbors do we keep
323312 if (max_num > 0 ) {
324313 max_count = max_num;
325314 }
326- // Reserve the memory
315+ // Reserve the memory
327316
328317 size_t size = 0 ; // total number of edges
329318 for (auto & inds_dists : all_inds_dists){
@@ -332,6 +321,7 @@ size_t batch_nanoflann_neighbors (std::vector<scalar_t>& queries,
332321 else
333322 size += max_count;
334323 }
324+
335325 neighbors_indices->resize (size * 2 );
336326 i0 = 0 ;
337327 sum_sb = 0 ;
0 commit comments