3차원 포인트 클라우드 스캔 기반 객체 추적

이 글은 앞의 내용을 종합해, 3차원 포인트 클라우드를 입력받아, 스캔 기반으로 객체를 추적하는 방법을 개발해 본다.

1. 개요
객체를 추적하기 위해서는 포인트 클라우드를 적절히 세그먼테이션해야 하며, 각 세그먼트의 물성(중심점, AABB 정보 등)을 추출해야 한다. 각 세그먼트는 RANSAC등의 방법을 통해, 평면, 실린더 등 객체로 구분할 수 있다.

여기서는 단순히, 유클리디언 거리로 포인트 클라우드를 세그먼테이션하고, 세그먼트의 중심점 및 최대 영역을 획득해 본다.

2. 코딩
앞의 글과 같이 PCL 패키지를 생성해 보고, node의 소스를 다음과 같이 코딩한다.

#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>
// #include <pcl/features/moment_of_inertia_estimation.h>

ros::Publisher pub, pub_obj;

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::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
 
  pcl::PointCloud<pcl::PointXYZI> TotalCloud, ObjectCloud; 
  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
      pcl::PointCloud<pcl::PointXYZI>::Ptr segment(new pcl::PointCloud<pcl::PointXYZI> ());

        pcl::PointXYZI min_point_AABB, max_point_AABB, mass_center;
        // Eigen::Vector3f mass_center;
        min_point_AABB.x = min_point_AABB.y = min_point_AABB.z = 10000000000.0;
      max_point_AABB.x = max_point_AABB.y = max_point_AABB.z = -10000000000.0;

    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);
            segment->push_back(pt2);
           
            min_point_AABB.x = std::min(min_point_AABB.x, pt.x);
            min_point_AABB.y = std::min(min_point_AABB.y, pt.y);
            min_point_AABB.z = std::min(min_point_AABB.z, pt.z);
            min_point_AABB.intensity = (float)(j + 1);

            max_point_AABB.x = std::max(max_point_AABB.x, pt.x);
            max_point_AABB.y = std::max(max_point_AABB.y, pt.y);
            max_point_AABB.z = std::max(max_point_AABB.z, pt.z);
            max_point_AABB.intensity = (float)(j + 1);           
    }
         
        /* feature_extractor.setInputCloud (segment);
        feature_extractor.compute ();


        feature_extractor.getAABB (min_point_AABB, max_point_AABB); 
        feature_extractor.getMassCenter (mass_center);   */
       
        mass_center.x = (max_point_AABB.x - min_point_AABB.x) / 2.0 + min_point_AABB.x;
        mass_center.y = (max_point_AABB.y - min_point_AABB.y) / 2.0 + min_point_AABB.y;
        mass_center.z = (max_point_AABB.z - min_point_AABB.z) / 2.0 + min_point_AABB.z;
        mass_center.intensity = (float)(j + 1);               
                       
        ObjectCloud.push_back(min_point_AABB);
        ObjectCloud.push_back(max_point_AABB);
        ObjectCloud.push_back(mass_center);
   
    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);

    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p2;
  pcl::toPCLPointCloud2(ObjectCloud, cloud_p2);
  
  sensor_msgs::PointCloud2 output2; 
  pcl_conversions::fromPCL(cloud_p2, output2);
  output2.header.frame_id = "camera_depth_optical_frame";   
  pub_obj.publish(output2);   

 
  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 segments
  pub = nh.advertise<sensor_msgs::PointCloud2> ("pclsegments", 1);

  // Create a ROS publisher for the objects
  pub_obj = nh.advertise<sensor_msgs::PointCloud2> ("pclobjects", 1);
 
  // Spin
  ros::spin ();
}

3. 실험
실행하면, 다음과 같이 세그먼테이션되고, 각 세그먼트의 중심점과 최대 영역점이 8 pixel의 점으로 표시된다.

4. 결론
특정 객체를 찾은 후에, 객체의 물성을 획득하면, 그 객체를 추적하거나, 그립하는 것이 가능하다. 이와 엑추에이터를 연계하기 위해서는 PID제어(앞의 글 참고)가 고려되어야 한다.
또한, moment_of_inertia_estimation.h와 같은 최신 버전의 API가 Indigo PCL버전에서는 제공되지 않는 것은 아쉬움이 있다.