AverageFilter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <math.h>
00011 #include <limits>
00012 #include "AverageFilter.h"
00013 #include "hrpsys/idl/pointcloud.hh"
00014 
00015 // Module specification
00016 // <rtc-template block="module_spec">
00017 static const char* spec[] =
00018   {
00019     "implementation_id", "AverageFilter",
00020     "type_name",         "AverageFilter",
00021     "description",       "Average Filter",
00022     "version",           HRPSYS_PACKAGE_VERSION,
00023     "vendor",            "AIST",
00024     "category",          "example",
00025     "activity_type",     "DataFlowComponent",
00026     "max_instance",      "10",
00027     "language",          "C++",
00028     "lang_type",         "compile",
00029     // Configuration variables
00030     "conf.default.resolution", "0.01",
00031     "conf.default.windowSize", "4",
00032     "conf.default.dilation", "0",
00033 
00034     ""
00035   };
00036 // </rtc-template>
00037 
00038 AverageFilter::AverageFilter(RTC::Manager* manager)
00039   : RTC::DataFlowComponentBase(manager),
00040     // <rtc-template block="initializer">
00041     m_originalIn("original", m_original),
00042     m_filteredOut("filtered", m_filtered),
00043     // </rtc-template>
00044     dummy(0)
00045 {
00046 }
00047 
00048 AverageFilter::~AverageFilter()
00049 {
00050 }
00051 
00052 
00053 
00054 RTC::ReturnCode_t AverageFilter::onInitialize()
00055 {
00056   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00057   // <rtc-template block="bind_config">
00058   // Bind variables and configuration variable
00059   bindParameter("resolution", m_resolution, "0.01");
00060   bindParameter("windowSize", m_windowSize, "4");
00061   bindParameter("dilation", m_dilation, "0");
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 AverageFilter::onFinalize()
00109 {
00110   return RTC::RTC_OK;
00111 }
00112 */
00113 
00114 /*
00115 RTC::ReturnCode_t AverageFilter::onStartup(RTC::UniqueId ec_id)
00116 {
00117   return RTC::RTC_OK;
00118 }
00119 */
00120 
00121 /*
00122 RTC::ReturnCode_t AverageFilter::onShutdown(RTC::UniqueId ec_id)
00123 {
00124   return RTC::RTC_OK;
00125 }
00126 */
00127 
00128 RTC::ReturnCode_t AverageFilter::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 AverageFilter::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 AverageFilter::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     if (!m_original.data.length()) return RTC::RTC_OK;
00148 
00149     // compute bbox
00150     float xmin, xmax, ymin, ymax;
00151     float *src = (float *)m_original.data.get_buffer();
00152     unsigned int npoint = m_original.data.length()/m_original.point_step;
00153     for (unsigned int i=0; i<npoint; i++){
00154       if (i==0){
00155         xmin = xmax = src[0];
00156         ymin = ymax = src[1];
00157       }else{
00158         if (xmin > src[0]) xmin = src[0];
00159         if (xmax < src[0]) xmax = src[0];
00160         if (ymin > src[1]) ymin = src[1];
00161         if (ymax < src[1]) ymax = src[1];
00162       }
00163       src += 4;
00164     }
00165     //std::cout << "xmin=" << xmin << ", xmax=" << xmax << ", ymin=" << ymin << ", ymax=" << ymax << std::endl;
00166     
00167     float xstart = (floor(xmin/m_resolution)-m_windowSize)*m_resolution;
00168     float ystart = (floor(ymin/m_resolution)-m_windowSize)*m_resolution;
00169     int nx = (xmax - xstart)/m_resolution+m_windowSize*2;
00170     int ny = (ymax - ystart)/m_resolution+m_windowSize*2;
00171     std::vector<float> cell(nx*ny, std::numeric_limits<float>::quiet_NaN());
00172     //std::cout << "xstart=" << xstart << ", ystart=" << ystart << std::endl;
00173     //std::cout << "nx=" << nx << ", ny=" << ny << std::endl;
00174     
00175     src = (float *)m_original.data.get_buffer();
00176     if (!m_dilation){
00177         for (unsigned int i=0; i<npoint; i++){
00178             int ix = round((src[0] - xstart)/m_resolution);
00179             int iy = round((src[1] - ystart)/m_resolution);
00180             int rank = ix+nx*iy;
00181             double z = cell[rank], z_new = src[2];
00182             if (isnan(z) || !isnan(z) && z_new > z){
00183                 cell[rank] = z_new;
00184             }
00185             src += 4;
00186         }
00187     }else{
00188         for (unsigned int i=0; i<npoint; i++){
00189             int ix = floor((src[0] - xstart)/m_resolution);
00190             int iy = floor((src[1] - ystart)/m_resolution);
00191             for (int j=0; j<2; j++){
00192                 for (int k=0; k<2; k++){
00193                     int rank = ix+j + nx*(iy+k);
00194                     double z = cell[rank], z_new = src[2];
00195                     if (isnan(z) || !isnan(z) && z_new > z){
00196                         cell[rank] = z_new;
00197                     }
00198                 }
00199             }
00200             src += 4;
00201         }
00202     }
00203 
00204     
00205     m_filtered.data.length(nx*ny*m_filtered.point_step); // shrinked later
00206     float *dst = (float *)m_filtered.data.get_buffer();
00207     npoint=0;
00208     int whalf = m_windowSize/2;
00209     for (int x=whalf; x<nx-whalf; x++){
00210       for (int y=whalf; y<ny-whalf; y++){
00211         int cnt=0;
00212         float zsum=0;
00213         //if (isnan(cell[x + nx*y])) continue;
00214         for (int dx=-whalf; dx<=whalf; dx++){
00215           for (int dy=-whalf; dy<whalf; dy++){
00216             int index = x + dx + nx*(y + dy);
00217             if (!isnan(cell[index])){
00218               zsum += cell[index];
00219               cnt++;
00220             }
00221           }
00222         }
00223         if (cnt){
00224           dst[0] = xstart + m_resolution*x;
00225           dst[1] = ystart + m_resolution*y;
00226           dst[2] = zsum/cnt;
00227           dst += 4;
00228           npoint++;
00229         }
00230       }
00231     }
00232     m_filtered.width = npoint;
00233     m_filtered.row_step = m_filtered.point_step*m_filtered.width;
00234     m_filtered.data.length(npoint*m_filtered.point_step);
00235 
00236     m_filteredOut.write();
00237   }
00238 
00239   return RTC::RTC_OK;
00240 }
00241 
00242 /*
00243 RTC::ReturnCode_t AverageFilter::onAborting(RTC::UniqueId ec_id)
00244 {
00245   return RTC::RTC_OK;
00246 }
00247 */
00248 
00249 /*
00250 RTC::ReturnCode_t AverageFilter::onError(RTC::UniqueId ec_id)
00251 {
00252   return RTC::RTC_OK;
00253 }
00254 */
00255 
00256 /*
00257 RTC::ReturnCode_t AverageFilter::onReset(RTC::UniqueId ec_id)
00258 {
00259   return RTC::RTC_OK;
00260 }
00261 */
00262 
00263 /*
00264 RTC::ReturnCode_t AverageFilter::onStateUpdate(RTC::UniqueId ec_id)
00265 {
00266   return RTC::RTC_OK;
00267 }
00268 */
00269 
00270 /*
00271 RTC::ReturnCode_t AverageFilter::onRateChanged(RTC::UniqueId ec_id)
00272 {
00273   return RTC::RTC_OK;
00274 }
00275 */
00276 
00277 
00278 
00279 extern "C"
00280 {
00281 
00282   void AverageFilterInit(RTC::Manager* manager)
00283   {
00284     RTC::Properties profile(spec);
00285     manager->registerFactory(profile,
00286                              RTC::Create<AverageFilter>,
00287                              RTC::Delete<AverageFilter>);
00288   }
00289 
00290 };
00291 
00292 


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