rs-depth-filter.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2019 Intel Corporation. All Rights Reserved.
3 
4 #include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
6 #include <opencv2/opencv.hpp> // Include OpenCV API
7 
8 #include <fstream>
9 
10 #include "tiny-profiler.h"
11 #include "downsample.h"
12 
14 {
16  res.width /= f; res.height /= f;
17  res.fx /= f; res.fy /= f;
18  res.ppx /= f; res.ppy /= f;
19  return res;
20 }
21 
23 {
24 public:
26  : filter([this](rs2::frame f, rs2::frame_source& src) {
27  scoped_timer t("collision_avoidance_filter");
28  sdk_handle(f, src);
29  })
30  {
31  }
32 
33 private:
34  void downsample(const cv::Mat& depth, const cv::Mat& ir)
35  {
36  scoped_timer t("Depth and IR Downsample");
37 
38  constexpr float DOWNSAMPLE_FRACTION = 1.0f / DOWNSAMPLE_FACTOR;
39 
40  if (_decimated_depth.cols != depth.cols / DOWNSAMPLE_FACTOR ||
41  _decimated_depth.rows != depth.rows / DOWNSAMPLE_FACTOR)
42  {
43  _decimated_depth = cv::Mat(depth.rows / DOWNSAMPLE_FACTOR, depth.cols / DOWNSAMPLE_FACTOR, CV_16U);
44  }
45 
47  cv::resize(ir, _decimated_ir, cv::Size(), DOWNSAMPLE_FRACTION, DOWNSAMPLE_FRACTION, cv::INTER_NEAREST);
48  }
49 
50  void main_filter()
51  {
52  scoped_timer t("Main Filter");
53 
54  const auto num_sub_images = sub_areas.size();
55 
56  #pragma omp parallel for
57  for (int i = 0; i < num_sub_images; i++)
58  {
61  cv::bitwise_or(sub_areas[i].edge_mask, sub_areas[i].harris_mask, sub_areas[i].combined_mask);
62 
63  // morphology: open(src, element) = dilate(erode(src,element))
64  cv::morphologyEx(sub_areas[i].combined_mask, sub_areas[i].combined_mask,
65  cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
66  sub_areas[i].decimated_depth.copyTo(sub_areas[i].output_depth, sub_areas[i].combined_mask);
67  }
68  }
69 
71  {
72  auto frames = f.as<rs2::frameset>();
73  assert(frames);
74 
75  auto depth_frame = frames.get_depth_frame();
76  assert(depth_frame);
77 
78  auto ir_frame = frames.get_infrared_frame();
79  assert(ir_frame);
80 
81  if (!_output_ir_profile || _input_ir_profile.get() != ir_frame.get_profile().get())
82  {
83  auto p = ir_frame.get_profile().as<rs2::video_stream_profile>();
84  auto intr = p.get_intrinsics() / DOWNSAMPLE_FACTOR;
86  _output_ir_profile = p.clone(p.stream_type(), p.stream_index(), p.format(),
87  p.width() / DOWNSAMPLE_FACTOR, p.height() / DOWNSAMPLE_FACTOR, intr);
88  }
89 
90  if (!_output_depth_profile || _input_depth_profile.get() != depth_frame.get_profile().get())
91  {
92  auto p = depth_frame.get_profile().as<rs2::video_stream_profile>();
93  auto intr = p.get_intrinsics() / DOWNSAMPLE_FACTOR;
95  _output_depth_profile = p.clone(p.stream_type(), p.stream_index(), p.format(),
96  p.width() / DOWNSAMPLE_FACTOR, p.height() / DOWNSAMPLE_FACTOR, intr);
97  }
98 
99  cv::Mat matDepth(cv::Size(depth_frame.get_width(), depth_frame.get_height()), CV_16U, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);
100  cv::Mat matGray(cv::Size(ir_frame.get_width(), ir_frame.get_height()), CV_8U, (void*)ir_frame.get_data(), cv::Mat::AUTO_STEP);
101 
102  downsample(matDepth, matGray);
103 
105 
106  main_filter();
107 
108  auto newW = ir_frame.get_width() / DOWNSAMPLE_FACTOR;
109  auto newH = ir_frame.get_height() / DOWNSAMPLE_FACTOR;
110 
111  auto res_ir = src.allocate_video_frame(_output_ir_profile, ir_frame, 0,
112  newW, newH, ir_frame.get_bytes_per_pixel() * newW);
113  memcpy((void*)res_ir.get_data(), _decimated_ir.data, newW * newH);
114 
115  auto res_depth = src.allocate_video_frame(_output_depth_profile, depth_frame, 0,
116  newW, newH, depth_frame.get_bytes_per_pixel() * newW, RS2_EXTENSION_DEPTH_FRAME);
117  memcpy((void*)res_depth.get_data(), _depth_output.data, newW * newH * 2);
118 
119  std::vector<rs2::frame> fs{ res_ir, res_depth };
120  auto cmp = src.allocate_composite_frame(fs);
121  src.frame_ready(cmp);
122  }
123 
124  static constexpr auto DOWNSAMPLE_FACTOR = 4;
125  static constexpr size_t NUM_SUB_IMAGES = 4; // this number directly relates to how many threads are used
126 
127  struct sub_area
128  {
130  cv::Mat decimated_ir;
131  cv::Mat edge_mask;
132  cv::Mat harris_mask;
133  cv::Mat combined_mask;
134  cv::Mat output_depth;
135  cv::Mat float_ir;
136  cv::Mat corners;
137  cv::Mat scharr_x;
138  cv::Mat scharr_y;
139  cv::Mat abs_scharr_x;
140  cv::Mat abs_scharr_y;
141  };
142 
143  std::vector<sub_area> sub_areas;
144 
145  // To avoid re-allocation every cycle some matrices are members
147  cv::Mat _decimated_ir;
148  cv::Mat _mask_edge;
149  cv::Mat _mask_harris;
150  cv::Mat _mask_combined;
151  cv::Mat _depth_output;
152  cv::Mat _ir_float;
153  cv::Mat _corners;
154  cv::Mat _scharr_x;
155  cv::Mat _scharr_y;
156  cv::Mat _abs_scharr_x;
157  cv::Mat _abs_scharr_y;
158 
161 
164 
166  {
167  area->decimated_ir.convertTo(area->float_ir, CV_32F);
168  cv::cornerHarris(area->float_ir, area->corners, 2, 3, 0.04);
169  cv::threshold(area->corners, area->corners, 300, 255, cv::THRESH_BINARY);
170  area->corners.convertTo(area->harris_mask, CV_8U);
171  }
172 
173  void filter_edges(sub_area* area)
174  {
175  cv::Scharr(area->decimated_ir, area->scharr_x, CV_16S, 1, 0);
176  cv::convertScaleAbs(area->scharr_x, area->abs_scharr_x);
177  cv::Scharr(area->decimated_ir, area->scharr_y, CV_16S, 0, 1);
178  cv::convertScaleAbs(area->scharr_y, area->abs_scharr_y);
179  cv::addWeighted(area->abs_scharr_y, 0.5, area->abs_scharr_y, 0.5, 0, area->edge_mask);
180  cv::threshold(area->edge_mask, area->edge_mask, 192, 255, cv::THRESH_BINARY);
181  }
182 
183  void init_matrices(const cv::Mat& ir_resized)
184  {
185  const int sizeX = ir_resized.cols;
186  const int sizeY = ir_resized.rows;
187  const int sizeYperSubImage = sizeY / NUM_SUB_IMAGES;
188  const int sizeXtimesSizeY = sizeX * sizeY;
189 
190  // test on one case if something has changed - this would then apply to all other image matrices as well
191  bool needToReinitialize = _mask_edge.size() != ir_resized.size() || _mask_edge.type() != CV_8U;
192 
193  if (!needToReinitialize)
194  {
195  // this needs resetting every frame
196  memset(_depth_output.data, 0, 2 * sizeXtimesSizeY);
197  return;
198  }
199 
200  static_assert(NUM_SUB_IMAGES >= 1 && NUM_SUB_IMAGES < 64, "NUM_SUB_IMAGES value might be wrong");
201 
202  _mask_edge = cv::Mat(ir_resized.size(), CV_8U);
203  _mask_harris = cv::Mat(ir_resized.size(), CV_8U);
204  _mask_combined = cv::Mat(ir_resized.size(), CV_8U);
205  _depth_output = cv::Mat(ir_resized.size(), CV_16U);
206  _ir_float = cv::Mat(ir_resized.size(), CV_32F);
207  _corners = cv::Mat(ir_resized.size(), CV_32F);
208  _scharr_x = cv::Mat(ir_resized.size(), CV_16S);
209  _scharr_y = cv::Mat(ir_resized.size(), CV_16S);
210  _abs_scharr_x = cv::Mat(ir_resized.size(), CV_8U);
211  _abs_scharr_y = cv::Mat(ir_resized.size(), CV_8U);
212 
213  // this needs to be cleared out, as we only copy to it with a mask later
214  memset(_depth_output.data, 0, 2 * sizeXtimesSizeY);
215 
216  std::vector<cv::Rect> rects;
217  for (int i = 0; i < NUM_SUB_IMAGES; i++)
218  {
219  cv::Rect rect(0, i * sizeYperSubImage, sizeX, sizeYperSubImage);
220  rects.push_back(rect);
221  }
222 
223  for (auto&& rect : rects)
224  {
225  sub_area si;
228  si.edge_mask = _mask_edge(rect);
232  si.float_ir = _ir_float(rect);
233  si.corners = _corners(rect);
234  si.scharr_x = _scharr_x(rect);
235  si.scharr_y = _scharr_y(rect);
238  sub_areas.push_back(si);
239  }
240  }
241 };
242 
243 int main(int argc, char * argv[]) try
244 {
245  rs2::colorizer color_map;
246 
248 
249  // NOTE: This example is strongly coupled with D43x cameras
250  // With minor modifications it can be executed with other D400 cameras,
251  // and even the SR300.
252  // However, part of the value of this example is the real-life case-study it is based on
254  cfg.enable_stream(RS2_STREAM_DEPTH, 848, 480);
256 
257  using namespace cv;
258  const auto window_name = "Display Image";
259  namedWindow(window_name, WINDOW_AUTOSIZE);
260 
262 
263  // See camera-settings.json next to the source / binaries
264  std::ifstream file("./camera-settings.json");
265  if (file.good())
266  {
267  std::string str((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
268 
269  auto prof = cfg.resolve(pipe);
270  if (auto advanced = prof.get_device().as<rs400::advanced_mode>())
271  {
272  advanced.load_json(str);
273  }
274  }
275  else
276  {
277  std::cout << "Couldn't find camera-settings.json, skipping custom settings!" << std::endl;
278  }
279 
280  pipe.start(cfg);
281 
282  while (waitKey(1) < 0 && getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
283  {
284  rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
285 
286  data = data.apply_filter(filter);
287 
288  rs2::frame depth = data.get_depth_frame().apply_filter(color_map);
289 
290  // Query frame size (width and height)
291  const int w = depth.as<rs2::video_frame>().get_width();
292  const int h = depth.as<rs2::video_frame>().get_height();
293 
294  // Create OpenCV matrix of size (w,h) from the colorized depth data
295  Mat image(Size(w, h), CV_8UC3, (void*)depth.get_data(), Mat::AUTO_STEP);
296 
297  // Update the window with new data
298  imshow(window_name, image);
299  }
300 
301  return EXIT_SUCCESS;
302 }
303 catch (const rs2::error & e)
304 {
305  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
306  return EXIT_FAILURE;
307 }
308 catch (const std::exception& e)
309 {
310  std::cerr << e.what() << std::endl;
311  return EXIT_FAILURE;
312 }
313 
314 
315 
frame apply_filter(filter_interface &filter)
Definition: rs_frame.hpp:593
frameset wait_for_frames(unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
void downsample_min_4x4(const cv::Mat &source, cv::Mat *pDest)
Definition: downsample.cpp:13
void downsample(const cv::Mat &depth, const cv::Mat &ir)
static constexpr auto DOWNSAMPLE_FACTOR
GLfloat GLfloat p
Definition: glext.h:12687
rs2::stream_profile _input_depth_profile
void filter_edges(sub_area *area)
GLint GLint GLsizei GLsizei GLsizei depth
const void * get_data() const
Definition: rs_frame.hpp:545
Definition: cah-model.h:10
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
GLenum src
Definition: glext.h:1751
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
e
Definition: rmse.py:177
GLint GLint GLint GLint GLint GLint GLint GLbitfield GLenum filter
void frame_ready(frame result) const
GLenum GLenum GLsizei void * image
depth_frame get_depth_frame() const
Definition: rs_frame.hpp:1006
GLdouble t
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
rs2::stream_profile _input_ir_profile
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
GLdouble f
static constexpr size_t NUM_SUB_IMAGES
void sdk_handle(rs2::frame &f, rs2::frame_source &src)
std::vector< sub_area > sub_areas
std::ostream & cout()
void filter_harris(sub_area *area)
rs2::stream_profile _output_depth_profile
frame allocate_composite_frame(std::vector< frame > frames) const
void enable_stream(rs2_stream stream_type, int stream_index, int width, int height, rs2_format format=RS2_FORMAT_ANY, int framerate=0)
Definition: example.hpp:70
filter(std::shared_ptr< rs2_processing_block > block, int queue_size=1)
pipeline_profile resolve(std::shared_ptr< rs2_pipeline > p) const
int main(int argc, char *argv[])
const rs2_stream_profile * get() const
Definition: rs_frame.hpp:137
Video stream intrinsics.
Definition: rs_types.h:58
std::ostream & cerr()
int i
GLuint res
Definition: glext.h:8856
pipeline_profile start()
rs2_intrinsics operator/(const rs2_intrinsics &i, int f)
stream_profile clone(rs2_stream type, int index, rs2_format format) const
Definition: rs_frame.hpp:63
void init_matrices(const cv::Mat &ir_resized)
const std::string & get_failed_function() const
Definition: rs_types.hpp:112
Definition: parser.hpp:150
rs2::stream_profile _output_ir_profile
T as() const
Definition: rs_frame.hpp:580


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