first commit

pointcloud_kdd_radius.cpp

@@ -0,0 +1,141 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011-2024 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.
+ *************************************************************************/
+
+#include <cstdlib>
+#include <ctime>
+#include <iostream>
+#include <chrono>
+#include <nanoflann.hpp>
+
+#include "utils.h"
+
+using namespace std;
+
+const long long pointNum = 10000000; //节点数；
+const int knn = 100; //k近邻；
+long long queryNum = 10000;
+double maxRange = 1000;
+
+template <typename num_t>
+void kdtree_demo(const size_t N)
+{
+    using std::cout;
+    using std::endl;
+
+    PointCloud<num_t> cloud;
+    
+    // Generate points:
+    generateRandomPointCloud<double>(cloud, N, maxRange);
+    
+    // construct a kd-tree index:
+    using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor<
+        nanoflann::L2_Simple_Adaptor<num_t, PointCloud<num_t>>,
+        PointCloud<num_t>, 3 /* dim */
+        >;
+    auto insertStart = std::chrono::high_resolution_clock::now();
+    my_kd_tree_t index(3 /*dim*/, cloud, {10 /* max leaf */});
+
+    auto insertEnd = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> duration = insertEnd - insertStart;
+    double insertTime = duration.count();
+    cout<< "插入数据花费的时间为"<< insertTime << "秒"<<"("<<pointNum<<")"<<endl;
+#if 0
+	// Test resize of dataset and rebuild of index:
+	cloud.pts.resize(cloud.pts.size()*0.5);
+	index.buildIndex();
+#endif
+
+    //const num_t query_pt[3] = {0.5, 0.5, 0.5};
+    
+    // ----------------------------------------------------------------
+    // knnSearch():  Perform a search for the N closest points
+    // ----------------------------------------------------------------
+    {
+        auto queryStart = std::chrono::high_resolution_clock::now();
+        size_t                num_results = knn;
+        std::vector<uint32_t> ret_index(num_results);
+        std::vector<num_t>    out_dist_sqr(num_results);
+        for (size_t i = 0; i < queryNum; i++)
+        {
+            num_t query_pt[3] ;
+            query_pt[0] =  maxRange * (rand() % 1000) / 1000;
+            query_pt[1] =  maxRange * (rand() % 1000) / 1000;
+            query_pt[2] =  maxRange * (rand() % 1000) / 1000;
+            num_results = index.knnSearch(
+            &query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);
+                ret_index.resize(num_results);
+                out_dist_sqr.resize(num_results);
+        }
+        auto queryEnd = std::chrono::high_resolution_clock::now();
+        std::chrono::duration<double> duration2 = queryEnd - queryStart;
+        double queryTime = duration2.count();
+        cout<< "查询数据花费的时间为"<< queryTime << "秒"<<"("<<queryNum<<")"<<endl;
+        // In case of less points in the tree than requested:
+        
+    /*
+        cout << "knnSearch(): num_results=" << num_results << "\n";
+        for (size_t i = 0; i < num_results; i++)
+            cout << "idx[" << i << "]=" << ret_index[i] << " dist[" << i
+                 << "]=" << out_dist_sqr[i] << endl;
+        cout << "\n";
+        */
+    }
+    /*
+    // ----------------------------------------------------------------
+    // radiusSearch(): Perform a search for the points within search_radius
+    // ----------------------------------------------------------------
+    {
+        const num_t search_radius = static_cast<num_t>(0.1);
+        std::vector<nanoflann::ResultItem<uint32_t, num_t>> ret_matches;
+
+        // nanoflanSearchParamsameters params;
+        // params.sorted = false;
+
+        const size_t nMatches =
+            index.radiusSearch(&query_pt[0], search_radius, ret_matches);
+
+        cout << "radiusSearch(): radius=" << search_radius << " -> " << nMatches
+             << " matches\n";
+        for (size_t i = 0; i < nMatches; i++)
+            cout << "idx[" << i << "]=" << ret_matches[i].first << " dist[" << i
+                 << "]=" << ret_matches[i].second << endl;
+        cout << "\n";
+    }
+    */
+}
+
+int main()
+{
+    // Randomize Seed
+    cout<<"knn="<<knn<<endl;
+    cout<<"pointNum="<<pointNum<<endl;
+    srand(static_cast<unsigned int>(time(nullptr)));
+    //kdtree_demo<float>(4);
+    kdtree_demo<double>(pointNum);
+    return 0;
+}