Point Cloud Libraryを試す（その４：平面抽出）

今回はポイントクラウドから平面を抽出します。

その前に、前回使ったPointXYZRGBの色の部分がfloat rgbとなっていて気になったので調べてみました。

// pack r/g/b into rgb
 uint8_t r = 255, g = 0, b = 0;    // Example: Red color
 uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
 p.rgb = *reinterpret_cast<float*>(&rgb);

↑のような感じで使うみたいです。

悪名高きreinterpret_castか！と思ったら、

ver.1.1.0からはr,g,b変数も使えるみたいで、

point.r = 255;

point.g = 100;

point.b = 10;

みたいに指定できるようです。

PCLのAPIリファレンスはこちらです。
Point Cloud Library (PCL): PCL API Documentation

という小ネタを挟みまして、今回は平面検出です。

↓のようにPointCloudから平面っぽいところを切りだしてくれるはずです。

http://www.youtube.com/watch?v=ZTK7NR1Xx4c&feature=player_embedded

チュートリアルはこちら

http://pointclouds.org/documentation/tutorials/planar_segmentation.php#planar-segmentation

チュートリアルにあるコードは簡単なんですが、ビューワが付いていないので、ビューワを付けてみました。
以下をplanar_segmentation.cppとして保存してください。

view plainprint?

1. #include <iostream>  
2. #include <pcl/ModelCoefficients.h>  
3. #include <pcl/io/pcd_io.h>  
4. #include <pcl/point_types.h>  
5. #include <pcl/sample_consensus/method_types.h>  
6. #include <pcl/sample_consensus/model_types.h>  
7. #include <pcl/segmentation/sac_segmentation.h>  
8. #include <pcl/visualization/cloud_viewer.h>  
9.   
10. int  
11. main (int argc, char** argv)  
12. {  
13.   pcl::PointCloud<pcl::PointXYZRGB> cloud;  
14.   
15.   // Fill in the cloud data  
16.   cloud.width  = 15;  
17.   cloud.height = 10;  
18.   cloud.points.resize (cloud.width * cloud.height);  
19.   
20.   // Generate the data  
21.   size_t i;  
22.   for (i = 0; i < cloud.points.size ()/2; ++i)  
23.     {  
24.       // 平面におく  
25.       cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);  
26.       cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);  
27.       cloud.points[i].z = 0.1 * rand () / (RAND_MAX + 1.0);  
28.       cloud.points[i].r = 255;  
29.       cloud.points[i].g = 255;  
30.       cloud.points[i].b = 255;  
31.     }  
32.   
33.   for (; i < cloud.points.size (); ++i)  
34.     {  
35.       // ちらばらせる  
36.       cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);  
37.       cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);  
38.       cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);  
39.       cloud.points[i].r = 255;  
40.       cloud.points[i].g = 255;  
41.       cloud.points[i].b = 255;  
42.     }  
43.   
44.   // Set a few outliers  
45.   cloud.points[0].z = 2.0;  
46.   cloud.points[3].z = -2.0;  
47.   cloud.points[6].z = 4.0;  
48.   
49.   std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl;  
50.   for (size_t i = 0; i < cloud.points.size (); ++i)  
51.     std::cerr << "    " << cloud.points[i].x << " "  
52.      << cloud.points[i].y << " "  
53.      << cloud.points[i].z << std::endl;  
54.   
55.   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
56.   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
57.   // Create the segmentation object  
58.   pcl::SACSegmentation<pcl::PointXYZRGB> seg;  
59.   // Optional  
60.   seg.setOptimizeCoefficients (true);  
61.   // Mandatory  
62.   seg.setModelType (pcl::SACMODEL_PLANE);  
63.   seg.setMethodType (pcl::SAC_RANSAC);  
64.   seg.setDistanceThreshold (0.1);  
65.   
66.   seg.setInputCloud (cloud.makeShared ());  
67.   seg.segment (*inliers, *coefficients);  
68.   
69.   if (inliers->indices.size () == 0)  
70.     {  
71.       PCL_ERROR ("Could not estimate a planar model for the given dataset.");  
72.       return (-1);  
73.     }  
74.   
75.   std::cerr << "Model coefficients: " << coefficients->values[0] << " "  
76.    << coefficients->values[1] << " "  
77.    << coefficients->values[2] << " "  
78.    << coefficients->values[3] << std::endl;  
79.   
80.   std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;  
81.   for (size_t i = 0; i < inliers->indices.size (); ++i) {  
82.     std::cerr << inliers->indices[i] << "    " << cloud.points[inliers->indices[i]].x << " "  
83.      << cloud.points[inliers->indices[i]].y << " "  
84.      << cloud.points[inliers->indices[i]].z << std::endl;  
85.     cloud.points[inliers->indices[i]].r = 255;  
86.     cloud.points[inliers->indices[i]].g = 0;  
87.     cloud.points[inliers->indices[i]].b = 0;  
88.   }  
89.   pcl::visualization::CloudViewer viewer("Cloud Viewer");  
90.   
91.   viewer.showCloud(cloud.makeShared());  
92.   
93.   while (!viewer.wasStopped ())  
94.     {  
95.        
96.     }  
97.   
98.   return (0);  
99. }  

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>

int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZRGB> cloud;

  // Fill in the cloud data
  cloud.width  = 15;
  cloud.height = 10;
  cloud.points.resize (cloud.width * cloud.height);

  // Generate the data
  size_t i;
  for (i = 0; i < cloud.points.size ()/2; ++i)
    {
      // 平面におく
      cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].z = 0.1 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].r = 255;
      cloud.points[i].g = 255;
      cloud.points[i].b = 255;
    }

  for (; i < cloud.points.size (); ++i)
    {
      // ちらばらせる
      cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      cloud.points[i].r = 255;
      cloud.points[i].g = 255;
      cloud.points[i].b = 255;
    }

  // Set a few outliers
  cloud.points[0].z = 2.0;
  cloud.points[3].z = -2.0;
  cloud.points[6].z = 4.0;

  std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl;
  for (size_t i = 0; i < cloud.points.size (); ++i)
    std::cerr << "    " << cloud.points[i].x << " "
     << cloud.points[i].y << " "
     << cloud.points[i].z << std::endl;

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.1);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
    {
      PCL_ERROR ("Could not estimate a planar model for the given dataset.");
      return (-1);
    }

  std::cerr << "Model coefficients: " << coefficients->values[0] << " "
   << coefficients->values[1] << " "
   << coefficients->values[2] << " "
   << coefficients->values[3] << std::endl;

  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
  for (size_t i = 0; i < inliers->indices.size (); ++i) {
    std::cerr << inliers->indices[i] << "    " << cloud.points[inliers->indices[i]].x << " "
     << cloud.points[inliers->indices[i]].y << " "
     << cloud.points[inliers->indices[i]].z << std::endl;
    cloud.points[inliers->indices[i]].r = 255;
    cloud.points[inliers->indices[i]].g = 0;
    cloud.points[inliers->indices[i]].b = 0;
  }
  pcl::visualization::CloudViewer viewer("Cloud Viewer");

  viewer.showCloud(cloud.makeShared());

  while (!viewer.wasStopped ())
    {
     
    }

  return (0);
}

するとこんな感じで平面っぽいところにあるポイントが赤く表示されました。

CMakeLists.txtはいつもと同じです。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR) project(planar_segmentation) find_package(PCL 1.1 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) add_executable (planar_segmentation planar_segmentation.cpp) target_link_libraries (planar_segmentation ${PCL_LIBRARIES})

$ cmake .

$ make

でビルドして、

$ ./planar_segmentation

で実行してみます。

SACSegmentationというClassが今回の本質のようです。ここにSACMODEL_PLANE＝平面モデルを指定したり、

setDistanceThresholdでモデルとの誤差の閾値を指定しています。setMethodTypeで計算方法を選択（今回はSAC_RANSAC）しています。RANSACは確率的なモデル推定だったきがします。

coefficients->valuesに入っているのは推定されたモデル

ax+by+cz+d=0の平面の式になります。

これで平面上にあるポイントが検出できました。

さらにKinectでやってみました。前回のKinectのキャプチャサンプルに今回のを合体させてみました。

一番広い平面が真っ赤にそまります。これは楽しい！

一応ソースコードです。適当です。

view plainprint?

1. #include <iostream>  
2. #include <pcl/ModelCoefficients.h>  
3. #include <pcl/io/openni_grabber.h>  
4. #include <pcl/io/pcd_io.h>  
5. #include <pcl/point_types.h>  
6. #include <pcl/sample_consensus/method_types.h>  
7. #include <pcl/sample_consensus/model_types.h>  
8. #include <pcl/segmentation/sac_segmentation.h>  
9. #include <pcl/visualization/cloud_viewer.h>  
10.   
11. void segmentate(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double threshould) {  
12.   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);  
13.   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);  
14.   // Create the segmentation object  
15.   pcl::SACSegmentation<pcl::PointXYZRGB> seg;  
16.   // Optional  
17.   seg.setOptimizeCoefficients (true);  
18.   // Mandatory  
19.   seg.setModelType (pcl::SACMODEL_PLANE);  
20.   seg.setMethodType (pcl::SAC_RANSAC);  
21.   seg.setDistanceThreshold (threshould);  
22.   
23.   seg.setInputCloud (cloud.makeShared ());  
24.   seg.segment (*inliers, *coefficients);  
25.   
26.   for (size_t i = 0; i < inliers->indices.size (); ++i) {  
27.     cloud.points[inliers->indices[i]].r = 255;  
28.     cloud.points[inliers->indices[i]].g = 0;  
29.     cloud.points[inliers->indices[i]].b = 0;  
30.   }  
31. }  
32.   
33.  class SimpleOpenNIViewer  
34.  {  
35.    public:  
36.      SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}  
37.   
38.      void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)  
39.      {  
40.        if (!viewer.wasStopped()) {  
41.   pcl::PointCloud<pcl::PointXYZRGB> segmented_cloud(*cloud);  
42.   segmentate(segmented_cloud, 0.01);  
43.          viewer.showCloud (segmented_cloud.makeShared());  
44.        }  
45.      }  
46.   
47.      void run ()  
48.      {  
49.        pcl::Grabber* interface = new pcl::OpenNIGrabber();  
50.   
51.        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =  
52.          boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);  
53.   
54.        interface->registerCallback (f);  
55.        interface->start ();  
56.        while (!viewer.wasStopped())  
57.        {  
58.          sleep (1);  
59.        }  
60.   
61.        interface->stop ();  
62.      }  
63.   
64.      pcl::visualization::CloudViewer viewer;  
65.  };  
66.   
67.  int main ()  
68.  {  
69.    SimpleOpenNIViewer v;  
70.    v.run ();  
71.    return 0;  
72.  }  

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>

void segmentate(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double threshould) {
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (threshould);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (*inliers, *coefficients);

  for (size_t i = 0; i < inliers->indices.size (); ++i) {
    cloud.points[inliers->indices[i]].r = 255;
    cloud.points[inliers->indices[i]].g = 0;
    cloud.points[inliers->indices[i]].b = 0;
  }
}

 class SimpleOpenNIViewer
 {
   public:
     SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}

     void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
     {
       if (!viewer.wasStopped()) {
  pcl::PointCloud<pcl::PointXYZRGB> segmented_cloud(*cloud);
  segmentate(segmented_cloud, 0.01);
         viewer.showCloud (segmented_cloud.makeShared());
       }
     }

     void run ()
     {
       pcl::Grabber* interface = new pcl::OpenNIGrabber();

       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
         boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);

       interface->registerCallback (f);
       interface->start ();
       while (!viewer.wasStopped())
       {
         sleep (1);
       }

       interface->stop ();
     }

     pcl::visualization::CloudViewer viewer;
 };

 int main ()
 {
   SimpleOpenNIViewer v;
   v.run ();
   return 0;
 }