Guide
#include <pcl/octree/octree_search.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/compression/octree_pointcloud_compression.h>
search
void octree_search()
{
srand(1234);
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.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);
std::vector<int> pointIdxVec;
if (octree.voxelSearch(searchPoint, 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;
}
int K = 10;
std::vector<int> pointIdxNKNSearch;
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;
}
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 (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;
}
}
change detection
void octree_change_detection()
{
srand(1234);
float resolution = 32.0f;
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);
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();
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();
std::vector<int> newPointIdxVector;
octree.getPointIndicesFromNewVoxels(newPointIdxVector);
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;
}
Reference
History