26 #include <SimoxUtility/algorithm/string/string_tools.h>
35 #include <Image/ByteImage.h>
38 #include <IceUtil/Time.h>
50 DrawColor24Bit parse_color(std::string color)
53 std::vector<unsigned char> rgb_vals;
56 for (
const std::string& val : rgb)
58 const long long int val_check = std::stoll(val);
60 if (0 <= val_check and val_check <= 255)
62 rgb_vals.push_back(
static_cast<unsigned char>(val_check));
66 if (rgb_vals.size() != 3)
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");
72 return DrawColor24Bit{rgb_vals[0], rgb_vals[1], rgb_vals[2]};
77 depthfilter::Component::~Component()
84 depthfilter::Component::onInitPointCloudAndImageProcessor()
87 m_ipc_in = getProperty<std::string>(
"ipc.ImageProviderIn");
89 usingImageProvider(m_ipc_in);
90 usingPointCloudProvider(m_ipc_in);
93 m_ipc_out = getProperty<std::string>(
"ipc.ImageProviderOut");
97 const int val = getProperty<int>(
"conf.DepthThreshold");
101 m_mode = replace_mode::undercut_threshold;
102 m_threshold =
static_cast<unsigned int>(-val);
106 m_mode = replace_mode::exceed_threshold;
107 m_threshold =
static_cast<unsigned int>(val);
112 m_color_depth = parse_color(getProperty<std::string>(
"conf.ColorDepth"));
113 m_color_invalid = parse_color(getProperty<std::string>(
"conf.ColorInvalid"));
118 depthfilter::Component::onConnectPointCloudAndImageProcessor()
120 const bool wait_for_proxy =
true;
123 unsigned int num_images = 1;
125 if (not getProperty<bool>(
"conf.RGBOnly"))
127 num_images =
static_cast<unsigned int>(provider_info.
numberImages);
130 getPointCloudProvider(m_ipc_in, wait_for_proxy);
143 enableResultImagesAndPointClouds<pcl::PointXYZRGBA>(m_ipc_out,
144 static_cast<int>(num_images),
151 m_buffer =
new CByteImage*[2];
154 m_buffer_single =
new CByteImage*[1];
160 depthfilter::Component::onDisconnectPointCloudAndImageProcessor()
165 delete[] m_buffer_single;
170 depthfilter::Component::onExitPointCloudAndImageProcessor()
177 depthfilter::Component::getDefaultName()
const
179 return "DepthFilter";
184 depthfilter::Component::process()
187 const Time timeout = Time::seconds(1);
188 if (not waitForImages(m_ipc_in, timeout))
190 ARMARX_WARNING <<
"Timeout while waiting for images (>" << timeout <<
")";
193 if (!waitForPointClouds(1000))
195 ARMARX_WARNING <<
"Timeout while waiting for pointclouds (>" << timeout <<
")";
201 MetaInfoSizeBasePtr info;
202 getImages(m_ipc_in, m_buffer, info);
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);
209 if (getProperty<bool>(
"conf.RGBOnly"))
211 m_buffer_single[0] = m_buffer[0];
212 provideResultImages(m_buffer_single, info);
216 provideResultImages(m_buffer, info);
219 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentPointCloud(
new pcl::PointCloud<pcl::PointXYZRGBA>());
220 getPointClouds(currentPointCloud);
221 provideResultPointClouds(m_ipc_out, currentPointCloud);
227 depthfilter::Component::applyDepthFilter(
228 const CByteImage& depth,
229 const unsigned int threshold,
230 const int grow_radius,
232 const armarx::DrawColor24Bit& color_depth,
233 const armarx::DrawColor24Bit& color_invalid,
238 const bool noise_resistant =
false;
239 const unsigned int pixels_size =
static_cast<unsigned int>(rgb.width * rgb.height) * 3;
241 for (
int y = 0; y < rgb.height; y++)
243 for (
int x = 0; x < rgb.width; x++)
245 int pp = y * rgb.width * 3 + x * 3;
250 int minval = dd * dd * 1 / 8;
253 for (
int y = 0; y < rgb.height; y += dd)
255 for (
int x = 0; x < rgb.width; x += dd)
259 for (
int xx = x; xx <
std::min(x + dd, rgb.width); xx++)
261 for (
int yy = y; yy <
std::min(y + dd, rgb.height); yy++)
263 int pixel_pos = y * rgb.width * 3 + x * 3;
265 depth.pixels[pixel_pos + 0],
266 depth.pixels[pixel_pos + 1],
267 depth.pixels[pixel_pos + 2],
274 else if ((mode == replace_mode::exceed_threshold and z_value > threshold)
275 or (mode == replace_mode::undercut_threshold and z_value < threshold))
287 if (val_ctr >= minval)
289 for (
int xx =
std::max(0, x - grow_radius); xx <=
std::min(rgb.width - 1, x + grow_radius + dd); xx++)
291 for (
int yy =
std::max(0, y - grow_radius); yy <=
std::min(rgb.height - 1, y + grow_radius + dd); yy++)
293 int pp = yy * rgb.width * 3 + xx * 3;
305 for (
unsigned int pixel_pos = 0; pixel_pos < pixels_size; pixel_pos += 3)
308 depth.pixels[pixel_pos + 0],
309 depth.pixels[pixel_pos + 1],
310 depth.pixels[pixel_pos + 2],
314 if ((mode == replace_mode::exceed_threshold and z_value > threshold)
315 or (mode == replace_mode::undercut_threshold and z_value < threshold))
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;
321 else if (!mask.pixels[pixel_pos])
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;
332 depthfilter::Component::createPropertyDefinitions()
338 "ipc.ImageProviderIn",
340 "Name of the input image provider (before filter)."
342 defs->defineOptionalProperty<std::string>(
343 "ipc.ImageProviderOut",
345 "Name of the output image provider (after filter)."
349 defs->defineOptionalProperty<
int>(
350 "conf.DepthThreshold",
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."
356 defs->defineOptionalProperty<std::string>(
359 "Color to replace for depth. \nFormat: R,G,B \nColor values: 0-255"
361 defs->defineOptionalProperty<std::string>(
364 "Color to replace for invalid patches. \nFormat: R,G,B \nColor values: 0-255"
366 defs->defineOptionalProperty<
bool>(
369 "Provide filtered RGB image only instead of RGB-D image."