AverageFilter.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <math.h>
11 #include <limits>
12 #include "AverageFilter.h"
13 #include "hrpsys/idl/pointcloud.hh"
14 
15 // Module specification
16 // <rtc-template block="module_spec">
17 static const char* spec[] =
18  {
19  "implementation_id", "AverageFilter",
20  "type_name", "AverageFilter",
21  "description", "Average Filter",
22  "version", HRPSYS_PACKAGE_VERSION,
23  "vendor", "AIST",
24  "category", "example",
25  "activity_type", "DataFlowComponent",
26  "max_instance", "10",
27  "language", "C++",
28  "lang_type", "compile",
29  // Configuration variables
30  "conf.default.resolution", "0.01",
31  "conf.default.windowSize", "4",
32  "conf.default.dilation", "0",
33 
34  ""
35  };
36 // </rtc-template>
37 
39  : RTC::DataFlowComponentBase(manager),
40  // <rtc-template block="initializer">
41  m_originalIn("original", m_original),
42  m_filteredOut("filtered", m_filtered),
43  // </rtc-template>
44  dummy(0)
45 {
46 }
47 
49 {
50 }
51 
52 
53 
54 RTC::ReturnCode_t AverageFilter::onInitialize()
55 {
56  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
57  // <rtc-template block="bind_config">
58  // Bind variables and configuration variable
59  bindParameter("resolution", m_resolution, "0.01");
60  bindParameter("windowSize", m_windowSize, "4");
61  bindParameter("dilation", m_dilation, "0");
62 
63  // </rtc-template>
64 
65  // Registration: InPort/OutPort/Service
66  // <rtc-template block="registration">
67  // Set InPort buffers
68  addInPort("originalIn", m_originalIn);
69 
70  // Set OutPort buffer
71  addOutPort("filteredOut", m_filteredOut);
72 
73  // Set service provider to Ports
74 
75  // Set service consumers to Ports
76 
77  // Set CORBA Service Ports
78 
79  // </rtc-template>
80 
82 
83  m_filtered.height = 1;
84  m_filtered.type = "xyz";
85  m_filtered.fields.length(3);
86  m_filtered.fields[0].name = "x";
87  m_filtered.fields[0].offset = 0;
88  m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32;
89  m_filtered.fields[0].count = 4;
90  m_filtered.fields[1].name = "y";
91  m_filtered.fields[1].offset = 4;
92  m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32;
93  m_filtered.fields[1].count = 4;
94  m_filtered.fields[2].name = "z";
95  m_filtered.fields[2].offset = 8;
96  m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32;
97  m_filtered.fields[2].count = 4;
98  m_filtered.is_bigendian = false;
99  m_filtered.point_step = 16;
100  m_filtered.is_dense = true;
101 
102  return RTC::RTC_OK;
103 }
104 
105 
106 
107 /*
108 RTC::ReturnCode_t AverageFilter::onFinalize()
109 {
110  return RTC::RTC_OK;
111 }
112 */
113 
114 /*
115 RTC::ReturnCode_t AverageFilter::onStartup(RTC::UniqueId ec_id)
116 {
117  return RTC::RTC_OK;
118 }
119 */
120 
121 /*
122 RTC::ReturnCode_t AverageFilter::onShutdown(RTC::UniqueId ec_id)
123 {
124  return RTC::RTC_OK;
125 }
126 */
127 
129 {
130  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
131  return RTC::RTC_OK;
132 }
133 
135 {
136  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
137  return RTC::RTC_OK;
138 }
139 
140 RTC::ReturnCode_t AverageFilter::onExecute(RTC::UniqueId ec_id)
141 {
142  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
143 
144  if (m_originalIn.isNew()){
145  m_originalIn.read();
146 
147  if (!m_original.data.length()) return RTC::RTC_OK;
148 
149  // compute bbox
150  float xmin, xmax, ymin, ymax;
151  float *src = (float *)m_original.data.get_buffer();
152  unsigned int npoint = m_original.data.length()/m_original.point_step;
153  for (unsigned int i=0; i<npoint; i++){
154  if (i==0){
155  xmin = xmax = src[0];
156  ymin = ymax = src[1];
157  }else{
158  if (xmin > src[0]) xmin = src[0];
159  if (xmax < src[0]) xmax = src[0];
160  if (ymin > src[1]) ymin = src[1];
161  if (ymax < src[1]) ymax = src[1];
162  }
163  src += 4;
164  }
165  //std::cout << "xmin=" << xmin << ", xmax=" << xmax << ", ymin=" << ymin << ", ymax=" << ymax << std::endl;
166 
167  float xstart = (floor(xmin/m_resolution)-m_windowSize)*m_resolution;
168  float ystart = (floor(ymin/m_resolution)-m_windowSize)*m_resolution;
169  int nx = (xmax - xstart)/m_resolution+m_windowSize*2;
170  int ny = (ymax - ystart)/m_resolution+m_windowSize*2;
171  std::vector<float> cell(nx*ny, std::numeric_limits<float>::quiet_NaN());
172  //std::cout << "xstart=" << xstart << ", ystart=" << ystart << std::endl;
173  //std::cout << "nx=" << nx << ", ny=" << ny << std::endl;
174 
175  src = (float *)m_original.data.get_buffer();
176  if (!m_dilation){
177  for (unsigned int i=0; i<npoint; i++){
178  int ix = round((src[0] - xstart)/m_resolution);
179  int iy = round((src[1] - ystart)/m_resolution);
180  int rank = ix+nx*iy;
181  double z = cell[rank], z_new = src[2];
182  if (isnan(z) || !isnan(z) && z_new > z){
183  cell[rank] = z_new;
184  }
185  src += 4;
186  }
187  }else{
188  for (unsigned int i=0; i<npoint; i++){
189  int ix = floor((src[0] - xstart)/m_resolution);
190  int iy = floor((src[1] - ystart)/m_resolution);
191  for (int j=0; j<2; j++){
192  for (int k=0; k<2; k++){
193  int rank = ix+j + nx*(iy+k);
194  double z = cell[rank], z_new = src[2];
195  if (isnan(z) || !isnan(z) && z_new > z){
196  cell[rank] = z_new;
197  }
198  }
199  }
200  src += 4;
201  }
202  }
203 
204 
205  m_filtered.data.length(nx*ny*m_filtered.point_step); // shrinked later
206  float *dst = (float *)m_filtered.data.get_buffer();
207  npoint=0;
208  int whalf = m_windowSize/2;
209  for (int x=whalf; x<nx-whalf; x++){
210  for (int y=whalf; y<ny-whalf; y++){
211  int cnt=0;
212  float zsum=0;
213  //if (isnan(cell[x + nx*y])) continue;
214  for (int dx=-whalf; dx<=whalf; dx++){
215  for (int dy=-whalf; dy<whalf; dy++){
216  int index = x + dx + nx*(y + dy);
217  if (!isnan(cell[index])){
218  zsum += cell[index];
219  cnt++;
220  }
221  }
222  }
223  if (cnt){
224  dst[0] = xstart + m_resolution*x;
225  dst[1] = ystart + m_resolution*y;
226  dst[2] = zsum/cnt;
227  dst += 4;
228  npoint++;
229  }
230  }
231  }
232  m_filtered.width = npoint;
233  m_filtered.row_step = m_filtered.point_step*m_filtered.width;
234  m_filtered.data.length(npoint*m_filtered.point_step);
235 
237  }
238 
239  return RTC::RTC_OK;
240 }
241 
242 /*
243 RTC::ReturnCode_t AverageFilter::onAborting(RTC::UniqueId ec_id)
244 {
245  return RTC::RTC_OK;
246 }
247 */
248 
249 /*
250 RTC::ReturnCode_t AverageFilter::onError(RTC::UniqueId ec_id)
251 {
252  return RTC::RTC_OK;
253 }
254 */
255 
256 /*
257 RTC::ReturnCode_t AverageFilter::onReset(RTC::UniqueId ec_id)
258 {
259  return RTC::RTC_OK;
260 }
261 */
262 
263 /*
264 RTC::ReturnCode_t AverageFilter::onStateUpdate(RTC::UniqueId ec_id)
265 {
266  return RTC::RTC_OK;
267 }
268 */
269 
270 /*
271 RTC::ReturnCode_t AverageFilter::onRateChanged(RTC::UniqueId ec_id)
272 {
273  return RTC::RTC_OK;
274 }
275 */
276 
277 
278 
279 extern "C"
280 {
281 
283  {
285  manager->registerFactory(profile,
286  RTC::Create<AverageFilter>,
287  RTC::Delete<AverageFilter>);
288  }
289 
290 };
291 
292 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
static const char * spec[]
OutPort< PointCloudTypes::PointCloud > m_filteredOut
int rank
png_uint_32 i
coil::Properties & getProperties()
bool addOutPort(const char *name, OutPortBase &outport)
ExecutionContextHandle_t UniqueId
PointCloudTypes::PointCloud m_original
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
list index
AverageFilter(RTC::Manager *manager)
Constructor.
prop
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
double m_resolution
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
PointCloudTypes::PointCloud m_filtered
virtual bool isNew()
virtual ~AverageFilter()
Destructor.
virtual bool write(DataType &value)
Average Filter.
virtual RTC::ReturnCode_t onInitialize()
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void AverageFilterInit(RTC::Manager *manager)
InPort< PointCloudTypes::PointCloud > m_originalIn


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20