#include-iostreamn#include-vectorn#include-ctimen#include-pclpoint_cloudhn#include-pcloctreeoctreehn#include-boostthreadthreadhppn#include-pclvisualizationpcl_visualizerhnusing-namespace-std;nintnmainint-argc-char-argvnn--------srandunsigned-inttimeN
把涉及PCL库的计算算法提取成不使用PCL库的代码，需要自己实现以下几个函数：
- rand()函数用于生成随机数，可以使用C++标准库中的std::rand()函数代替。
- 欧几里得距离计算函数可以自己实现，代码如下：
float euclideanDistance(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)
{
float dx = p1.x - p2.x;
float dy = p1.y - p2.y;
float dz = p1.z - p2.z;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
- 半径内近邻搜索可以使用kd-tree实现，代码如下：
class KDNode
{
public:
pcl::PointXYZ point;
int index;
KDNode* left;
KDNode* right;
KDNode(const pcl::PointXYZ& p, int i) : point(p), index(i), left(nullptr), right(nullptr) {}
};
class KDTree
{
public:
KDTree(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
std::vector<int> indices(cloud.size());
std::iota(indices.begin(), indices.end(), 0);
root = buildTree(cloud, indices, 0, cloud.size()-1, 0);
}
std::vector<int> radiusSearch(const pcl::PointXYZ& searchPoint, float radius)
{
std::vector<int> indices;
radiusSearch(root, searchPoint, radius, indices);
return indices;
}
private:
KDNode* root;
float getAxisValue(const pcl::PointXYZ& p, int axis)
{
switch (axis)
{
case 0: return p.x;
case 1: return p.y;
case 2: return p.z;
}
return 0;
}
KDNode* buildTree(const pcl::PointCloud<pcl::PointXYZ>& cloud, const std::vector<int>& indices, int start, int end, int depth)
{
if (start > end) return nullptr;
int mid = (start + end) / 2;
int axis = depth % 3;
std::nth_element(indices.begin()+start, indices.begin()+mid, indices.begin()+end+1,
[&](int i1, int i2) { return getAxisValue(cloud[i1], axis) < getAxisValue(cloud[i2], axis); });
KDNode* node = new KDNode(cloud[indices[mid]], indices[mid]);
node->left = buildTree(cloud, indices, start, mid-1, depth+1);
node->right = buildTree(cloud, indices, mid+1, end, depth+1);
return node;
}
void radiusSearch(KDNode* node, const pcl::PointXYZ& searchPoint, float radius, std::vector<int>& indices)
{
if (node == nullptr) return;
float dist = euclideanDistance(searchPoint, node->point);
if (dist < radius)
{
indices.push_back(node->index);
}
int axis = indices.size() % 3;
if (searchPoint.data[axis] - radius < node->point.data[axis])
{
radiusSearch(node->left, searchPoint, radius, indices);
}
if (searchPoint.data[axis] + radius > node->point.data[axis])
{
radiusSearch(node->right, searchPoint, radius, indices);
}
}
};
完整代码如下：
#include <iostream>
#include <vector>
#include <cmath>
#include <ctime>
using namespace std;
float euclideanDistance(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)
{
float dx = p1.x - p2.x;
float dy = p1.y - p2.y;
float dz = p1.z - p2.z;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
class KDNode
{
public:
pcl::PointXYZ point;
int index;
KDNode* left;
KDNode* right;
KDNode(const pcl::PointXYZ& p, int i) : point(p), index(i), left(nullptr), right(nullptr) {}
};
class KDTree
{
public:
KDTree(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
std::vector<int> indices(cloud.size());
std::iota(indices.begin(), indices.end(), 0);
root = buildTree(cloud, indices, 0, cloud.size()-1, 0);
}
std::vector<int> radiusSearch(const pcl::PointXYZ& searchPoint, float radius)
{
std::vector<int> indices;
radiusSearch(root, searchPoint, radius, indices);
return indices;
}
private:
KDNode* root;
float getAxisValue(const pcl::PointXYZ& p, int axis)
{
switch (axis)
{
case 0: return p.x;
case 1: return p.y;
case 2: return p.z;
}
return 0;
}
KDNode* buildTree(const pcl::PointCloud<pcl::PointXYZ>& cloud, const std::vector<int>& indices, int start, int end, int depth)
{
if (start > end) return nullptr;
int mid = (start + end) / 2;
int axis = depth % 3;
std::nth_element(indices.begin()+start, indices.begin()+mid, indices.begin()+end+1,
[&](int i1, int i2) { return getAxisValue(cloud[i1], axis) < getAxisValue(cloud[i2], axis); });
KDNode* node = new KDNode(cloud[indices[mid]], indices[mid]);
node->left = buildTree(cloud, indices, start, mid-1, depth+1);
node->right = buildTree(cloud, indices, mid+1, end, depth+1);
return node;
}
void radiusSearch(KDNode* node, const pcl::PointXYZ& searchPoint, float radius, std::vector<int>& indices)
{
if (node == nullptr) return;
float dist = euclideanDistance(searchPoint, node->point);
if (dist < radius)
{
indices.push_back(node->index);
}
int axis = indices.size() % 3;
if (searchPoint.data[axis] - radius < node->point.data[axis])
{
radiusSearch(node->left, searchPoint, radius, indices);
}
if (searchPoint.data[axis] + radius > node->point.data[axis])
{
radiusSearch(node->right, searchPoint, radius, indices);
}
}
};
int main(int argc, char** argv)
{
srand((unsigned int)time(NULL));
vector<pcl::PointXYZ> cloudData(1000);
// 创建点云数据
for (size_t i = 0; i < cloudData.size(); ++i)
{
cloudData[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloudData[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloudData[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
vector<vector<int>> octreeData(1000);
float resolution = 0.1;
for (size_t i = 0; i < cloudData.size(); ++i)
{
int x = std::floor(cloudData[i].x / resolution);
int y = std::floor(cloudData[i].y / resolution);
int z = std::floor(cloudData[i].z / resolution);
int index = x + y*10 + z*100;
octreeData[index].push_back(i);
}
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);
//半径内近邻搜索
vector<int>pointIdxRadiusSearch;
vector<float>pointRadiusSquaredDistance;
float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << endl;
KDTree kdTree(cloudData);
auto indices = kdTree.radiusSearch(searchPoint, radius);
for (size_t i = 0; i < indices.size(); ++i)
{
int index = indices[i];
cout << " " << cloudData[index].x
<< " " << cloudData[index].y
<< " " << cloudData[index].z
<< " (squared distance: " << euclideanDistance(searchPoint, cloudData[index]) << ")" << endl;
}
// 初始化点云可视化对象
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对点云着色可视化 (red).
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = cloudData.size();
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i] = cloudData[i];
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(cloud, 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud, target_color, "target cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
// 等待直到可视化窗口关闭
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}
return (0);
}
原文地址: https://www.cveoy.top/t/topic/p3t 著作权归作者所有。请勿转载和采集!