Search-Octree-PCL-Cpp (70%)
PCL-CPP ๊ธฐ๋ฐ Octree ํ์
์ฝ๋๋ [์ด๊ณณ]์์ ๋ค์ด๋ก๋ ๊ฐ๋ฅํฉ๋๋ค. ์ํํ์ผ์ [cloud_cluster_0.pcd]์ ์ฌ์ฉํ์์ต๋๋ค.
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <iostream>
#include <vector>
//Spatial Partitioning and Search Operations with Octrees
//http://pointclouds.org/documentation/tutorials/octree.php#octree-search
//Commnets : Hunjung, Lim (hunjung.lim@hotmail.com)
int main()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// *.PCD ํ์ผ ์ฝ๊ธฐ (https://raw.githubusercontent.com/adioshun/gitBook_Tutorial_PCL/master/Intermediate/sample/cloud_cluster_0.pcd)
pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud_cluster_0.pcd", *cloud);
// ์๊ฐ์ ํ์ธ์ ์ํด ์์ ํต์ผ (255,255,255)
for (size_t i = 0; i < cloud->points.size(); ++i){
cloud->points[i].r = 255;
cloud->points[i].g = 255;
cloud->points[i].b = 255;
}
//Octree ์ค๋ธ์ ํธ ์์ฑ
float resolution = 0.03f; //๋ณต์
ํฌ๊ธฐ ์ค์ (Set octree voxel resolution)
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(cloud); // ์
๋ ฅ
octree.addPointsFromInputCloud(); //Octree ์์ฑ (Build Octree)
//๊ธฐ์ค์ (searchPoint) ์ค์ ๋ฐฉ๋ฒ #1(x,y,z ์ขํ ์ง์ )
//pcl::PointXYZRGB searchPoint;
//searchPoint.x = 0.026256f;
//searchPoint.y = -1.464739f;
//searchPoint.z = 0.929567f;
//๊ธฐ์ค์ (searchPoint) ์ค์ ๋ฐฉ๋ฒ #2(3000๋ฒ์งธ ํฌ์ธํธ)
pcl::PointXYZRGB searchPoint = cloud->points[3000];
//๊ธฐ์ค์ ์ขํ ์ถ๋ ฅ
std::cout << "searchPoint :" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << std::endl;
//๊ธฐ์ค์ ๊ณผ ๋์ผํ ๋ณต์
๋ด ์กด์ฌ ํ๋ ํ๋ ํฌ์ธํธ ํ์(Voxel Neighbor Search)
std::vector<int> pointIdxVec; //๊ฒฐ๊ณผ๋ฌผ ํฌ์ธํธ์ Index ์ ์ฅ(Save the result vector of the voxel neighbor search)
if (octree.voxelSearch(searchPoint, pointIdxVec))
{
//์๊ฐ์ ํ์ธ์ ์ํ์ฌ ์์ ๋ณ๊ฒฝ (255,0,0)
for (size_t i = 0; i < pointIdxVec.size(); ++i){
cloud->points[pointIdxVec[i]].r = 255;
cloud->points[pointIdxVec[i]].g = 0;
cloud->points[pointIdxVec[i]].b = 0;
}
}
// ๊ธฐ์ค์ ์์ ๊ฐ๊น์ด ์์์ค K๋ฒ์งธ๊น์ง์ ํฌ์ธํธ ํ์ (K nearest neighbor search)
int K = 50; // ํ์ํ ํฌ์ธํธ ์ ์ค์
std::vector<int> pointIdxNKNSearch; //Save the index result of the K nearest neighbor
std::vector<float> pointNKNSquaredDistance; //Save the index result of the K nearest neighbor
if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
//์๊ฐ์ ํ์ธ์ ์ํ์ฌ ์์ ๋ณ๊ฒฝ (0,255,0)
for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){
cloud->points[pointIdxNKNSearch[i]].r = 0;
cloud->points[pointIdxNKNSearch[i]].g = 255;
cloud->points[pointIdxNKNSearch[i]].b = 0;
}
}
// ํ์๋ ์ ์ ์ ์ถ๋ ฅ
std::cout << "K = 50 nearest neighbors:" << pointIdxNKNSearch.size() << endl;
//๊ธฐ์ค์ ์์ ์ง์ ๋ ๋ฐ๊ฒฝ๋ด ํฌ์ธํธ ํ์ (Neighbor search within radius)
float radius = 0.02; //ํ์ํ ๋ฐ๊ฒฝ ์ค์ (Set the search radius)
std::vector<int> pointIdxRadiusSearch; //Save the index of each neighbor
std::vector<float> pointRadiusSquaredDistance; //Save the square of the Euclidean distance between each neighbor and the search point
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
//์๊ฐ์ ํ์ธ์ ์ํ์ฌ ์์ ๋ณ๊ฒฝ (0,0,255)
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i){
cloud->points[pointIdxRadiusSearch[i]].r = 0;
cloud->points[pointIdxRadiusSearch[i]].g = 0;
cloud->points[pointIdxRadiusSearch[i]].b = 255;
}
}
// ํ์๋ ์ ์ ์ ์ถ๋ ฅ
std::cout << "Radius 0.02 nearest neighbors: " << pointIdxRadiusSearch.size() << endl;
// ์์ฑ๋ ํฌ์ธํธํด๋ผ์ฐ๋ ์ ์ฅ
pcl::io::savePCDFile<pcl::PointXYZRGB>("Octree_AllinOne.pcd", *cloud);
}
๊ฒฐ๊ณผ
searchPoint :0.0346006 -1.46636 0.975463
K = 50 nearest neighbors:50
Radius 0.02 nearest neighbors: 141
์ฐธ๊ณ ์์น
๊ฒฐ๊ณผ
๊ฐ ๊ธฐ๋ฅ๋ณ ์ฝ๋ 3๊ฐ
1. Neighbors within voxel search
์ฝ๋๋ [์ด๊ณณ]์์ ๋ค์ด๋ก๋ ๊ฐ๋ฅํฉ๋๋ค. ์ํํ์ผ์ [cloud_cluster_0.pcd]์ ์ฌ์ฉํ์์ต๋๋ค.
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud_cluster_0.pcd", *cloud);
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree (resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
pcl::PointXYZRGB searchPoint;
searchPoint.x = 0.026256f;
searchPoint.y = -1.464739f;
searchPoint.z = 0.929567f;
// Neighbors within voxel search
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;
}
}
๊ฒฐ๊ณผ
...
-0.00606756 -1.46653 0.797328
-0.00904433 -1.46755 0.796737
-0.0120327 -1.46887 0.795969
...
2. K nearest neighbor search
์ฝ๋๋ [์ด๊ณณ]์์ ๋ค์ด๋ก๋ ๊ฐ๋ฅํฉ๋๋ค. ์ํํ์ผ์ [cloud_cluster_0.pcd]์ ์ฌ์ฉํ์์ต๋๋ค.
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud_cluster_0.pcd", *cloud);
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree (resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
pcl::PointXYZRGB searchPoint;
searchPoint.x = 0.026256f;
searchPoint.y = -1.464739f;
searchPoint.z = 0.929567f;
// K nearest neighbor search
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;
}
}
๊ฒฐ๊ณผ
K nearest neighbor search at (0.026256 -1.46474 0.929567) with K=10
0.0262559 -1.46474 0.929567 (squared distance: 3.69042e-13)
0.0234182 -1.46435 0.929759 (squared distance: 8.24415e-06)
0.0290953 -1.46517 0.929357 (squared distance: 8.28962e-06)
0.0262519 -1.46476 0.932708 (squared distance: 9.86657e-06)
0.0262599 -1.46472 0.926419 (squared distance: 9.90814e-06)
0.0290885 -1.46518 0.932502 (squared distance: 1.68363e-05)
0.0234196 -1.46433 0.926612 (squared distance: 1.69452e-05)
0.0234169 -1.46437 0.932899 (squared distance: 1.93018e-05)
0.029102 -1.46515 0.926206 (squared distance: 1.95655e-05)
0.0205821 -1.46402 0.929919 (squared distance: 3.28378e-05)
3. Neighbors within radius search
์ฝ๋๋ [์ด๊ณณ]์์ ๋ค์ด๋ก๋ ๊ฐ๋ฅํฉ๋๋ค. ์ํํ์ผ์ [cloud_cluster_0.pcd]์ ์ฌ์ฉํ์์ต๋๋ค.
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud_cluster_0.pcd", *cloud);
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree (resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
pcl::PointXYZRGB searchPoint;
searchPoint.x = 0.026256f;
searchPoint.y = -1.464739f;
searchPoint.z = 0.929567f;
// Neighbors within radius search
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;
}
}
๊ฒฐ๊ณผ
...
-0.00606756 -1.46653 0.797328
-0.00904433 -1.46755 0.796737
-0.0120327 -1.46887 0.795969
...
Last updated
Was this helpful?