다중 실린더(Cylinder) 형상 추출

앞의 글에서는 다중 평면을 추출해 보았다. 이 글에서는 다중 실린더를 추출해 보도록 한다.

1. 개요
다중 실린더는 평면보다 처리할 것들이 더 많다. 실린더를 추출하기 전에 평면을 추출해, 제거해야 하며, 적절한 실린더 파라메터를 설정해 주어야 한다.

2. 코딩
다음과 같이 코딩해 본다. 추출할 반경은 0.05 미터에서 0.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/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.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);

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

    // remove plane
  for(int i = 0; i < 4; i++)
  {
      if(cloud.size() < 10000)
             break;
      seg.setInputCloud (cloud.makeShared ());
     
      pcl::ModelCoefficients coefficients;
      // pcl::PointIndices inliers;
      pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 
      seg.segment (*inliers, coefficients);
     
      // Create the filtering object
      pcl::ExtractIndices<pcl::PointXYZ> extract;
   
      // Extract the inliers
      ROS_INFO("extract the inliers"); 
      pcl::PointCloud<pcl::PointXYZ> in_cloud;
      extract.setInputCloud (cloud.makeShared ());
      extract.setIndices (inliers);
      extract.setNegative (false);
      extract.filter (in_cloud);
      extract.setNegative (true);
      extract.filter (cloud);
  }


  // Algorithm
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg2;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  // Datasets
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices);

  pcl::PointCloud<pcl::PointXYZI> TotalCloud;
  sensor_msgs::PointCloud2 output;
  for(int i = 0; i < 1; i++)
  {
        // Estimate point normals
        ne.setSearchMethod (tree);
        ne.setInputCloud (cloud.makeShared ());
        ne.setKSearch (50);
        ne.compute (*cloud_normals);

        // Create the segmentation object for cylinder segmentation and set all the parameters
        seg2.setOptimizeCoefficients (true);
        seg2.setModelType (pcl::SACMODEL_CYLINDER);
        seg2.setMethodType (pcl::SAC_RANSAC);
        seg2.setNormalDistanceWeight (0.1);
        seg2.setMaxIterations (1000);
        seg2.setDistanceThreshold (0.05);
        seg2.setRadiusLimits (0.05, 0.2);
        seg2.setInputCloud (cloud.makeShared ());
        seg2.setInputNormals (cloud_normals);

        // Obtain the cylinder inliers and coefficients
        seg2.segment (*inliers_cylinder, *coefficients_cylinder);
        std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

        // Write the cylinder inliers to disk
        pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
        pcl::ExtractIndices<PointT> extract;       
        extract.setInputCloud (cloud.makeShared ());
        extract.setIndices (inliers_cylinder);
        extract.setNegative (false);
        extract.filter (*cloud_cylinder);
       
      pcl::PointCloud<pcl::PointXYZ>::iterator index = cloud_cylinder->begin();
      for(; index != cloud_cylinder->end(); index++)
      {
         pcl::PointXYZ pt = *index;
         pcl::PointXYZI pt2;
         pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
         pt2.intensity = (float)(i + 1);

         TotalCloud.push_back(pt2);
      }
     
      ROS_INFO("%d. cylinder point cloud = %d", i, (int)TotalCloud.size()); 
  }
  
    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(TotalCloud, cloud_p); 
  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. 실행 결과
실행 결과는 다음과 같다. PCL에서 7개의 파라메터가 추출되는 데, 앞의 3개 값은 실린더 축의 첫번째 좌표 P1, 다음 3개 값은 두번째 좌표 P2, 마지막 값은 반경이다. 0.18 반경을 가진 실린더가 추출되었음을 알 수 있다.

ROS rviz로 확인해 보면, 실린더와 유사한 형상이 추출되었음을 확인할 수 있다. 

4. 실험
이제 몇가지 실험을 해 본다. 실험을 위해 반경 0.05 m 인 파이프를 준비한다.

실행해 테스트해 보면, 다음과 같이 0.04 m 반경을 가진 파이프를 인식한 것을 확인할 수 있다. 인식 오차는 1 cm 정도이다.

rviz에서 확인해 보자. 우선 파이프를 추출하기 전 포인트 클라우드이다.

파이프를 인식해 보면, 앞의 포인트 클라우드에서 파이프 부분을 제대로 추출하였음을 확인할 수 있다. 다만, 일부 평면이 추출되었는 데, 이는 실린더 추출 시 설정 파라메터에서, 속도 문제로 반복횟수를 1000으로 낮추는 바람에, 허용오차가 제대로 고려되지 않은 문제로 보인다. 반경의 오차도 같은 이유로 높아진 원인이 있다. 물론, 불완전한 포인트 클라우드도 반경 오차에 영향을 미치고 있다. 

이제   for(int i = 0; i < 1; i++)  부분을 두번 실행되도록 2로 수정한다. 그리고 빌드해 실행해 본다. 그럼 포인트 클라우드에서 설정된 파라메터 조건에 맞는 추출된 2개의 실런더 형상을 확인할 수 있을 것이다 .

5. 결론
앞의 시험 결과, 처리 성능만 개선한다면, 파이프 파라메터와 피팅(fitting)율을 높일 수 있기 때문에, 파이프 추출 시 정밀도 등을 높일 수 있다. 그럼에도 불구하고, 평면에 비해, 실린더는 형상 인식에 더 많은 계산 시간이 필요하여, 실시간성이 요구되는 응용에는 적합하지 않을 수 있다.