spatial-filter.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #include "../include/librealsense2/hpp/rs_sensor.hpp"
5 #include "../include/librealsense2/hpp/rs_processing.hpp"
6 #include "option.h"
7 #include "environment.h"
8 #include "context.h"
9 #include "software-device.h"
10 #include "proc/synthetic-stream.h"
12 #include "proc/spatial-filter.h"
13 
14 namespace librealsense
15 {
17  {
25  };
26 
27  // The weight of the current pixel for smoothing is bounded within [25..100]%
28  const float alpha_min_val = 0.25f;
29  const float alpha_max_val = 1.f;
30  const float alpha_default_val = 0.5f;
31  const float alpha_step = 0.01f;
32 
33  // The depth gradient below which the smoothing will occur as number of depth levels
35  const uint8_t delta_max_val = 50;
37  const uint8_t delta_step = 1;
38 
39  // the number of passes used in the iterative smoothing approach
44 
45  // The holes filling mode
50 
52  depth_processing_block("Spatial Filter"),
53  _spatial_alpha_param(alpha_default_val),
54  _spatial_delta_param(delta_default_val),
55  _spatial_iterations(filter_iter_def),
56  _width(0), _height(0), _stride(0), _bpp(0),
57  _extension_type(RS2_EXTENSION_DEPTH_FRAME),
58  _current_frm_size_pixels(0),
59  _stereoscopic_depth(false),
60  _focal_lenght_mm(0.f),
61  _stereo_baseline_mm(0.f),
62  _holes_filling_mode(holes_fill_def),
63  _holes_filling_radius(0)
64  {
67 
68  auto spatial_filter_alpha = std::make_shared<ptr_option<float>>(
71  alpha_step,
73  &_spatial_alpha_param, "Alpha factor of Exp.moving average, 1 = no filter, 0 = infinite filter");
74 
75  auto spatial_filter_delta = std::make_shared<ptr_option<uint8_t>>(
78  delta_step,
80  &_spatial_delta_param, "Edge-preserving Threshold");
81  spatial_filter_delta->on_set([this, spatial_filter_delta](float val)
82  {
83  std::lock_guard<std::mutex> lock(_mutex);
84 
85  if (!spatial_filter_delta->is_valid(val))
87  << "Unsupported spatial delta: " << val << " is out of range.");
88 
89  _spatial_delta_param = static_cast<uint8_t>(val);
90  _spatial_edge_threshold = float(_spatial_delta_param);
91  });
92 
93  auto spatial_filter_iterations = std::make_shared<ptr_option<uint8_t>>(
98  &_spatial_iterations, "Filtering iterations");
99 
100  auto holes_filling_mode = std::make_shared<ptr_option<uint8_t>>(
105  &_holes_filling_mode, "Holes filling mode");
106 
107  holes_filling_mode->set_description(sp_hf_disabled, "Disabled");
108  holes_filling_mode->set_description(sp_hf_2_pixel_radius, "2-pixel radius");
109  holes_filling_mode->set_description(sp_hf_4_pixel_radius, "4-pixel radius");
110  holes_filling_mode->set_description(sp_hf_8_pixel_radius, "8-pixel radius");
111  holes_filling_mode->set_description(sp_hf_16_pixel_radius, "16-pixel radius");
112  holes_filling_mode->set_description(sp_hf_unlimited_radius, "Unlimited");
113 
114  holes_filling_mode->on_set([this, holes_filling_mode](float val)
115  {
116  std::lock_guard<std::mutex> lock(_mutex);
117 
118  if (!holes_filling_mode->is_valid(val))
120  << "Unsupported mode for spatial holes filling selected: value " << val << " is out of range.");
121 
122  _holes_filling_mode = static_cast<uint8_t>(val);
123  switch (_holes_filling_mode)
124  {
125  case sp_hf_disabled:
126  _holes_filling_radius = 0; // disabled
127  break;
129  _holes_filling_radius = 0xff; // Unrealistic smearing; not particulary useful
130  break;
135  _holes_filling_radius = 0x1 << _holes_filling_mode; // 2's exponential radius
136  break;
137  default:
139  << "Unsupported spatial hole-filling requested: value " << _holes_filling_mode << " is out of range.");
140  break;
141  }
142  });
143 
144  register_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, spatial_filter_alpha);
145  register_option(RS2_OPTION_FILTER_SMOOTH_DELTA, spatial_filter_delta);
146  register_option(RS2_OPTION_FILTER_MAGNITUDE, spatial_filter_iterations);
147  register_option(RS2_OPTION_HOLES_FILL, holes_filling_mode);
148  }
149 
151  {
152  rs2::frame tgt;
153 
155  tgt = prepare_target_frame(f, source);
156 
157  // Spatial domain transform edge-preserving filter
159  dxf_smooth<float>(const_cast<void*>(tgt.get_data()), _spatial_alpha_param, _spatial_edge_threshold, _spatial_iterations);
160  else
161  dxf_smooth<uint16_t>(const_cast<void*>(tgt.get_data()), _spatial_alpha_param, _spatial_edge_threshold, _spatial_iterations);
162 
163  return tgt;
164  }
165 
167  {
168  if (f.get_profile().get() != _source_stream_profile.get())
169  {
172 
174  _bpp = (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ? sizeof(float) : sizeof(uint16_t);
176  _focal_lenght_mm = vp.get_intrinsics().fx;
177  _width = vp.width();
178  _height = vp.height();
179  _stride = _width * _bpp;
181 
182  // Check if the new frame originated from stereo-based depth sensor
183  // retrieve the stereo baseline parameter
184  // TODO refactor disparity parameters into the frame's metadata
185  auto snr = ((frame_interface*)f.get())->get_sensor().get();
187 
188  // Playback sensor
189  if (auto a = As<librealsense::extendable_interface>(snr))
190  {
193  {
194  dss = ptr;
196  }
197  }
198  else if (auto depth_emul = As<librealsense::software_sensor>(snr))
199  {
200  // Software device can obtain these options via Options interface
201  if (depth_emul->supports_option(RS2_OPTION_STEREO_BASELINE))
202  {
203  _stereo_baseline_mm = depth_emul->get_option(RS2_OPTION_STEREO_BASELINE).query()*1000.f;
204  _stereoscopic_depth = true;
205  }
206  }
207  else // Live sensor
208  {
209  _stereoscopic_depth = Is<librealsense::depth_stereo_sensor>(snr);
210  dss = As<librealsense::depth_stereo_sensor>(snr);
212  _stereo_baseline_mm = dss->get_stereo_baseline_mm();
213  }
214 
215  _spatial_edge_threshold = _spatial_delta_param;// (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ?
216  // (_focal_lenght_mm * _stereo_baseline_mm) / float(_spatial_delta_param) : _spatial_delta_param;
217  }
218  }
219 
221  {
222  // Allocate and copy the content of the original Depth data to the target
224 
225  memmove(const_cast<void*>(tgt.get_data()), f.get_data(), _current_frm_size_pixels * _bpp);
226  return tgt;
227  }
228 
230  {
231  float *image = reinterpret_cast<float*>(image_data);
232 
233  int v, u;
234 
235  for (v = 0; v < _height;) {
236  // left to right
237  float *im = image + v * _width;
238  float state = *im;
239  float previousInnovation = state;
240 
241  im++;
242  float innovation = *im;
243  u = int(_width) - 1;
244  if (!(*(int*)&previousInnovation > 0))
245  goto CurrentlyInvalidLR;
246  // else fall through
247 
248  CurrentlyValidLR:
249  for (;;) {
250  if (*(int*)&innovation > 0) {
251  float delta = previousInnovation - innovation;
252  bool smallDifference = delta < deltaZ && delta > -deltaZ;
253 
254  if (smallDifference) {
255  float filtered = innovation * alpha + state * (1.0f - alpha);
256  *im = state = filtered;
257  }
258  else {
259  state = innovation;
260  }
261  u--;
262  if (u <= 0)
263  goto DoneLR;
264  previousInnovation = innovation;
265  im += 1;
266  innovation = *im;
267  }
268  else { // switch to CurrentlyInvalid state
269  u--;
270  if (u <= 0)
271  goto DoneLR;
272  previousInnovation = innovation;
273  im += 1;
274  innovation = *im;
275  goto CurrentlyInvalidLR;
276  }
277  }
278 
279  CurrentlyInvalidLR:
280  for (;;) {
281  u--;
282  if (u <= 0)
283  goto DoneLR;
284  if (*(int*)&innovation > 0) { // switch to CurrentlyValid state
285  previousInnovation = state = innovation;
286  im += 1;
287  innovation = *im;
288  goto CurrentlyValidLR;
289  }
290  else {
291  im += 1;
292  innovation = *im;
293  }
294  }
295  DoneLR:
296 
297  // right to left
298  im = image + (v + 1) * _width - 2; // end of row - two pixels
299  previousInnovation = state = im[1];
300  u = int(_width) - 1;
301  innovation = *im;
302  if (!(*(int*)&previousInnovation > 0))
303  goto CurrentlyInvalidRL;
304  // else fall through
305  CurrentlyValidRL:
306  for (;;) {
307  if (*(int*)&innovation > 0) {
308  float delta = previousInnovation - innovation;
309  bool smallDifference = delta < deltaZ && delta > -deltaZ;
310 
311  if (smallDifference) {
312  float filtered = innovation * alpha + state * (1.0f - alpha);
313  *im = state = filtered;
314  }
315  else {
316  state = innovation;
317  }
318  u--;
319  if (u <= 0)
320  goto DoneRL;
321  previousInnovation = innovation;
322  im -= 1;
323  innovation = *im;
324  }
325  else { // switch to CurrentlyInvalid state
326  u--;
327  if (u <= 0)
328  goto DoneRL;
329  previousInnovation = innovation;
330  im -= 1;
331  innovation = *im;
332  goto CurrentlyInvalidRL;
333  }
334  }
335 
336  CurrentlyInvalidRL:
337  for (;;) {
338  u--;
339  if (u <= 0)
340  goto DoneRL;
341  if (*(int*)&innovation > 0) { // switch to CurrentlyValid state
342  previousInnovation = state = innovation;
343  im -= 1;
344  innovation = *im;
345  goto CurrentlyValidRL;
346  }
347  else {
348  im -= 1;
349  innovation = *im;
350  }
351  }
352  DoneRL:
353  v++;
354  }
355  }
356 
358  {
359  float *image = reinterpret_cast<float*>(image_data);
360 
361  int v, u;
362 
363  // we'll do one column at a time, top to bottom, bottom to top, left to right,
364 
365  for (u = 0; u < _width;) {
366 
367  float *im = image + u;
368  float state = im[0];
369  float previousInnovation = state;
370 
371  v = int(_height) - 1;
372  im += _width;
373  float innovation = *im;
374 
375  if (!(*(int*)&previousInnovation > 0))
376  goto CurrentlyInvalidTB;
377  // else fall through
378 
379  CurrentlyValidTB:
380  for (;;) {
381  if (*(int*)&innovation > 0) {
382  float delta = previousInnovation - innovation;
383  bool smallDifference = delta < deltaZ && delta > -deltaZ;
384 
385  if (smallDifference) {
386  float filtered = innovation * alpha + state * (1.0f - alpha);
387  *im = state = filtered;
388  }
389  else {
390  state = innovation;
391  }
392  v--;
393  if (v <= 0)
394  goto DoneTB;
395  previousInnovation = innovation;
396  im += _width;
397  innovation = *im;
398  }
399  else { // switch to CurrentlyInvalid state
400  v--;
401  if (v <= 0)
402  goto DoneTB;
403  previousInnovation = innovation;
404  im += _width;
405  innovation = *im;
406  goto CurrentlyInvalidTB;
407  }
408  }
409 
410  CurrentlyInvalidTB:
411  for (;;) {
412  v--;
413  if (v <= 0)
414  goto DoneTB;
415  if (*(int*)&innovation > 0) { // switch to CurrentlyValid state
416  previousInnovation = state = innovation;
417  im += _width;
418  innovation = *im;
419  goto CurrentlyValidTB;
420  }
421  else {
422  im += _width;
423  innovation = *im;
424  }
425  }
426  DoneTB:
427 
428  im = image + u + (_height - 2) * _width;
429  state = im[_width];
430  previousInnovation = state;
431  innovation = *im;
432  v = int(_height) - 1;
433  if (!(*(int*)&previousInnovation > 0))
434  goto CurrentlyInvalidBT;
435  // else fall through
436  CurrentlyValidBT:
437  for (;;) {
438  if (*(int*)&innovation > 0) {
439  float delta = previousInnovation - innovation;
440  bool smallDifference = delta < deltaZ && delta > -deltaZ;
441 
442  if (smallDifference) {
443  float filtered = innovation * alpha + state * (1.0f - alpha);
444  *im = state = filtered;
445  }
446  else {
447  state = innovation;
448  }
449  v--;
450  if (v <= 0)
451  goto DoneBT;
452  previousInnovation = innovation;
453  im -= _width;
454  innovation = *im;
455  }
456  else { // switch to CurrentlyInvalid state
457  v--;
458  if (v <= 0)
459  goto DoneBT;
460  previousInnovation = innovation;
461  im -= _width;
462  innovation = *im;
463  goto CurrentlyInvalidBT;
464  }
465  }
466 
467  CurrentlyInvalidBT:
468  for (;;) {
469  v--;
470  if (v <= 0)
471  goto DoneBT;
472  if (*(int*)&innovation > 0) { // switch to CurrentlyValid state
473  previousInnovation = state = innovation;
474  im -= _width;
475  innovation = *im;
476  goto CurrentlyValidBT;
477  }
478  else {
479  im -= _width;
480  innovation = *im;
481  }
482  }
483  DoneBT:
484  u++;
485  }
486  }
487 }
static const textual_icon lock
Definition: model-views.h:218
rs2_frame * get() const
Definition: rs_frame.hpp:590
const uint8_t filter_iter_max
rs2::stream_profile _target_stream_profile
rs2::frame prepare_target_frame(const rs2::frame &f, const rs2::frame_source &source)
stream_profile get_profile() const
Definition: rs_frame.hpp:557
const void * get_data() const
Definition: rs_frame.hpp:545
unsigned short uint16_t
Definition: stdint.h:79
const uint8_t holes_fill_def
unsigned char uint8_t
Definition: stdint.h:78
GLenum GLenum GLsizei void * image
void register_option(rs2_option id, std::shared_ptr< option > option)
Definition: options.h:86
GLboolean GLboolean GLboolean GLboolean a
frame allocate_video_frame(const stream_profile &profile, const frame &original, int new_bpp=0, int new_width=0, int new_height=0, int new_stride=0, rs2_extension frame_type=RS2_EXTENSION_VIDEO_FRAME) const
GLuint GLfloat * val
GLdouble f
bool is() const
Definition: rs_frame.hpp:570
const uint8_t holes_fill_step
GLfloat GLfloat GLfloat alpha
const uint8_t delta_default_val
const uint8_t delta_min_val
rs2::stream_profile _source_stream_profile
const uint8_t filter_iter_def
const uint8_t holes_fill_max
virtual float get_stereo_baseline_mm() const =0
void recursive_filter_horizontal_fp(void *image_data, float alpha, float deltaZ)
const float alpha_default_val
const float alpha_step
void recursive_filter_vertical_fp(void *image_data, float alpha, float deltaZ)
void update_configuration(const rs2::frame &f)
const uint8_t delta_step
rs2_format format() const
Definition: rs_frame.hpp:44
const float alpha_max_val
const uint8_t delta_max_val
const rs2_stream_profile * get() const
Definition: rs_frame.hpp:137
const uint8_t filter_iter_min
const uint8_t holes_fill_min
GLsizei GLsizei GLchar * source
const float alpha_min_val
im
Definition: rmse.py:121
GLdouble v
stream_profile clone(rs2_stream type, int index, rs2_format format) const
Definition: rs_frame.hpp:63
rs2::frame process_frame(const rs2::frame_source &source, const rs2::frame &f) override
std::string to_string(T value)
const uint8_t filter_iter_step


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:41