主要记录源码,加上了一些自己的理解。
一、k-d tree 实验
代码
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
int main (int argc, char**argv)
{
/* 创建点云对象 填充数据 */
srand (time (NULL)); //初始化随机种子
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->width =1000; //1000数据量
cloud->height =1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i=0; i< cloud->points.size (); ++i)
{
cloud->points[i].x =1024.0f* rand () / (RAND_MAX +1.0f);
cloud->points[i].y =1024.0f* rand () / (RAND_MAX +1.0f);
cloud->points[i].z =1024.0f* rand () / (RAND_MAX +1.0f);
}
/* 创建KdTreeFLANN对象 导入点云 创建查询点 */
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud); //设置搜索空间 导入点云数据
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);
/* 1、k近邻搜索K个近邻点 */
int K =10;
std::vector<int> pointIdxNKNSearch(K); //存储查询点K个近邻索引 即点云的索引
std::vector<float> pointNKNSquaredDistance(K); //存储查询点到近邻点对应平方距离
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0 )
{
//打印出找到的近邻坐标
for (size_t i=0; i<pointIdxNKNSearch.size (); ++i)
std::cout<<" "<< cloud->points[ pointIdxNKNSearch[i] ].x
<<" "<< cloud->points[pointIdxNKNSearch[i] ].y
<<" "<< cloud->points[pointIdxNKNSearch[i] ].z
<<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
}
/* 2、k近邻搜索 在半径r内搜索近邻 半径随机产生*/
std::vector<int> pointIdxRadiusSearch; //存储近邻点索引 即点云的索引
std::vector<float> pointRadiusSquaredDistance; //存储查询点到近邻点对应平方距离
float radius =256.0f* rand () / (RAND_MAX +1.0f);
std::cout<<"Neighbors within radius search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with radius="<< radius <<std::endl;
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >0 )
{
//打印出找到的近邻坐标
for (size_t i=0; i<pointIdxRadiusSearch.size (); ++i)
std::cout<<" "<< cloud->points[ pointIdxRadiusSearch[i] ].x
<<" "<< cloud->points[pointIdxRadiusSearch[i] ].y
<<" "<< cloud->points[pointIdxRadiusSearch[i] ].z
<<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
}
return 0;
}
实验结果
- K近邻点
- 半径内近邻点
K nearest neighbor search at (411.158 880.737 460.739) with K=10
344.052 824.84 440.437 (squared distance: 8039.8)
340.176 824.447 451.771 (squared distance: 8287.43)
457.357 961.427 498.372 (squared distance: 10061.4)
386.369 949.562 377.057 (squared distance: 12354.2)
342.65 961.743 422.617 (squared distance: 12708.6)
432.821 936.234 560.646 (squared distance: 13530.6)
325.776 817.144 510.196 (squared distance: 13780.3)
427.329 924.143 569.831 (squared distance: 14046.6)
328.045 786.592 415.483 (squared distance: 17819.2)
493.282 774.574 513.514 (squared distance: 20800.1)
Neighbors within radius search at (411.158 880.737 460.739) with radius=182.626
344.052 824.84 440.437 (squared distance: 8039.8)
340.176 824.447 451.771 (squared distance: 8287.43)
457.357 961.427 498.372 (squared distance: 10061.4)
386.369 949.562 377.057 (squared distance: 12354.2)
342.65 961.743 422.617 (squared distance: 12708.6)
432.821 936.234 560.646 (squared distance: 13530.6)
325.776 817.144 510.196 (squared distance: 13780.3)
427.329 924.143 569.831 (squared distance: 14046.6)
328.045 786.592 415.483 (squared distance: 17819.2)
493.282 774.574 513.514 (squared distance: 20800.1)
305.511 827.495 546.885 (squared distance: 21417.2)
446.927 857.028 607.057 (squared distance: 23250.6)
455.42 1004.78 566.654 (squared distance: 28564.7)
560.851 792.209 449.129 (squared distance: 30379.9)
312.008 909.221 604.652 (squared distance: 31353.2)
533.265 1011.98 465.879 (squared distance: 32160.2)
566.013 802.704 412.312 (squared distance: 32414.4)
二、octree(八叉树)实验
代码
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char**argv)
{
/* 创建点云数据 */
srand ((unsigned int) time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width =1000;
cloud->height =1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i=0; i< cloud->points.size (); ++i)
{
cloud->points[i].x =1024.0f* rand () / (RAND_MAX +1.0f);
cloud->points[i].y =1024.0f* rand () / (RAND_MAX +1.0f);
cloud->points[i].z =1024.0f* rand () / (RAND_MAX +1.0f);
}
/* 创建一个 八叉树实例 */
float resolution =128.0f; //设置分辨率 最小体素的尺寸
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //octree初始化
octree.setInputCloud (cloud); //输入点云
octree.addPointsFromInputCloud (); //构建八叉树
/* 创建查询点 */
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);
/* 1、体素内近邻搜索 */
std::vector<int> pointIdxVec; //存储体素近邻搜索的结果向量 本人小白暂时认为是存在数组中
if (octree.voxelSearch (searchPoint, pointIdxVec)) //执行搜索,返回结果到pointIdxVec
{
std::cout<<"Neighbors within voxel search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z<<")"
<<std::endl;
for (size_t i=0; i<pointIdxVec.size (); ++i)
std::cout<<" "<< cloud->points[pointIdxVec[i]].x
<<" "<< cloud->points[pointIdxVec[i]].y
<<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
}
/* 2、K近邻搜索 搜索查询点周围的K个点*/
int K =10;
std::vector<int> pointIdxNKNSearch; //存储k近邻搜索点的索引
std::vector<float> pointNKNSquaredDistance; //存储查询点和搜索点之间的平方距离
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0)
{
//打印搜索结果
for (size_t i=0; i<pointIdxNKNSearch.size (); ++i)
std::cout<<" "<< cloud->points[pointIdxNKNSearch[i] ].x
<<" "<< cloud->points[pointIdxNKNSearch[i] ].y
<<" "<< cloud->points[pointIdxNKNSearch[i] ].z
<<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
}
/* 3、半径内近邻搜索 */
std::vector<int> pointIdxRadiusSearch; //存储k近邻搜索点的索引
std::vector<float> pointRadiusSquaredDistance; //存储查询点和搜索点之间的平方距离
float radius =256.0f* rand () / (RAND_MAX +1.0f); //随机一个半径
std::cout<<"Neighbors within radius search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with radius="<< radius <<std::endl;
if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >0)
{
//打印搜索结果
for (size_t i=0; i<pointIdxRadiusSearch.size (); ++i)
std::cout<<" "<< cloud->points[pointIdxRadiusSearch[i] ].x
<<" "<< cloud->points[pointIdxRadiusSearch[i] ].y
<<" "<< cloud->points[pointIdxRadiusSearch[i] ].z
<<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
}
return 0;
}
实验结果
- 体素内近邻点
- K近邻点
- 半径内近邻点
Neighbors within voxel search at (967.804 532.324 52.2935)
937.831 531.68 49.3266
981.128 498.083 80.6423
913.34 493.828 87.5745
K nearest neighbor search at (967.804 532.324 52.2935) with K=10
937.831 531.68 49.3266 (squared distance: 907.584)
981.128 498.083 80.6423 (squared distance: 2153.6)
1019.96 513.102 33.9256 (squared distance: 3426.77)
913.34 493.828 87.5745 (squared distance: 5692.96)
911.987 617.928 90.9567 (squared distance: 11938.6)
945.028 430.944 91.9093 (squared distance: 12366)
853.015 558.682 65.3296 (squared distance: 14041.2)
931.551 437.429 156.498 (squared distance: 21177.7)
836.204 453.168 79.5034 (squared distance: 24324.5)
849.1 616.626 125.015 (squared distance: 26485.9)
Neighbors within radius search at (967.804 532.324 52.2935) with radius=81.4588
937.831 531.68 49.3266 (squared distance: 907.584)
981.128 498.083 80.6423 (squared distance: 2153.6)
913.34 493.828 87.5745 (squared distance: 5692.96)
1019.96 513.102 33.9256 (squared distance: 3426.77)
三、octree空间变化检测实验
代码
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char**argv)
{
srand ((unsigned int) time (NULL));
/* 初始化空间变化检测对象 该类继承了Octree2BuffBase类 */
float resolution =32.0f; // 八叉树分辨率 即体素的大小
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
/* 在双缓冲内存中添加参考点云cloudA */
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ>);
cloudA->width =128;
cloudA->height =1;
cloudA->points.resize (cloudA->width *cloudA->height);
for (size_t i=0; i<cloudA->points.size (); ++i)
{
cloudA->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
cloudA->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
cloudA->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
}
octree.setInputCloud (cloudA); //添加点云到八叉树
octree.addPointsFromInputCloud (); //建立八叉树
octree.switchBuffers (); // 交换八叉树缓存,但是cloudA对应的八叉树仍在内存中
/* 在双缓冲内存中添加点云cloudB 和cloudA共享八叉树对象 */
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
cloudB->width =128;
cloudB->height =1;
cloudB->points.resize (cloudB->width *cloudB->height);
for (size_t i=0; i<cloudB->points.size (); ++i)
{
cloudB->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
cloudB->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
cloudB->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
}
octree.setInputCloud (cloudB); //添加点云到八叉树
octree.addPointsFromInputCloud (); //建立八叉树
/* 检测cloudB相对于cloudA多了多少 而不能检测少了多少 */
std::vector<int> newPointIdxVector; //存储新加点索引的对象
octree.getPointIndicesFromNewVoxels (newPointIdxVector); //获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
//打印输出点
std::cout<<"Output from getPointIndicesFromNewVoxels:"<<std::endl;
for (size_t i=0; i<newPointIdxVector.size (); ++i)
std::cout<<i<<"# Index:"<<newPointIdxVector[i]
<<" Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "
<<cloudB->points[newPointIdxVector[i]].y <<" "
<<cloudB->points[newPointIdxVector[i]].z <<std::endl;
}
实验结果
检测cloudB相对于cloudA多了多少 而不能检测少了多少。
Output from getPointIndicesFromNewVoxels:
0# Index:36 Point:6.37422 56.3796 0.909605
1# Index:52 Point:8.7016 53.0328 9.36858
2# Index:5 Point:1.58947 50.6516 58.0161
3# Index:34 Point:5.84086 50.7628 57.8813