PlaneRemover.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <pcl/io/pcd_io.h>
00011 #include <pcl/point_types.h>
00012 #include <pcl/segmentation/sac_segmentation.h>
00013 #include <pcl/filters/extract_indices.h>
00014 #include "PlaneRemover.h"
00015 #include "hrpsys/idl/pointcloud.hh"
00016 
00017 // Module specification
00018 // <rtc-template block="module_spec">
00019 static const char* spec[] =
00020   {
00021     "implementation_id", "PlaneRemover",
00022     "type_name",         "PlaneRemover",
00023     "description",       "Plane Remover",
00024     "version",           HRPSYS_PACKAGE_VERSION,
00025     "vendor",            "AIST",
00026     "category",          "example",
00027     "activity_type",     "DataFlowComponent",
00028     "max_instance",      "10",
00029     "language",          "C++",
00030     "lang_type",         "compile",
00031     // Configuration variables
00032     "conf.default.distanceThd", "0.02",
00033     "conf.default.pointNumThd", "500",
00034 
00035     ""
00036   };
00037 // </rtc-template>
00038 
00039 PlaneRemover::PlaneRemover(RTC::Manager* manager)
00040   : RTC::DataFlowComponentBase(manager),
00041     // <rtc-template block="initializer">
00042     m_originalIn("original", m_original),
00043     m_filteredOut("filtered", m_filtered),
00044     // </rtc-template>
00045     dummy(0)
00046 {
00047 }
00048 
00049 PlaneRemover::~PlaneRemover()
00050 {
00051 }
00052 
00053 
00054 
00055 RTC::ReturnCode_t PlaneRemover::onInitialize()
00056 {
00057   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00058   // <rtc-template block="bind_config">
00059   // Bind variables and configuration variable
00060   bindParameter("distanceThd", m_distThd, "0.02");
00061   bindParameter("pointNumThd", m_pointNumThd, "500");
00062   
00063   // </rtc-template>
00064 
00065   // Registration: InPort/OutPort/Service
00066   // <rtc-template block="registration">
00067   // Set InPort buffers
00068   addInPort("originalIn", m_originalIn);
00069 
00070   // Set OutPort buffer
00071   addOutPort("filteredOut", m_filteredOut);
00072   
00073   // Set service provider to Ports
00074   
00075   // Set service consumers to Ports
00076   
00077   // Set CORBA Service Ports
00078   
00079   // </rtc-template>
00080 
00081   RTC::Properties& prop = getProperties();
00082 
00083   m_filtered.height = 1;
00084   m_filtered.type = "xyz";
00085   m_filtered.fields.length(3);
00086   m_filtered.fields[0].name = "x";
00087   m_filtered.fields[0].offset = 0;
00088   m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32;
00089   m_filtered.fields[0].count = 4;
00090   m_filtered.fields[1].name = "y";
00091   m_filtered.fields[1].offset = 4;
00092   m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32;
00093   m_filtered.fields[1].count = 4;
00094   m_filtered.fields[2].name = "z";
00095   m_filtered.fields[2].offset = 8;
00096   m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32;
00097   m_filtered.fields[2].count = 4;
00098   m_filtered.is_bigendian = false;
00099   m_filtered.point_step = 16;
00100   m_filtered.is_dense = true;
00101 
00102   return RTC::RTC_OK;
00103 }
00104 
00105 
00106 
00107 /*
00108 RTC::ReturnCode_t PlaneRemover::onFinalize()
00109 {
00110   return RTC::RTC_OK;
00111 }
00112 */
00113 
00114 /*
00115 RTC::ReturnCode_t PlaneRemover::onStartup(RTC::UniqueId ec_id)
00116 {
00117   return RTC::RTC_OK;
00118 }
00119 */
00120 
00121 /*
00122 RTC::ReturnCode_t PlaneRemover::onShutdown(RTC::UniqueId ec_id)
00123 {
00124   return RTC::RTC_OK;
00125 }
00126 */
00127 
00128 RTC::ReturnCode_t PlaneRemover::onActivated(RTC::UniqueId ec_id)
00129 {
00130   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00131   return RTC::RTC_OK;
00132 }
00133 
00134 RTC::ReturnCode_t PlaneRemover::onDeactivated(RTC::UniqueId ec_id)
00135 {
00136   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00137   return RTC::RTC_OK;
00138 }
00139 
00140 RTC::ReturnCode_t PlaneRemover::onExecute(RTC::UniqueId ec_id)
00141 {
00142   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00143 
00144   if (m_originalIn.isNew()){
00145     m_originalIn.read();
00146 
00147     // CORBA -> PCL
00148     pcl::PointCloud<pcl::PointXYZ>::Ptr original (new pcl::PointCloud<pcl::PointXYZ>);
00149     original->points.resize(m_original.width*m_original.height);
00150     float *src = (float *)m_original.data.get_buffer();
00151     for (unsigned int i=0; i<original->points.size(); i++){
00152       original->points[i].x = src[0];
00153       original->points[i].y = src[1];
00154       original->points[i].z = src[2];
00155       src += 4;
00156     }
00157 
00158     // PROCESSING
00159 
00160     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00161     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00162     // Create the segmentation object
00163     pcl::SACSegmentation<pcl::PointXYZ> seg;
00164     // Optional
00165     seg.setOptimizeCoefficients (true);
00166     // Mandatory
00167     seg.setModelType (pcl::SACMODEL_PLANE);
00168     seg.setMethodType (pcl::SAC_RANSAC);
00169     seg.setDistanceThreshold (m_distThd);
00170   
00171     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = original;
00172     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
00173   
00174     pcl::ExtractIndices<pcl::PointXYZ> extract;
00175   
00176     while(1){
00177       seg.setInputCloud (cloud);
00178       seg.segment (*inliers, *coefficients);
00179     
00180       if (inliers->indices.size () < m_pointNumThd) break;
00181     
00182       extract.setInputCloud( cloud );
00183       extract.setIndices( inliers );
00184       extract.setNegative( true );
00185       extract.filter( *cloud_f );
00186       cloud = cloud_f;
00187     }
00188 
00189     //std::cout << "PLaneRemover: original = " << original->points.size() << ", filtered = " << cloud->points.size() << ", thd=" << m_distThd << std::endl;
00190 
00191     // PCL -> CORBA
00192     m_filtered.width = cloud->points.size();
00193     m_filtered.row_step = m_filtered.point_step*m_filtered.width;
00194     m_filtered.data.length(m_filtered.height*m_filtered.row_step);
00195     float *dst = (float *)m_filtered.data.get_buffer();
00196     for (unsigned int i=0; i<cloud->points.size(); i++){
00197       dst[0] = cloud->points[i].x;
00198       dst[1] = cloud->points[i].y;
00199       dst[2] = cloud->points[i].z;
00200       dst += 4;
00201     }
00202     m_filteredOut.write();
00203   }
00204 
00205   return RTC::RTC_OK;
00206 }
00207 
00208 /*
00209 RTC::ReturnCode_t PlaneRemover::onAborting(RTC::UniqueId ec_id)
00210 {
00211   return RTC::RTC_OK;
00212 }
00213 */
00214 
00215 /*
00216 RTC::ReturnCode_t PlaneRemover::onError(RTC::UniqueId ec_id)
00217 {
00218   return RTC::RTC_OK;
00219 }
00220 */
00221 
00222 /*
00223 RTC::ReturnCode_t PlaneRemover::onReset(RTC::UniqueId ec_id)
00224 {
00225   return RTC::RTC_OK;
00226 }
00227 */
00228 
00229 /*
00230 RTC::ReturnCode_t PlaneRemover::onStateUpdate(RTC::UniqueId ec_id)
00231 {
00232   return RTC::RTC_OK;
00233 }
00234 */
00235 
00236 /*
00237 RTC::ReturnCode_t PlaneRemover::onRateChanged(RTC::UniqueId ec_id)
00238 {
00239   return RTC::RTC_OK;
00240 }
00241 */
00242 
00243 
00244 
00245 extern "C"
00246 {
00247 
00248   void PlaneRemoverInit(RTC::Manager* manager)
00249   {
00250     RTC::Properties profile(spec);
00251     manager->registerFactory(profile,
00252                              RTC::Create<PlaneRemover>,
00253                              RTC::Delete<PlaneRemover>);
00254   }
00255 
00256 };
00257 
00258 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55