occlusion-filter.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2018 Intel Corporation. All Rights Reserved.
3 
4 #include "../include/librealsense2/rs.hpp"
5 #include "../include/librealsense2/rsutil.h"
8 //#include "../../common/tiny-profiler.h"
9 #include <vector>
10 #include <cmath>
11 
12 
13 namespace librealsense
14 {
15  occlusion_filter::occlusion_filter() : _occlusion_filter(occlusion_monotonic_scan) , _occlusion_scanning(horizontal)
16  {
17  }
18 
20  {
23  }
24 
25  void occlusion_filter::process(float3* points, float2* uv_map, const std::vector<float2> & pix_coord, const rs2::depth_frame& depth) const
26  {
27  switch (_occlusion_filter)
28  {
29  case occlusion_none:
30  break;
32  monotonic_heuristic_invalidation(points, uv_map, pix_coord, depth);
33  break;
34  default:
35  throw std::runtime_error(to_string() << "Unsupported occlusion filter type " << _occlusion_filter << " requested");
36  break;
37  }
38  }
39  int gcd(int a, int b) {
40  if (b == 0)
41  return a;
42  return gcd(b, a % b);
43  }
44  // Return the greatest common divisor of a
45  // and b which lie in the given range.
46  int maxDivisorRange(int a, int b, int lo, int hi)
47  {
48  if (lo > hi)
49  {
50  std::swap(hi, lo);
51  }
52  int g = gcd(a, b);
53  int res = g;
54 
55  // Loop from 1 to sqrt(GCD(a, b).
56  for (int i = lo; i * i <= g && i <= hi; i++)
57 
58  if ((g % i == 0) && (g / i) <= hi)
59  {
60  res = g / i;
61  break;
62  }
63 
64  return res;
65  }
66  template<size_t SIZE>
67  void rotate_image_optimized(byte* dest[], const byte* source, int width, int height)
68  {
69 
70  auto width_out = height;
71  auto height_out = width;
72 
73  auto out = dest[0];
74  auto buffer_size = maxDivisorRange(height, width, 1, ROTATION_BUFFER_SIZE);
75 
76  byte** buffer = new byte * [buffer_size];
77  for (int i = 0; i < buffer_size; ++i)
78  buffer[i] = new byte[buffer_size * SIZE];
79 
80 
81  for (int i = 0; i <= height - buffer_size; i = i + buffer_size)
82  {
83  for (int j = 0; j <= width - buffer_size; j = j + buffer_size)
84  {
85  for (int ii = 0; ii < buffer_size; ++ii)
86  {
87  for (int jj = 0; jj < buffer_size; ++jj)
88  {
89  auto source_index = ((j + jj) + (width * (i + ii))) * SIZE;
90  memcpy((void*)&(buffer[(buffer_size-1 - jj)][(buffer_size-1 - ii) * SIZE]), &source[source_index], SIZE);
91  }
92  }
93 
94  for (int ii = 0; ii < buffer_size; ++ii)
95  {
96  auto out_index = (((height_out - buffer_size - j + 1) * width_out) - i - buffer_size + (ii)*width_out);
97  memcpy(&out[(out_index)*SIZE], (buffer[ii]), buffer_size * SIZE);
98  }
99  }
100  }
101 
102  for (int i = 0; i < buffer_size; ++i)
103  {
104  delete[] buffer[i];
105  }
106  delete[] buffer;
107 
108  }
109  // IMPORTANT! This implementation is based on the assumption that the RGB sensor is positioned strictly to the left of the depth sensor.
110  // namely D415/D435 and SR300. The implementation WILL NOT work properly for different setups
111  // Heuristic occlusion invalidation algorithm:
112  // - Use the uv texels calculated when projecting depth to color
113  // - Scan each line from left to right and check the the U coordinate in the mapping is raising monotonically.
114  // - The occlusion is designated as U coordinate for a given pixel is less than the U coordinate of the predecessing pixel.
115  // - The UV mapping for the occluded pixel is reset to (0,0). Later on the (0,0) coordinate in the texture map is overwritten
116  // with a invalidation color such as black/magenta according to the purpose (production/debugging)
117  void occlusion_filter::monotonic_heuristic_invalidation(float3* points, float2* uv_map, const std::vector<float2>& pix_coord, const rs2::depth_frame& depth) const
118  {
119  float occZTh = 0.1f; //meters
120  int occDilationSz = 1;
121  auto points_width = _depth_intrinsics->width;
122  auto points_height = _depth_intrinsics->height;
123  auto pixels_ptr = pix_coord.data();
124  auto points_ptr = points;
125  auto uv_map_ptr = uv_map;
126  float maxInLine = -1;
127  float maxZ = 0;
128 
130  {
131  for( int y = 0; y < points_height; ++y )
132  {
133  maxInLine = -1;
134  maxZ = 0;
135  int occDilationLeft = 0;
136 
137  for(int x = 0; x < points_width; ++x )
138  {
139  if( points_ptr->z )
140  {
141  // Occlusion detection
142  if( pixels_ptr->x < maxInLine
143  || ( pixels_ptr->x == maxInLine && ( points_ptr->z - maxZ ) > occZTh ) )
144  {
145  *points_ptr = { 0, 0, 0 };
146  occDilationLeft = occDilationSz;
147  }
148  else
149  {
150  maxInLine = pixels_ptr->x;
151  maxZ = points_ptr->z;
152  if( occDilationLeft > 0 )
153  {
154  *points_ptr = { 0, 0, 0 };
155  occDilationLeft--;
156  }
157  }
158  }
159  ++points_ptr;
160  ++uv_map_ptr;
161  ++pixels_ptr;
162  }
163  }
164  }
165  else if (_occlusion_scanning == vertical)
166  {
167  auto rotated_depth_width = _depth_intrinsics->height;
168  auto rotated_depth_height = _depth_intrinsics->width;
169  auto depth_ptr = (byte*)(depth.get_data());
170  std::vector< byte > alloc( depth.get_bytes_per_pixel() * points_width * points_height );
171  byte* depth_planes[1];
172  depth_planes[0] = alloc.data();
173 
174  rotate_image_optimized<2>(depth_planes, (const byte*)(depth.get_data()), points_width, points_height);
175 
176  // scan depth frame after rotation: check if there is a noticed jump between adjacen pixels in Z-axis (depth), it means there could be occlusion.
177  // save suspected points and run occlusion-invalidation vertical scan only on them
178  // after rotation : height = points_width , width = points_height
179  for (int i = 0; i < rotated_depth_height; i++)
180  {
181  for (int j = 0; j < rotated_depth_width; j++)
182  {
183  // before depth frame rotation: occlusion detected in the positive direction of Y
184  // after rotation : scan from right to left (positive direction of X) to detect occlusion
185  // compare depth each pixel only with the pixel on its right (i,j+1)
186  auto index = (j + (rotated_depth_width * i));
187  auto uv_index = ((rotated_depth_height - i - 1) + (rotated_depth_width - j - 1) * rotated_depth_height);
188  auto index_right = index + 1;
189  uint16_t* diff_depth_ptr = (uint16_t*)depth_planes[0];
190  uint16_t diff_right = abs((uint16_t)(*(diff_depth_ptr + index)) - (uint16_t)(*(diff_depth_ptr + index_right)));
191  float scaled_threshold = DEPTH_OCCLUSION_THRESHOLD / _depth_units;
192  if (diff_right > scaled_threshold)
193  {
194  points_ptr = points + uv_index;
195  uv_map_ptr = uv_map + uv_index;
196  auto scan_win_size = maxDivisorRange(rotated_depth_height, rotated_depth_width, 1, VERTICAL_SCAN_WINDOW_SIZE);
197 
198  if (j >= scan_win_size) {
199  maxInLine = (uv_map_ptr - 1 * points_width)->y;
200  for (int y = 0; y <= scan_win_size; ++y)
201  {
202  if (((uv_map_ptr + y * points_width)->y < maxInLine))
203  {
204  *(points_ptr + y * points_width) = { 0.f, 0.f };
205  }
206  else
207  {
208  break;
209  }
210 
211  }
212 
213  }
214  }
215  }
216  }
217  }
218  }
219  // Prepare texture map without occlusion that for every texture coordinate there no more than one depth point that is mapped to it
220  // i.e. for every (u,v) map coordinate we select the depth point with minimum Z. all other points that are mapped to this texel will be invalidated
221  // Algo input data:
222  // Vector of 3D [xyz] coordinates of depth_width*depth_height size
223  // Vector of 2D [i,j] coordinates where the val[i,j] stores the texture coordinate (s,t) for the corresponding (i,j) pixel in depth frame
224  // Algo intermediate data:
225  // Vector of depth values (floats) in size of the mapped texture (different from depth width*height) where
226  // each (i,j) cell holds the minimal Z among all the depth pixels that are mapped to the specific texel
227  void occlusion_filter::comprehensive_invalidation(float3* points, float2* uv_map, const std::vector<float2> & pix_coord) const
228  {
229  auto depth_points = points;
230  auto mapped_pix = pix_coord.data();
231  size_t mapped_tex_width = _texels_intrinsics->width;
232  size_t mapped_tex_height = _texels_intrinsics->height;
233  size_t points_width = _depth_intrinsics->width;
234  size_t points_height = _depth_intrinsics->height;
235 
236  static const float z_threshold = 0.05f; // Compensate for temporal noise when comparing Z values
237 
238  // Clear previous data
239  memset((void*)(_texels_depth.data()), 0, _texels_depth.size() * sizeof(float));
240 
241  // Pass1 -generate texels mapping with minimal depth for each texel involved
242  for (size_t i = 0; i < points_height; i++)
243  {
244  for (size_t j = 0; j < points_width; j++)
245  {
246  if ((depth_points->z > 0.0001f) &&
247  (mapped_pix->x > 0.f) && (mapped_pix->x < mapped_tex_width) &&
248  (mapped_pix->y > 0.f) && (mapped_pix->y < mapped_tex_height))
249  {
250  size_t texel_index = (size_t)(mapped_pix->y)*mapped_tex_width + (size_t)(mapped_pix->x);
251 
252  if ((_texels_depth[texel_index] < 0.0001f) || ((_texels_depth[texel_index] + z_threshold) > depth_points->z))
253  {
254  _texels_depth[texel_index] = depth_points->z;
255  }
256  }
257 
258  ++depth_points;
259  ++mapped_pix;
260  }
261  }
262 
263  mapped_pix = pix_coord.data();
264  depth_points = points;
265  auto uv_ptr = uv_map;
266 
267  // Pass2 -invalidate depth texels with occlusion traits
268  for (size_t i = 0; i < points_height; i++)
269  {
270  for (size_t j = 0; j < points_width; j++)
271  {
272  if ((depth_points->z > 0.0001f) &&
273  (mapped_pix->x > 0.f) && (mapped_pix->x < mapped_tex_width) &&
274  (mapped_pix->y > 0.f) && (mapped_pix->y < mapped_tex_height))
275  {
276  size_t texel_index = (size_t)(mapped_pix->y)*mapped_tex_width + (size_t)(mapped_pix->x);
277 
278  if ((_texels_depth[texel_index] > 0.0001f) && ((_texels_depth[texel_index] + z_threshold) < depth_points->z))
279  {
280  *uv_ptr = { 0.f, 0.f };
281  }
282  }
283 
284  ++depth_points;
285  ++mapped_pix;
286  ++uv_ptr;
287  }
288  }
289  }
290 }
GLboolean GLboolean GLboolean b
GLboolean GLboolean g
GLint y
void set_texel_intrinsics(const rs2_intrinsics &in)
void monotonic_heuristic_invalidation(float3 *points, float2 *uv_map, const std::vector< float2 > &pix_coord, const rs2::depth_frame &depth) const
int get_bytes_per_pixel() const
Definition: rs_frame.hpp:707
GLint GLint GLsizei GLsizei GLsizei depth
const void * get_data() const
Definition: rs_frame.hpp:545
unsigned short uint16_t
Definition: stdint.h:79
GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat maxZ
Definition: glext.h:2919
occlusion_rect_type _occlusion_filter
int gcd(int a, int b)
optional_value< rs2_intrinsics > _depth_intrinsics
GLenum GLfloat * buffer
void comprehensive_invalidation(float3 *points, float2 *uv_map, const std::vector< float2 > &pix_coord) const
GLuint index
GLboolean GLboolean GLboolean GLboolean a
#define ROTATION_BUFFER_SIZE
GLdouble f
GLdouble x
GLint GLsizei GLsizei height
occlusion_scanning_type _occlusion_scanning
std::vector< float > _texels_depth
GLint j
void swap(nlohmann::json &j1, nlohmann::json &j2) noexcept(is_nothrow_move_constructible< nlohmann::json >::value andis_nothrow_move_assignable< nlohmann::json >::value)
exchanges the values of two JSON objects
Definition: json.hpp:12141
void rotate_image_optimized(byte *dest[], const byte *source, int width, int height)
unsigned char byte
Definition: src/types.h:52
#define VERTICAL_SCAN_WINDOW_SIZE
GLuint in
Definition: glext.h:8859
Video stream intrinsics.
Definition: rs_types.h:58
GLsizei GLsizei GLchar * source
LZ4LIB_API char * dest
Definition: lz4.h:438
int i
GLuint res
Definition: glext.h:8856
optional_value< rs2_intrinsics > _texels_intrinsics
void process(float3 *points, float2 *uv_map, const std::vector< float2 > &pix_coord, const rs2::depth_frame &depth) const
#define DEPTH_OCCLUSION_THRESHOLD
GLint GLsizei width
GLdouble GLdouble GLint GLint const GLdouble * points
std::string to_string(T value)
int maxDivisorRange(int a, int b, int lo, int hi)


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