C++实现PCL库的kd树划分,并且遍历叶子节点
时间: 2023-05-26 17:03:44 浏览: 61
下面给出C语言实现PCL库的kd树划分和遍历叶子节点的示例代码:
1. kd树划分
```c
void buildKDTree(PointCloud<PointXYZ>::Ptr cloud, int depth, KDNode<PointXYZ> &node) {
if(cloud->empty()) {
return;
}
int split_dim = depth % 3;
double split_val = calculateSplitVal(cloud, split_dim);
PointCloud<PointXYZ>::Ptr left_cloud(new PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr right_cloud(new PointCloud<PointXYZ>);
for(int i = 0; i < cloud->size(); i++) {
if(cloud->points[i].data[split_dim] < split_val) {
left_cloud->push_back(cloud->points[i]);
} else {
right_cloud->push_back(cloud->points[i]);
}
}
node.split_dim = split_dim;
node.split_val = split_val;
node.left_child.reset(new KDNode<PointXYZ>);
node.right_child.reset(new KDNode<PointXYZ>);
buildKDTree(left_cloud, depth+1, *(node.left_child));
buildKDTree(right_cloud, depth+1, *(node.right_child));
}
double calculateSplitVal(PointCloud<PointXYZ>::Ptr cloud, int dim) {
double sum = 0;
for(int i = 0; i < cloud->size(); i++) {
sum += cloud->points[i].data[dim];
}
return sum / cloud->size();
}
```
2. 遍历叶子节点
```c
void traverseLeafNodes(KDNode<PointXYZ> &node) {
if(node.left_child == nullptr && node.right_child == nullptr) {
// this is a leaf node
// do something with it
return;
}
if(node.left_child) {
traverseLeafNodes(*(node.left_child));
}
if(node.right_child) {
traverseLeafNodes(*(node.right_child));
}
}
```
这里假设KDNode的定义如下:
```c
template<typename PointT>
struct KDNode {
int split_dim;
double split_val;
shared_ptr<KDNode<PointT>> left_child;
shared_ptr<KDNode<PointT>> right_child;
};
```
其中,结构体成员split_dim表示分割维度,split_val表示分割点的值。left_child和right_child分别是左子树和右子树的指针。shared_ptr是C++11的智能指针,可以自动回收内存,避免内存泄漏。
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![txt](https://img-home.csdnimg.cn/images/20210720083642.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)