1.test.cpp
#include <ctime>
#include <cstdlib>
#include <iostream>
#include <nanoflann.hpp>
#include "utils.h"
using namespace std;
using namespace nanoflann;
void kdtree_demo(const size_t N)
{
//该点云为浮点云
PointCloud<float> cloud;
//生成个数为N的点集
generateRandomPointCloud(cloud, N);
//构造一个kd-tree索引
typedef KDTreeSingleIndexAdaptor<
L2_Simple_Adaptor<float, PointCloud<float>>,
PointCloud<float>,
3
> my_kd_tree_t;
my_kd_tree_t index(3, cloud, KDTreeSingleIndexAdaptorParams(10));
//编译索引
index.buildIndex();
//需要查询的点
const float query_pt[3] = {0.5, 0.5, 0.5};
//需要查询的邻近点个数
size_t num_results = 5;
//查询结果的索引
std::vector<size_t> ret_index(num_results);
//查询结果的距离
std::vector<float> out_dist_sqr(num_results);
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);
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;
}
int main()
{
//初始化随机数种子
srand(static_cast<unsigned int>(time(nullptr)));
kdtree_demo(100);
return 0;
}
2.utils.h
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2011-2016 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 <iostream>
template <typename T>
struct PointCloud
{
struct Point
{
T x,y,z;
};
std::vector<Point> pts;
// Must return the number of data points
inline size_t kdtree_get_point_count() const { return pts.size(); }
// Returns the dim'th component of the idx'th point in the class:
// Since this is inlined and the "dim" argument is typically an immediate value, the
// "if/else's" are actually solved at compile time.
inline T kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim == 0) return pts[idx].x;
else if (dim == 1) return pts[idx].y;
else return pts[idx].z;
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
};
template <typename T>
void generateRandomPointCloud(PointCloud<T> &point, const size_t N, const T max_range = 10)
{
// Generating Random Point Cloud
point.pts.resize(N);
for (size_t i = 0; i < N; i++)
{
point.pts[i].x = max_range * (rand() % 1000) / T(1000);
point.pts[i].y = max_range * (rand() % 1000) / T(1000);
point.pts[i].z = max_range * (rand() % 1000) / T(1000);
}
}
// This is an exampleof a custom data set class
template <typename T>
struct PointCloud_Quat
{
struct Point
{
T w,x,y,z;
};
std::vector<Point> pts;
// Must return the number of data points
inline size_t kdtree_get_point_count() const { return pts.size(); }
// Returns the dim'th component of the idx'th point in the class:
// Since this is inlined and the "dim" argument is typically an immediate value, the
// "if/else's" are actually solved at compile time.
inline T kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim==0) return pts[idx].w;
else if (dim==1) return pts[idx].x;
else if (dim==2) return pts[idx].y;
else return pts[idx].z;
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
};
template <typename T>
void generateRandomPointCloud_Quat(PointCloud_Quat<T> &point, const size_t N)
{
// Generating Random Quaternions
point.pts.resize(N);
T theta, X, Y, Z, sinAng, cosAng, mag;
for (size_t i=0;i<N;i++)
{
theta = static_cast<T>(nanoflann::pi_const<double>() * (((double)rand()) / RAND_MAX));
// Generate random value in [-1, 1]
X = static_cast<T>(2 * (((double)rand()) / RAND_MAX) - 1);
Y = static_cast<T>(2 * (((double)rand()) / RAND_MAX) - 1);
Z = static_cast<T>(2 * (((double)rand()) / RAND_MAX) - 1);
mag = sqrt(X*X + Y*Y + Z*Z);
X /= mag; Y /= mag; Z /= mag;
cosAng = cos(theta / 2);
sinAng = sin(theta / 2);
point.pts[i].w = cosAng;
point.pts[i].x = X * sinAng;
point.pts[i].y = Y * sinAng;
point.pts[i].z = Z * sinAng;
}
}
// This is an exampleof a custom data set class
template <typename T>
struct PointCloud_Orient
{
struct Point
{
T theta;
};
std::vector<Point> pts;
// Must return the number of data points
inline size_t kdtree_get_point_count() const { return pts.size(); }
// Returns the dim'th component of the idx'th point in the class:
// Since this is inlined and the "dim" argument is typically an immediate value, the
// "if/else's" are actually solved at compile time.
inline T kdtree_get_pt(const size_t idx, const size_t dim = 0) const
{
return pts[idx].theta;
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX& /* bb */) const { return false; }
};
template <typename T>
void generateRandomPointCloud_Orient(PointCloud_Orient<T> &point, const size_t N)
{
// Generating Random Orientations
point.pts.resize(N);
for (size_t i=0;i<N;i++) {
point.pts[i].theta = static_cast<T>(( 2 * nanoflann::pi_const<double>() * (((double)rand()) / RAND_MAX) ) - nanoflann::pi_const<double>());
}
}
void dump_mem_usage()
{
FILE* f = fopen("/proc/self/statm","rt");
if (!f) return;
char str[300];
size_t n = fread(str, 1, 200, f);
str[n] = 0;
printf("MEM: %s\n", str);
fclose(f);
}
3.编译源码
$ g++ -o test test.cpp -std=c++11
4.运行及其结果
$ ./test
knnSearch(): num_results=5
idx[0]=60 dist[0]=12.401
idx[1]=73 dist[1]=12.7531
idx[2]=32 dist[2]=18.3677
idx[3]=55 dist[3]=19.2101
idx[4]=14 dist[4]=19.4606
网友评论