유클리드 거리 기반 3차원 포인트 클라우드 세그먼테이션

이 글은 ROS PCL의 처리 속도를 고려해, 유클리드 거리 기반 3차원 포인트 클라우드 세그먼테이션을 처리해 보도록 한다.

1. 개요
유클리스 거리 기반 클러스터링 방법은 주어진 포인트 클라우드를 거리에 따라 그룹핑하는 효과적인 방법이다. 점 간 거리만 계산하고, 탐색하면 되므로, 처리속도가 다른 클러스터링 알고리즘에 비해 매우 빠르다. 다만, 세밀한 클러스터링을 할 수 없는 단점이 있다.

• euclid distance clustering

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배 이상 빠르게 처리된다. 다만, 세밀한 단위로 클러스터링되지 않은 한계가 있다. 이는 앞의 글에서 설명한 다른 기법과 적절히 혼용하면 해결할 수 있다.