Component.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::ArmarXObjects::DepthFilter
17  * @author Christian R. G. Dreher <c.dreher@kit.edu>
18  * @date 2019
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 
25 
26 #include <SimoxUtility/algorithm/string/string_tools.h>
27 
28 
29 // STD/STL
30 #include <exception>
31 #include <string>
32 
33 
34 // IVT
35 #include <Image/ByteImage.h>
36 
37 // Ice
38 #include <IceUtil/Time.h>
39 using namespace IceUtil;
40 
41 // ArmarX
43 using namespace armarx;
44 using namespace visionx;
45 using namespace visionx::depthfilter;
46 
47 
48 namespace
49 {
50  DrawColor24Bit parse_color(std::string color)
51  {
52 
53  std::vector<unsigned char> rgb_vals;
54  std::vector<std::string> rgb = simox::alg::split(color, ",");
55 
56  for (const std::string& val : rgb)
57  {
58  const long long int val_check = std::stoll(val);
59 
60  if (0 <= val_check and val_check <= 255)
61  {
62  rgb_vals.push_back(static_cast<unsigned char>(val_check));
63  }
64  }
65 
66  if (rgb_vals.size() != 3)
67  {
68  throw std::invalid_argument("Format for color must be `R,G,B`, where each of R, G and B "
69  "is a value between 0 and 255");
70  }
71 
72  return DrawColor24Bit{rgb_vals[0], rgb_vals[1], rgb_vals[2]};
73  }
74 }
75 
76 
77 depthfilter::Component::~Component()
78 {
79  // pass
80 }
81 
82 
83 void
84 depthfilter::Component::onInitPointCloudAndImageProcessor()
85 {
86  // Input image provider.
87  m_ipc_in = getProperty<std::string>("ipc.ImageProviderIn");
88 
89  usingImageProvider(m_ipc_in);
90  usingPointCloudProvider(m_ipc_in);
91 
92  // Output image provider.
93  m_ipc_out = getProperty<std::string>("ipc.ImageProviderOut");
94 
95  // Threshold and mode.
96  {
97  const int val = getProperty<int>("conf.DepthThreshold");
98 
99  if (val < 0)
100  {
101  m_mode = replace_mode::undercut_threshold;
102  m_threshold = static_cast<unsigned int>(-val);
103  }
104  else
105  {
106  m_mode = replace_mode::exceed_threshold;
107  m_threshold = static_cast<unsigned int>(val);
108  }
109  }
110 
111  // Replacement colors.
112  m_color_depth = parse_color(getProperty<std::string>("conf.ColorDepth"));
113  m_color_invalid = parse_color(getProperty<std::string>("conf.ColorInvalid"));
114 }
115 
116 
117 void
118 depthfilter::Component::onConnectPointCloudAndImageProcessor()
119 {
120  const bool wait_for_proxy = true;
121  ImageProviderInfo provider_info = getImageProvider(m_ipc_in, wait_for_proxy);
122 
123  unsigned int num_images = 1;
124 
125  if (not getProperty<bool>("conf.RGBOnly"))
126  {
127  num_images = static_cast<unsigned int>(provider_info.numberImages);
128  }
129 
130  getPointCloudProvider(m_ipc_in, wait_for_proxy);
131 
132  //ARMARX_IMPORTANT << "before enableResultImages";
133  //enableResultImages(
134  // static_cast<int>(num_images),
135  // provider_info.imageFormat.dimension,
136  // provider_info.destinationImageType,
137  // m_ipc_out
138  //);
139  //ARMARX_IMPORTANT << "before enableResultPointClouds";
140  //enableResultPointClouds<pcl::PointXYZRGBA>(m_ipc_out);
141 
142  ARMARX_IMPORTANT << "before error";
143  enableResultImagesAndPointClouds<pcl::PointXYZRGBA>(m_ipc_out,
144  static_cast<int>(num_images),
145  provider_info.imageFormat.dimension,
146  provider_info.destinationImageType
147  );
148  ARMARX_IMPORTANT << "after error";
149 
150 
151  m_buffer = new CByteImage*[2];
152  m_buffer[0] = visionx::tools::createByteImage(provider_info);
153  m_buffer[1] = visionx::tools::createByteImage(provider_info);
154  m_buffer_single = new CByteImage*[1];
155  m_buffer_mask = visionx::tools::createByteImage(provider_info);
156 }
157 
158 
159 void
160 depthfilter::Component::onDisconnectPointCloudAndImageProcessor()
161 {
162  delete m_buffer[0];
163  delete m_buffer[1];
164  delete[] m_buffer;
165  delete[] m_buffer_single;
166 }
167 
168 
169 void
170 depthfilter::Component::onExitPointCloudAndImageProcessor()
171 {
172  // pass
173 }
174 
175 
176 std::string
177 depthfilter::Component::getDefaultName() const
178 {
179  return "DepthFilter";
180 }
181 
182 
183 void
184 depthfilter::Component::process()
185 {
186  {
187  const Time timeout = Time::seconds(1);
188  if (not waitForImages(m_ipc_in, timeout))
189  {
190  ARMARX_WARNING << "Timeout while waiting for images (>" << timeout << ")";
191  return;
192  }
193  if (!waitForPointClouds(1000))
194  {
195  ARMARX_WARNING << "Timeout while waiting for pointclouds (>" << timeout << ")";
196  return;
197  }
198  }
199 
200  // Get images.
201  MetaInfoSizeBasePtr info;
202  getImages(m_ipc_in, m_buffer, info);
203 
204  // Apply depth filter.
205  applyDepthFilter(*m_buffer[1], m_threshold, m_grow_radius, m_mode, m_color_depth, m_color_invalid,
206  *m_buffer[0], *m_buffer_mask);
207 
208  // Provide result images.
209  if (getProperty<bool>("conf.RGBOnly"))
210  {
211  m_buffer_single[0] = m_buffer[0];
212  provideResultImages(m_buffer_single, info);
213  }
214  else
215  {
216  provideResultImages(m_buffer, info);
217  }
218 
219  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
220  getPointClouds(currentPointCloud);
221  provideResultPointClouds(m_ipc_out, currentPointCloud);
222 
223 }
224 
225 
226 void
227 depthfilter::Component::applyDepthFilter(
228  const CByteImage& depth,
229  const unsigned int threshold,
230  const int grow_radius,
231  const replace_mode mode,
232  const armarx::DrawColor24Bit& color_depth,
233  const armarx::DrawColor24Bit& color_invalid,
234  CByteImage& rgb,
235  CByteImage& mask
236 )
237 {
238  const bool noise_resistant = false;
239  const unsigned int pixels_size = static_cast<unsigned int>(rgb.width * rgb.height) * 3;
240 
241  for (int y = 0; y < rgb.height; y++)
242  {
243  for (int x = 0; x < rgb.width; x++)
244  {
245  int pp = y * rgb.width * 3 + x * 3;
246  mask.pixels[pp] = 0;
247  }
248  }
249  int dd = 4;
250  int minval = dd * dd * 1 / 8;
251 
252  // Loop over each pixel.
253  for (int y = 0; y < rgb.height; y += dd)
254  {
255  for (int x = 0; x < rgb.width; x += dd)
256  {
257  int val_ctr = 0;
258 
259  for (int xx = x; xx < std::min(x + dd, rgb.width); xx++)
260  {
261  for (int yy = y; yy < std::min(y + dd, rgb.height); yy++)
262  {
263  int pixel_pos = y * rgb.width * 3 + x * 3;
264  const unsigned int z_value = static_cast<unsigned int>(visionx::tools::rgbToDepthValue(
265  depth.pixels[pixel_pos + 0],
266  depth.pixels[pixel_pos + 1],
267  depth.pixels[pixel_pos + 2],
268  noise_resistant
269  ));
270  if (z_value == 0)
271  {
272  val_ctr += 0;
273  }
274  else if ((mode == replace_mode::exceed_threshold and z_value > threshold)
275  or (mode == replace_mode::undercut_threshold and z_value < threshold))
276  {
277  val_ctr += 0;
278  }
279  else
280  {
281  val_ctr++;
282  }
283  }
284  }
285 
286 
287  if (val_ctr >= minval)
288  {
289  for (int xx = std::max(0, x - grow_radius); xx <= std::min(rgb.width - 1, x + grow_radius + dd); xx++)
290  {
291  for (int yy = std::max(0, y - grow_radius); yy <= std::min(rgb.height - 1, y + grow_radius + dd); yy++)
292  {
293  int pp = yy * rgb.width * 3 + xx * 3;
294  mask.pixels[pp] = 1;
295  }
296  }
297  }
298  }
299  }
300 
301 
302 
303 
304  // Loop over each pixel.
305  for (unsigned int pixel_pos = 0; pixel_pos < pixels_size; pixel_pos += 3)
306  {
307  const unsigned int z_value = static_cast<unsigned int>(visionx::tools::rgbToDepthValue(
308  depth.pixels[pixel_pos + 0],
309  depth.pixels[pixel_pos + 1],
310  depth.pixels[pixel_pos + 2],
311  noise_resistant
312  ));
313 
314  if ((mode == replace_mode::exceed_threshold and z_value > threshold)
315  or (mode == replace_mode::undercut_threshold and z_value < threshold))
316  {
317  rgb.pixels[pixel_pos + 0] = color_depth.r;
318  rgb.pixels[pixel_pos + 1] = color_depth.g;
319  rgb.pixels[pixel_pos + 2] = color_depth.b;
320  }
321  else if (!mask.pixels[pixel_pos])
322  {
323  rgb.pixels[pixel_pos + 0] = color_invalid.r;
324  rgb.pixels[pixel_pos + 1] = color_invalid.g;
325  rgb.pixels[pixel_pos + 2] = color_invalid.b;
326  }
327  }
328 }
329 
330 
332 depthfilter::Component::createPropertyDefinitions()
333 {
334  PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
335 
336  // Options for inter process communication.
337  defs->defineOptionalProperty<std::string>(
338  "ipc.ImageProviderIn",
339  "ImageProvider",
340  "Name of the input image provider (before filter)."
341  );
342  defs->defineOptionalProperty<std::string>(
343  "ipc.ImageProviderOut",
344  "ImageProvider2",
345  "Name of the output image provider (after filter)."
346  );
347 
348  // Depth filter configuration.
349  defs->defineOptionalProperty<int>(
350  "conf.DepthThreshold",
351  1000,
352  "Depth threshold in [mm]. Positive values: Set all RGB values to `conf.Color` if actual "
353  "depth exceeds the depth threshold. Negative values: Set all RGB values to `conf.Color` "
354  "if actual depth undercuts the depth threshold."
355  );
356  defs->defineOptionalProperty<std::string>(
357  "conf.ColorDepth",
358  "255,255,255",
359  "Color to replace for depth. \nFormat: R,G,B \nColor values: 0-255"
360  );
361  defs->defineOptionalProperty<std::string>(
362  "conf.ColorInvalid",
363  "0,0,0",
364  "Color to replace for invalid patches. \nFormat: R,G,B \nColor values: 0-255"
365  );
366  defs->defineOptionalProperty<bool>(
367  "conf.RGBOnly",
368  false,
369  "Provide filtered RGB image only instead of RGB-D image."
370  );
371 
372  return defs;
373 }
visionx::depthfilter::Component::replace_mode
replace_mode
Definition: Component.h:51
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
visionx::ImageProviderInfo::numberImages
int numberImages
Number of images.
Definition: ImageProcessor.h:506
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::PropertyDefinitionContainer::defineOptionalProperty
PropertyDefinition< PropertyType > & defineOptionalProperty(const std::string &name, PropertyType defaultValue, const std::string &description="", PropertyDefinitionBase::PropertyConstness constness=PropertyDefinitionBase::eConstant)
Definition: PropertyDefinitionContainer.h:533
Component.h
visionx::ImageProviderInfo::imageFormat
ImageFormatInfo imageFormat
Image format struct that contains all necessary image information.
Definition: ImageProcessor.h:496
IceUtil
Definition: Instance.h:21
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
visionx::ImageProviderInfo
Definition: ImageProcessor.h:466
visionx::depthfilter
Definition: Component.h:41
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
visionx::ImageProviderInfo::destinationImageType
ImageType destinationImageType
Required image destination type.
Definition: ImageProcessor.h:491
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
IceUtil::Handle< class PropertyDefinitionContainer >
ImageUtil.h
visionx::tools::rgbToDepthValue
float rgbToDepthValue(unsigned char r, unsigned char g, unsigned char b, bool noiseResistant=false)
Definition: ImageUtil.h:142
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36