1. 개요
유클리스 거리 기반 클러스터링 방법은 주어진 포인트 클라우드를 거리에 따라 그룹핑하는 효과적인 방법이다. 점 간 거리만 계산하고, 탐색하면 되므로, 처리속도가 다른 클러스터링 알고리즘에 비해 매우 빠르다. 다만, 세밀한 클러스터링을 할 수 없는 단점이 있다.
2. 코딩
다음 프로그램은 계산 성능을 좀 더 높이기 위해, 복셀(voxel)을 이용해, LOD(level of detail)을 조정하여, 포인트 클라우드의 계산양을 줄인 후, 클러스터링한다. ROS에서 다음과 같이 코딩하고 빌드한다.
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
ros::Publisher pub;
typedef pcl::PointXYZ PointT;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, cloud);
// Data containers used
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud.makeShared());
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
std::cout << "Number of clusters is equal to " << cluster_indices.size () << std::endl;
pcl::PointCloud<pcl::PointXYZI> TotalCloud;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
pcl::PointXYZ pt = cloud_filtered->points[*pit];
pcl::PointXYZI pt2;
pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
pt2.intensity = (float)(j + 1);
TotalCloud.push_back(pt2);
}
j++;
}
// Convert To ROS data type
pcl::PCLPointCloud2 cloud_p;
pcl::toPCLPointCloud2(TotalCloud, cloud_p);
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_p, output);
output.header.frame_id = "camera_depth_optical_frame";
pub.publish(output);
ROS_INFO("published it.");
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);
// Create a ROS publisher for the output model coefficients
// pub = nh.advertise<pcl_msgs::ModelCoefficients> ("pclplaneoutput", 1);
pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
// Spin
ros::spin ();
}
3. 실험
빌드 후 실행하면 다음과 같은 결과가 출력된다.
rviz에서 살펴보면, 다음과 같이 점 간 거리에 따라 적절히 그룹핑되었음을 확인할 수 있다.
실제 구분된 세그먼트까지 거리를 측정해 보자. XTion에서 파이프까지 거리는 대략 60cm이다 (줄자가 없어, 일단 자로 테스트한다).
rviz에서 axis를 추가한다. 축 길이는 비교하기 좋게 0.6 (60cm)로 한다.
그리고, measure명령으로 거리를 재어 보다. 아래와 같이 거의 비슷하게 거리값이 나오는 것을 확인할 수 있다.
각도도 대략 비교해 보았다. 큰 차이가 없어 보인다.
파이프 지름과 rviz상에 측정된 지름을 비교해 보았다. 파이프는 약 10cm지름을 가지므로, rviz측정 상에서 1.4 cm정도 오차가 있으며, 이는 파이프 반쪽면의 일부만 스캔된 점, LOD 처리로 포인트 밀도가 줄어든 점으로 인한 것이다.
사다리를 놓고, 스캔을 해 보았다. 스캔된 가시영역과 그림자문제로 인해, 여러 개로 사다리가 세그먼테이션되는 것을 확인할 수 있다.
4. 결과
이번에는 계산 성능을 고려해, 점 간 거리에 따른 클러스터링을 처리해 보았다. 성능은 다른 클러스터링 및 형상 추출 기법에 비해, 최소 20배 이상 빠르게 처리된다. 다만, 세밀한 단위로 클러스터링되지 않은 한계가 있다. 이는 앞의 글에서 설명한 다른 기법과 적절히 혼용하면 해결할 수 있다.
댓글 없음:
댓글 쓰기