PCLを使って遊んでみる

やりたいこと

点群情報の処理をうまく扱えるようになりたい

教科書

• Documentation - Point Cloud Library (PCL)

Point Cloud Dataの作成

Documentation - Point Cloud Library (PCL)

実行手順（以下のスクリプトを同じ階層に準備した前提）

1. mkdir build

2. cd build

3. cmake ..

4. make

5. ./pcd_write_test

pcdの型

width, height, points.sizeの要素が必要で、あとはこのpointsにxyzのデータをつめる

  pcl::PointCloud<pcl::PointXYZ> cloud;

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

実装

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

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

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

  for (size_t i = 0; i < cloud.points.size (); ++i)
  {
    cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);
  std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << 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;

  return (0);
}

結果

Saved 5 data points to test_pcd.pcd.
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762

CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(MY_GRAND_PROJECT)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcd_write_test pcd_write.cpp)
target_link_libraries(pcd_write_test ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})

平面検出

Documentation - Point Cloud Library (PCL)

重要な部分

• seg.setDistanceThreshold (0.01);

  • このパラメータどれくらい分厚いものを平面と分類するかが変わる。

  • softkineticでは平面の点群が分厚くなる傾向にあるため0.03~0.04に設定すると良い

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // 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);

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

実装

#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>

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

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

  // Generate the data
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1.0;
  }

  // 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::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud);
  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;

  return (0);
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(planar_segmentation)

find_package(PCL 1.2 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})

ROS x PCLで平面検出

softkineticの点群データを受け取って平面検出を行う

├── CMakeLists.txt
├── package.xml
└── src
    └── planar_segmentation.cpp

実装

• seg.setDistanceThreshold (0.01) -> (0.04);に修正

#include <string>
#include <pcl/point_cloud.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>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

class PlaneSegmentation {
 public:
  PlaneSegmentation()
      : segmentation_cloud_(new pcl::PointCloud<pcl::PointXYZRGB>) {
    std::string topic_name = "/softkinetic_camera/depth/points";
    cloud_sub_ = nh_.subscribe(topic_name, 1, &PlaneSegmentation::SegmentationCb, this);

    if (!ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic_name, ros::Duration(10.0))) {
      ROS_ERROR("timeout");
      exit(EXIT_FAILURE);
    }

    plane_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/plane_segmentation", 1);
  }

  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr GetSegmentationCloud() const {
    return PlaneSegmentation::segmentation_cloud_;
  }

 private:
  ros::NodeHandle nh_;
  ros::Subscriber cloud_sub_;
  ros::Publisher plane_pub_;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmentation_cloud_;

  void SegmentationCb(const sensor_msgs::PointCloud2ConstPtr& msg) {
    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    pcl::fromROSMsg(*msg, cloud);

    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.04);
    seg.setInputCloud(cloud.makeShared());

    pcl::PointIndices inliers;
    pcl::ModelCoefficients coefficients;
    seg.segment(inliers, coefficients);

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

    sensor_msgs::PointCloud2 pub_cloud;
    pcl::toROSMsg(*segmentation_cloud_, pub_cloud);
    plane_pub_.publish(pub_cloud);
  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "plane_segmentation");
  PlaneSegmentation plane_segmentation;
  ros::Rate spin_rate(10);

  pcl::visualization::CloudViewer viewer("Viewer");

  while (ros::ok()) {
    ros::spinOnce();
    if (!viewer.wasStopped()) {
      viewer.showCloud(plane_segmentation.GetSegmentationCloud());
    }
    spin_rate.sleep();
  }

  return 0;
}

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
)

catkin_package(
  CATKIN_DEPENDS
  roscpp
  sensor_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(planar_segmentation src/planar_segmentation.cpp)

target_link_libraries(plane_detection
  ${catkin_LIBRARIES}

pacage.xml

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>pcl_conversions</build_depend>
  <build_depend>pcl_ros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>

  <run_depend>pcl_conversions</run_depend>
  <run_depend>pcl_ros</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>

結果

Tutorialと同様の結果がでていない気がする。何かおかしいかも。

帯みたいになってる。なんでだろう。。。

参考

• GitHub - daavoo/pyntcloud: pyntcloud is a Python library for working with 3D point clouds.

• GitHub - falcondai/py-ransac: python implemetation of RANSAC algorithm with a line/plane fitting example.

• [Python] Fitting plane/surface to a set of data points · GitHub

• RANSAC C++ examples – MRPT

• ja/pcl/Tutorials - ROS Wiki

• pcl_ros - ROS Wiki

• ROS勉強記録: Point Cloud Libraryを試す（その１：インストール＆コンパイルテスト編）

• wiki/pcl_sample at master · hcrlab/wiki · GitHub

• Point Cloud Library (PCL)の各モジュールの概要 - MyEnigma

• PCL - Point Cloud Library (PCL)

• Documentation - Point Cloud Library (PCL)

• ROS勉強記録: Point Cloud Libraryを試す（その１：インストール＆コンパイルテスト編）