곡률 기반 형상 세그먼테이션

앞에서는 평면과 실린더를 기반으로 세그먼테이션을 처리해 보았다. 이 기법들은 몇몇 한계가 있는 데, 평면은 평면만 추출하는 문제, 실린더 추출은 실린더 내 세밀한 곡률의 변화를 구분하지 못하는 문제가 있다. 그러므로, 이 글에서는 곡률을 기반으로 형상을 분리하는 예를 통해, 이런 문제를 좀 더 개선해 보려 한다.

1. 개요
곡률 기반 세그먼테이션은 포인트의 법선 벡터 N을 이용해, 이웃점 N과의 곡률 C를 계산하고, 곡률의 차이를 이용해, 형상을 세그먼테이션 하는 기술이다. 각 점의 C를 계산하고, 유사도에 의해, 점을 그룹핑한다. 이는 유사 특성을 가진 점의 영역을 키워나가는 것이므로 region growing 알고리즘으로도 알려져 있다. 이 결과로 변곡점 부분을 분리해, 포인트 클라우드를 곡률 단위로 분해한다. 자세한 부분은 다음 내용을 참조한다.

• region growing segmentation

2. 코딩/빌드
다음과 같이 코딩하고 빌드한다.

#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/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.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);
 
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(cloud, cloud, indices);  
  std::cout << "cloud points = " << cloud.size () << std::endl;

  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
  pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setSearchMethod (tree);
  normal_estimator.setInputCloud (cloud.makeShared());
  normal_estimator.setKSearch (50);
  normal_estimator.compute (*normals);

  std::vector <pcl::PointIndices> clusters;
  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
  reg.setMinClusterSize (50);
  reg.setMaxClusterSize (1000000);
  reg.setSearchMethod (tree);
  reg.setNumberOfNeighbours (50);
  reg.setInputCloud (cloud.makeShared());
  reg.setInputNormals (normals);
  reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
  reg.setCurvatureThreshold (1.0);

  reg.extract (clusters);

  std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
  
    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(*colored_cloud, 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. 실험
rosrun으로 실행한다.

그리고 rviz를 통해 확인하면, 곡률에 따라 세그먼테이션이 되고 있는 모습을 볼 수 있다.

파라메터를 조정하면, 얼마나 민감하게 곡률에 따라 클러스터링할 것인지를 조정할 수 있다. 아래 파이프를 보면, 크게 6개 영역으로 구분된 것을 확인할 수 있다.

다음 실제 파이프와 비교해 보았을 때, 곡률 경계에 따라 어느정도 적절히 구분되었음을 확인할 수 있다.

4. 결과
곡률에 따라 포인트 클라우드를 분리하여, 클러스터링하는 region growing 알고리즘은 형상을 곡률을 기반으로 한 세밀한 단위로 분해할 수 있도록 한다. 다만, 법선, 곡률 계산 및 클러스터링 처리까지 포함되어 있어, 계산 속도가 늦다. 만약, 뭉쳐진 포인트 클라우드 단위로 클러스터링 한다면, 유클리드 거리 기반 클러스터링 알고리즘을 사용한 것이 낫다.