MLSFilter.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/surface/mls.h>
00013 #include "MLSFilter.h"
00014 #include "hrpsys/idl/pointcloud.hh"
00015 
00016 // Module specification
00017 // <rtc-template block="module_spec">
00018 static const char* spec[] =
00019   {
00020     "implementation_id", "MLSFilter",
00021     "type_name",         "MLSFilter",
00022     "description",       "Moving Least Squares Filter",
00023     "version",           HRPSYS_PACKAGE_VERSION,
00024     "vendor",            "AIST",
00025     "category",          "example",
00026     "activity_type",     "DataFlowComponent",
00027     "max_instance",      "10",
00028     "language",          "C++",
00029     "lang_type",         "compile",
00030     // Configuration variables
00031     "conf.default.radius", "0.03",
00032 
00033     ""
00034   };
00035 // </rtc-template>
00036 
00037 MLSFilter::MLSFilter(RTC::Manager* manager)
00038   : RTC::DataFlowComponentBase(manager),
00039     // <rtc-template block="initializer">
00040     m_originalIn("original", m_original),
00041     m_filteredOut("filtered", m_filtered),
00042     // </rtc-template>
00043     dummy(0)
00044 {
00045 }
00046 
00047 MLSFilter::~MLSFilter()
00048 {
00049 }
00050 
00051 
00052 
00053 RTC::ReturnCode_t MLSFilter::onInitialize()
00054 {
00055   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00056   // <rtc-template block="bind_config">
00057   // Bind variables and configuration variable
00058   bindParameter("radius", m_radius, "0.03");
00059   
00060   // </rtc-template>
00061 
00062   // Registration: InPort/OutPort/Service
00063   // <rtc-template block="registration">
00064   // Set InPort buffers
00065   addInPort("originalIn", m_originalIn);
00066 
00067   // Set OutPort buffer
00068   addOutPort("filteredOut", m_filteredOut);
00069   
00070   // Set service provider to Ports
00071   
00072   // Set service consumers to Ports
00073   
00074   // Set CORBA Service Ports
00075   
00076   // </rtc-template>
00077 
00078   RTC::Properties& prop = getProperties();
00079 
00080   m_filtered.height = 1;
00081   m_filtered.type = "xyz";
00082   m_filtered.fields.length(3);
00083   m_filtered.fields[0].name = "x";
00084   m_filtered.fields[0].offset = 0;
00085   m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32;
00086   m_filtered.fields[0].count = 4;
00087   m_filtered.fields[1].name = "y";
00088   m_filtered.fields[1].offset = 4;
00089   m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32;
00090   m_filtered.fields[1].count = 4;
00091   m_filtered.fields[2].name = "z";
00092   m_filtered.fields[2].offset = 8;
00093   m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32;
00094   m_filtered.fields[2].count = 4;
00095   m_filtered.is_bigendian = false;
00096   m_filtered.point_step = 16;
00097   m_filtered.is_dense = true;
00098 
00099   return RTC::RTC_OK;
00100 }
00101 
00102 
00103 
00104 /*
00105 RTC::ReturnCode_t MLSFilter::onFinalize()
00106 {
00107   return RTC::RTC_OK;
00108 }
00109 */
00110 
00111 /*
00112 RTC::ReturnCode_t MLSFilter::onStartup(RTC::UniqueId ec_id)
00113 {
00114   return RTC::RTC_OK;
00115 }
00116 */
00117 
00118 /*
00119 RTC::ReturnCode_t MLSFilter::onShutdown(RTC::UniqueId ec_id)
00120 {
00121   return RTC::RTC_OK;
00122 }
00123 */
00124 
00125 RTC::ReturnCode_t MLSFilter::onActivated(RTC::UniqueId ec_id)
00126 {
00127   std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00128   return RTC::RTC_OK;
00129 }
00130 
00131 RTC::ReturnCode_t MLSFilter::onDeactivated(RTC::UniqueId ec_id)
00132 {
00133   std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00134   return RTC::RTC_OK;
00135 }
00136 
00137 RTC::ReturnCode_t MLSFilter::onExecute(RTC::UniqueId ec_id)
00138 {
00139   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00140 
00141   if (m_originalIn.isNew()){
00142     m_originalIn.read();
00143 
00144     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00145     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
00146 
00147     // RTM -> PCL
00148     cloud->points.resize(m_original.width*m_original.height);
00149     float *src = (float *)m_original.data.get_buffer();
00150     for (unsigned int i=0; i<cloud->points.size(); i++){
00151       cloud->points[i].x = src[0];
00152       cloud->points[i].y = src[1];
00153       cloud->points[i].z = src[2];
00154       src += 4;
00155     }
00156     
00157     // PCL Processing 
00158     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
00159     pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> mls;
00160     mls.setInputCloud (cloud);
00161     mls.setPolynomialFit (true);
00162     mls.setSearchMethod (tree);
00163     mls.setSearchRadius (m_radius);
00164     mls.process (*cloud_filtered);
00165 
00166     // PCL -> RTM
00167     m_filtered.width = cloud_filtered->points.size();
00168     m_filtered.row_step = m_filtered.point_step*m_filtered.width;
00169     m_filtered.data.length(m_filtered.height*m_filtered.row_step);
00170     float *dst = (float *)m_filtered.data.get_buffer();
00171     for (unsigned int i=0; i<cloud_filtered->points.size(); i++){
00172       dst[0] = cloud_filtered->points[i].x;
00173       dst[1] = cloud_filtered->points[i].y;
00174       dst[2] = cloud_filtered->points[i].z;
00175       dst += 4;
00176     }
00177     m_filteredOut.write();
00178   }
00179 
00180   return RTC::RTC_OK;
00181 }
00182 
00183 /*
00184 RTC::ReturnCode_t MLSFilter::onAborting(RTC::UniqueId ec_id)
00185 {
00186   return RTC::RTC_OK;
00187 }
00188 */
00189 
00190 /*
00191 RTC::ReturnCode_t MLSFilter::onError(RTC::UniqueId ec_id)
00192 {
00193   return RTC::RTC_OK;
00194 }
00195 */
00196 
00197 /*
00198 RTC::ReturnCode_t MLSFilter::onReset(RTC::UniqueId ec_id)
00199 {
00200   return RTC::RTC_OK;
00201 }
00202 */
00203 
00204 /*
00205 RTC::ReturnCode_t MLSFilter::onStateUpdate(RTC::UniqueId ec_id)
00206 {
00207   return RTC::RTC_OK;
00208 }
00209 */
00210 
00211 /*
00212 RTC::ReturnCode_t MLSFilter::onRateChanged(RTC::UniqueId ec_id)
00213 {
00214   return RTC::RTC_OK;
00215 }
00216 */
00217 
00218 
00219 
00220 extern "C"
00221 {
00222 
00223   void MLSFilterInit(RTC::Manager* manager)
00224   {
00225     RTC::Properties profile(spec);
00226     manager->registerFactory(profile,
00227                              RTC::Create<MLSFilter>,
00228                              RTC::Delete<MLSFilter>);
00229   }
00230 
00231 };
00232 
00233 


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