c++(kdtree)

最新推荐文章于 2024-05-03 20:26:31 发布
最新推荐文章于 2024-05-03 20:26:31 发布
阅读量751 收藏 4
点赞数 2
分类专栏： 点云课sl
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_43384504/article/details/106123767
版权 
#include <iostream>
#include <vector>
#include <algorithm>
#include <string>
#include <cmath>
#include <numeric>
#include<time.h>

using namespace std;

struct Point {

	Point(float xx, float yy, float zz) :x(xx), y(yy), z(zz){};
	Point() :x(0), y(0), z(0){};
	float x;
	float y;
	float z;
	bool operator == (const Point &p)const
	{
		return (x == p.x && y == p.y && z == p.z );
	}
	float operator [](int i)
	{
		if (i > 5)
			std::cout << "colorPoint[i]"<< std::endl;
			switch (i)
			{
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				break;
			}
		return 0;
	}
};
//操作符重载
std::ostream& operator <<(std::ostream &os, const std::vector<Point> &P)
{
	for (auto &i : P)
		std::cout << i.x << " " << i.y << " " << i.z << std::endl;
	//cout << endl;
	os.flush();
	return os;
}
std::ostream& operator <<(std::ostream &os, const Point &P)
{

	std::cout << P.x << " " << P.y << " " << P.z;
	//cout << endl;
	os.flush();
	return os;
}

class KdTree {
public:
	Point root;
	KdTree* parent;
	KdTree* leftChild;
	KdTree* rightChild;
	int ClusterIdx;

	int attribute;

	//默认构造函数
	KdTree()
	{
		parent = NULL;
		leftChild = NULL;
		rightChild = NULL;
		ClusterIdx = -1;
	}

	//判断kd树是否只是一个叶子结点
	bool isLeaf()
	{
		return (rightChild == NULL && leftChild == NULL);
	}
	//判断是否是树的根结点
	bool isRoot()
	{
		return (parent == NULL);
	}
	//判断该子kd树的根结点是否是其父kd树的左结点
	bool isLeft()
	{
		return parent->leftChild->root == root;
	}
	//判断该子kd树的根结点是否是其父kd树的右结点
	bool isRight()
	{
		return parent->rightChild->root == root;
	}
};

std::vector<std::vector<float> > Transpose(std::vector<Point> Matrix)
{
	unsigned row = (unsigned)Matrix.size();
	unsigned col = 3;
	std::vector<float> tmp_x, tmp_y, tmp_z;
	for (auto p : Matrix)
	{
		tmp_x.push_back(p.x);
		tmp_y.push_back(p.y);
		tmp_z.push_back(p.z);
	}
	std::vector<std::vector<float> > Trans;
	Trans.push_back(tmp_x);
	Trans.push_back(tmp_y);
	Trans.push_back(tmp_z);
	return Trans;
}

float findMiddleValue(std::vector<float> vec)
{
	sort(vec.begin(), vec.end());
	auto pos = vec.size() / 2;
	return vec[pos];
}

float computeVar(std::vector<float> A)
{
	
	float sum = (float)std::accumulate(A.begin(), A.end(), 0);
	float mean = sum / A.size();

	float accum = 0;
	for (auto &i : A)
		accum += (i - mean)*(i - mean);
	float var = accum / (A.size() - 1);
	return var;
}

int findSplitAttribute(std::vector<std::vector<float> > transData)
{
	std::vector<int> vars;
	for (auto i : transData)
		vars.push_back(computeVar(i));
	auto biggest = std::max_element(std::begin(vars), std::end(vars));
	int idx = std::distance(std::begin(vars), biggest);
	return idx;
}
void buildKdTree(KdTree* tree, std::vector<Point> data, unsigned depth)
{

	//样本的数量
	int samplesNum = data.size();
	
	//终止条件
	if (samplesNum == 0)
	{
		return;
	}
	if (samplesNum == 1)
	{
		tree->root = data[0];
		return;
	}
	//样本的维度
	std::vector<std::vector<float> > transData = Transpose(data);
	//选择切分属性
	//unsigned splitAttribute = depth % k;
	unsigned splitAttribute = findSplitAttribute(transData);
	std::vector<float> splitAttributeValues = transData[splitAttribute];
	//选择切分值
	float splitValue = findMiddleValue(splitAttributeValues);
	//cout << "splitValue" << splitValue  << endl;

	// 根据选定的切分属性和切分值，将数据集分为两个子集
	std::vector<Point> subset1;
	std::vector<Point> subset2;
	for (unsigned i = 0; i < samplesNum; ++i)
	{
		if (splitAttributeValues[i] == splitValue)
			tree->root = data[i];
		else
		{
			if (splitAttributeValues[i] < splitValue)
				subset1.push_back(data[i]);
			else
				subset2.push_back(data[i]);
		}
	}


	tree->attribute = splitAttribute;
	if (!subset1.empty())
	{
		tree->leftChild = new KdTree;
		tree->leftChild->parent = tree;
		buildKdTree(tree->leftChild, subset1, depth + 1);
	}
	if (!subset2.empty())
	{
		tree->rightChild = new KdTree;
		tree->rightChild->parent = tree;
		buildKdTree(tree->rightChild, subset2, depth + 1);
	}
}

void printKdTree(KdTree *tree, unsigned depth)
{
	for (unsigned i = 0; i < depth; ++i)
		cout << "\t";

	cout << tree->root << endl;
	if (tree->leftChild == NULL && tree->rightChild == NULL)//叶子节点
		return;
	else //非叶子节点
	{
		if (tree->leftChild != NULL)
		{
			for (unsigned i = 0; i < depth + 1; ++i)
				cout << "\t";
			cout << " left:";
			printKdTree(tree->leftChild, depth + 1);
		}

		cout << endl;
		if (tree->rightChild != NULL)
		{
			for (unsigned i = 0; i < depth + 1; ++i)
				cout << "\t";
			cout << "right:";
			printKdTree(tree->rightChild, depth + 1);
		}
		cout << endl;
	}
}

void getKdTreeValue(KdTree *tree, std::vector<Point> &out, std::vector<int> &idx)
{
	idx.push_back(tree->ClusterIdx);
	out.push_back(tree->root);
	if (tree->leftChild == NULL && tree->rightChild == NULL)//叶子节点
		return;
	else //非叶子节点
	{
		if (tree->leftChild != NULL)
			getKdTreeValue(tree->leftChild, out, idx);
		if (tree->rightChild != NULL)
			getKdTreeValue(tree->rightChild, out, idx);
	}
}
bool searchUndefinedIdx(KdTree *tree, KdTree* & newTree)
{
	//if (tree == NULL)
	//	return false;
	if (tree->ClusterIdx == -1)
	{
		newTree = tree;
		return true;
	}
	if (tree->leftChild == NULL && tree->rightChild == NULL)//叶子节点
		return false;
	//else
	//	return searchUndefinedIdx(tree->leftChild, newTree) | 
	//	searchUndefinedIdx(tree->rightChild, newTree);
	bool left = false;
	bool right = false;
	if (tree->leftChild != NULL)
		left = searchUndefinedIdx(tree->leftChild, newTree);
	if (tree->rightChild != NULL)
		right = searchUndefinedIdx(tree->rightChild, newTree);
	return (right | left);

}
float measureDistance(Point point1, Point point2, unsigned method)
{
	switch (method)
	{
	case 0://欧氏距离
	{
		float res = pow((point1.x - point2.x), 2) + pow((point1.y - point2.y), 2) + pow((point1.z - point2.z), 2);
		return sqrt(res);
	}
	case 1://曼哈顿距离
	{

		float res = abs(point1.x - point2.x) + abs(point1.y - point2.y) + abs(point1.z - point2.z);
		return res;
	}
	case 2://点云水平距离
	{
		float res = pow((point1.x - point2.x), 2) + pow((point1.y - point2.y), 2) + 0.1*pow((point1.z - point2.z), 2);
		return sqrt(res);
	}
	default:
	{
		cerr << "Invalid method!!" << endl;
		return -1;
	}
	}
}

void searchRadiusNeighbor(KdTree *tree, Point goal, float radius, std::vector<Point> &out)
{
	KdTree* currentTree = tree;
	//这里的 currentNearest分割点的坐标
	Point currentNearest = currentTree->root;
	// 点云自定义距离
	float currentDistance = measureDistance(goal, currentNearest, 2);
	//如果距离符合，输出
	if (currentDistance <= radius)
		out.push_back(currentNearest);
	// 是否为叶节点
	if (!currentTree->isLeaf())
	{
		// 当前分割属性
		unsigned index = currentTree->attribute;
		float districtDistance = -1;
		switch (index)
		{
		case 0:
		{
			districtDistance = goal.x - currentTree->root.x;
			break;
		}
		case 1:
		{
			districtDistance = goal.y - currentTree->root.y;
			break;
		}case 2:
		{
			districtDistance = goal.z - currentTree->root.z;
			break;
		}
		default:
			break;
		}
		if (districtDistance == -1)
			cout << "出错districtDistance == -1" << endl;
		if (abs(districtDistance) <= radius)
		{
			if (currentTree->leftChild != NULL)
				searchRadiusNeighbor(currentTree->leftChild, goal, radius, out);
			if (currentTree->rightChild != NULL)
				searchRadiusNeighbor(currentTree->rightChild, goal, radius, out);
		}
		else
		{
			if (currentTree->rightChild != NULL && districtDistance < 0)
				searchRadiusNeighbor(currentTree->leftChild, goal, radius, out);
			else if (currentTree->rightChild != NULL && districtDistance > 0)
				searchRadiusNeighbor(currentTree->rightChild, goal, radius, out);
		}
	}
}

void searchRadiusNeighborConditional(KdTree *tree, KdTree *target_tree, float radius, std::vector<KdTree*> &out)
{
	KdTree* currentTree = tree;
	Point currentNearest = currentTree->root;
	Point goal = target_tree->root;

	// 点云自定义距离
	if (currentTree->ClusterIdx == -1)
	{
		float currentDistance = measureDistance(goal, currentNearest, 0);
		if (currentDistance <= radius)
		{
			out.push_back(currentTree);
			currentTree->ClusterIdx = target_tree->ClusterIdx;
		}
	}


	// 是否为叶节点
	if (!currentTree->isLeaf())
	{
		// 当前分割属性
		unsigned index = currentTree->attribute;
		float districtDistance = goal[index] - currentTree->root[index];

		if (abs(districtDistance) <= radius)
		{
			if (currentTree->leftChild != NULL)
				searchRadiusNeighborConditional(currentTree->leftChild, target_tree, radius, out);
			if (currentTree->rightChild != NULL)
				searchRadiusNeighborConditional(currentTree->rightChild, target_tree, radius, out);
		}
		else
		{
			if (currentTree->leftChild != NULL && districtDistance < 0)
				searchRadiusNeighborConditional(currentTree->leftChild, target_tree, radius, out);
			else if (currentTree->rightChild != NULL && districtDistance > 0)
				searchRadiusNeighborConditional(currentTree->rightChild, target_tree, radius, out);
		}
	}
}



int main()
{
	//读取 .bin 文件 (这里可以写一个接口)
	std::FILE *pFile = fopen("000000.bin", "rb");
	fseek(pFile, 0, SEEK_END);    // file pointer goes to the end of the file  
	long fileSize = ftell(pFile); // file size  
	rewind(pFile);                // rewind file pointer to the beginning 
	float *rawData = new float[fileSize];
	fread(rawData, sizeof(float), fileSize / sizeof(float), pFile);
	long number_of_points = fileSize / 4 / sizeof(float);//the number of  points
	std::vector<Point> pcd;

	for (size_t i = 0; i < number_of_points; i++)
	{
		Point temp;
		temp.x = *(rawData+i*4);
		temp.y = *(rawData+i*4+1);
		temp.z = *(rawData+i*4+2);
		pcd.push_back(temp);
	}
	

	KdTree my_kdtree;
	unsigned dee=0;


	clock_t start, end;
	start = clock();
	buildKdTree(&my_kdtree, pcd, dee);
	end = clock();		//程序结束用时
	double endtime = (double)(end - start) / CLOCKS_PER_SEC;
	cout << "data size" << pcd.size() << endl;
	cout << "Total time(build tree):" << endtime * 1000 << "ms" << endl;	//ms为单位




	return 0;
}

 

 kdtree 的c++ 实现，12w的点大概要500ms，速度还是很慢，后面再看能不能改进（有同学是50ms）

Open3D C++ KD tree的使用
学习使你进步。
08-15 163
在这篇文章中，我们介绍了Open3D库中的KD tree，并展示了如何使用search_knn_vector_3d函数来搜索最近邻点。同时，也介绍了search_radius_vector_3d和search_hybrid_vector_3d函数。这些函数可以帮助我们方便快速地搜索点云数据中的相似点。除了search_knn_vector_3d之外，Open3D库还提供了其他的搜索函数，如search_radius_vector_3d和search_hybrid_vector_3d。
kdtree的C/C++实现
07-28
一个非常高效基于Kd-tree数据结构的2D、3D的近邻查询算法，原作者John Tsiombikas，使用C++封装并给了测试测序。
KDTree C++实现
m0_51800912的博客
07-13 806
简介   k-d树（k-dimensional），是一种分割k维数据空间的数据结构（对数据点在k维空间中划分的一种数据结构），主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。 举例   上图就是一颗kdtree，可以看出kdtree是二叉搜索树的变种。   kdtree的性质： kdtree具有平衡的特质，两树叶的高度差不超过1。(树越平衡代表着分割得越平均，搜索的时间越少) 数据只存放在叶子结点，而根结点和中间结点存放一些空间划分信息（例如划分维度、划分值）。 将每一个元组按0排序（第一
KD-Tree（C++实现）
Nick
11-27 7383
参考资料： https://blog.csdn.net/dymodi/article/details/46830071 https://github.com/WiseDoge/libkdtree 作为存取高维数据的一种数据结构，k-d tree 在静态查询和插入方面的效率还是很高的。本文在这里对 k-d tree 的内容作一些介绍，可能也会结合自己使用 k-d tree 的一些体验作一些点评...
KDTree的C++实现
nnzzll的博客
05-13 2865
KDTree的C++实现
kdTree算法实现（C++代码实现）
weixin_45645810的博客
06-20 1903
kdTree算法实现（C++代码实现） VS中需要安装Eigen包，可自行百度下载，添加至属性——包含项目即可。 kdTree.h文件 #ifndef KD_TREE_H #define KD_TREE_H #define lson (rt << 1)//左节点 #define rson (rt << 1 | 1)//右节点 #include #include #include <Eigen/Dense> #include using std::vector; us
KDTree（C++）.zip
07-13
使用C++实现的基于KDTree的邻近搜索，代码添加了注释。
c++ KDTree
11-11
KDTree() : root(NULL) {} void insert(vector<int> point) { root = insert(root, point, 0); } Node* insert(Node* node, vector<int> point, int depth) { if (node == NULL) { return new Node(point);...
c++ kd tree
07-25
kd树是一种用于高维数据搜索的数据结构。它被用于解决空间搜索问题，如最近邻搜索和范围搜索。通过将数据点按照特定维度进行划分，并以树形结构组织，kd树可以有效地减小搜索空间，提高搜索效率。 构建kd树的过程...
kdtree实现源码(C++) 范围查询
05-27
kd-tree（k-dimensional树的简称），是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。K-D树是二进制空间分割树的特殊的情况。 在计算机科学里，k-d树（ k-维树的...
kd-Tree源码（c++）
03-01
之前写的一个kd-tree，程序里用了一些qt的数据结构，基本不影响阅读。该程序仅供初学kd-tree的同学参考，
KD-Tree C++程序
05-31
一个清晰明了的KD-Tree程序，用来学习很不错
kd-tree c++ 实现
06-19
提供了一种简单的的kd-tree 实现。包含三个.h文件
kd-Tree c++实现（kd树）
10-25
c++实现的一个类库，用于数字几何处理 A Library for Approximate Nearest Neighbor Searching 1 源码 2.win下测试vs工程 3.pdf文档
KDTree  C++实现
weixin_30586085的博客
08-14 131
参考：https://blog.csdn.net/qing101hua/article/details/53228668 #include <iostream> #include <vector> #include <stack> #include <cmath> #define KDtreeSize 1000 #define UL unsig...
KD树的C++实现
新安浅滩的博客
10-06 6409
理论介绍 kd树(K-dimension tree)是一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。kd树是是一种二叉树，表示对k维空间的一个划分，构造kd树相当于不断地用垂直于坐标轴的超平面将K维空间切分，构成一系列的K维超矩形区域。kd树的每个结点对应于一个k维超矩形区域。利用kd树可以省去对大部分数据点的搜索，从而减少搜索的计算量。kd树的算法步骤 　 kd树可
KD-Tree 算法的 C++ 实现
Doge的博客
12-15 1483
KD-Tree 算法的 C++ 实现阅读本文前，建议查阅相关资料，了解 KNN 算法与 KD 树。基础知识如图所示，假设一个点 a 目前的最近邻点为 b，如果存在相对于 b 离 a 更近的点，那么这个点一定在以 a 为圆心，ab 为半径的圆内。 现右侧的区域是未知的，如果 a 到分界线的距离 l 大于目前的最近距离 L（圆半径），则没有必要在右侧的未知区域继续寻找最近邻点（如图一），反之，则要继续
KD树 C++实现
yhaida的博客
01-04 728
已知样本空间如何快速查询得到其近邻？唯有以空间换时间，建立索引是最基本的解决方式。但是索引建立的方式各有不同，kd树只是是其中一种。它的思想如同分治法，即：利用已有数据对k维空间进行切分。注意：在一维空间里面，二叉查找树就是KD树的情形。对于一颗二叉查找树，可以在空间上理解：树的每个节点把对应父节点切成的空间再切分，从而形成各个不同的子空间。查找某点的所在位置时，就变成了查找点所在子空间。二叉查找树仅仅是一维，如果换到二维？如下图， 这样可以将平面分为两个部分。
KD-Tree构建及邻近点搜索 C++实现
最新发布
ts00001M的博客
05-03 1175
KDTree C++实现
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值