rs-grabcuts.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 <librealsense2/rs.hpp> // Include RealSense Cross Platform API
5 #include <opencv2/opencv.hpp> // Include OpenCV API
6 #include "../cv-helpers.hpp" // Helper functions for conversions between RealSense and OpenCV
7 
8 int main(int argc, char * argv[]) try
9 {
10  using namespace cv;
11  using namespace rs2;
12 
13  // Define colorizer and align processing-blocks
14  colorizer colorize;
16 
17  // Start the camera
18  pipeline pipe;
19  pipe.start();
20 
21  const auto window_name = "Display Image";
22  namedWindow(window_name, WINDOW_AUTOSIZE);
23 
24  // We are using StructuringElement for erode / dilate operations
25  auto gen_element = [](int erosion_size)
26  {
27  return getStructuringElement(MORPH_RECT,
28  Size(erosion_size + 1, erosion_size + 1),
29  Point(erosion_size, erosion_size));
30  };
31 
32  const int erosion_size = 3;
33  auto erode_less = gen_element(erosion_size);
34  auto erode_more = gen_element(erosion_size * 2);
35 
36  // The following operation is taking grayscale image,
37  // performs threashold on it, closes small holes and erodes the white area
38  auto create_mask_from_depth = [&](Mat& depth, int thresh, ThresholdTypes type)
39  {
40  threshold(depth, depth, thresh, 255, type);
41  dilate(depth, depth, erode_less);
42  erode(depth, depth, erode_more);
43  };
44 
45  // Skips some frames to allow for auto-exposure stabilization
46  for (int i = 0; i < 10; i++) pipe.wait_for_frames();
47 
48  while (waitKey(1) < 0 && getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
49  {
50  frameset data = pipe.wait_for_frames();
51  // Make sure the frameset is spatialy aligned
52  // (each pixel in depth image corresponds to the same pixel in the color image)
53  frameset aligned_set = align_to.process(data);
54  frame depth = aligned_set.get_depth_frame();
55  auto color_mat = frame_to_mat(aligned_set.get_color_frame());
56 
57  // Colorize depth image with white being near and black being far
58  // This will take advantage of histogram equalization done by the colorizer
60  frame bw_depth = depth.apply_filter(colorize);
61 
62  // Generate "near" mask image:
63  auto near = frame_to_mat(bw_depth);
64  cvtColor(near, near, COLOR_BGR2GRAY);
65  // Take just values within range [180-255]
66  // These will roughly correspond to near objects due to histogram equalization
67  create_mask_from_depth(near, 180, THRESH_BINARY);
68 
69  // Generate "far" mask image:
70  auto far = frame_to_mat(bw_depth);
71  cvtColor(far, far, COLOR_BGR2GRAY);
72  far.setTo(255, far == 0); // Note: 0 value does not indicate pixel near the camera, and requires special attention
73  create_mask_from_depth(far, 100, THRESH_BINARY_INV);
74 
75  // GrabCut algorithm needs a mask with every pixel marked as either:
76  // BGD, FGB, PR_BGD, PR_FGB
77  Mat mask;
78  mask.create(near.size(), CV_8UC1);
79  mask.setTo(Scalar::all(GC_BGD)); // Set "background" as default guess
80  mask.setTo(GC_PR_BGD, far == 0); // Relax this to "probably background" for pixels outside "far" region
81  mask.setTo(GC_FGD, near == 255); // Set pixels within the "near" region to "foreground"
82 
83  // Run Grab-Cut algorithm:
84  Mat bgModel, fgModel;
85  grabCut(color_mat, mask, Rect(), bgModel, fgModel, 1, GC_INIT_WITH_MASK);
86 
87  // Extract foreground pixels based on refined mask from the algorithm
88  Mat3b foreground = Mat3b::zeros(color_mat.rows, color_mat.cols);
89  color_mat.copyTo(foreground, (mask == GC_FGD) | (mask == GC_PR_FGD));
90  imshow(window_name, foreground);
91  }
92 
93  return EXIT_SUCCESS;
94 }
95 catch (const rs2::error & e)
96 {
97  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
98  return EXIT_FAILURE;
99 }
100 catch (const std::exception& e)
101 {
102  std::cerr << e.what() << std::endl;
103  return EXIT_FAILURE;
104 }
105 
106 
107 
frame apply_filter(filter_interface &filter)
Definition: rs_frame.hpp:593
frameset wait_for_frames(unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
GLint GLuint mask
GLint GLint GLsizei GLsizei GLsizei depth
Definition: cah-model.h:10
e
Definition: rmse.py:177
def all()
Definition: devices.py:166
GLdouble far
depth_frame get_depth_frame() const
Definition: rs_frame.hpp:1006
video_frame get_color_frame() const
Definition: rs_frame.hpp:1015
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
frameset process(frameset frames)
GLenum type
int main(int argc, char *argv[])
Definition: rs-grabcuts.cpp:8
std::ostream & cerr()
int i
pipeline_profile start()
void set_option(rs2_option option, float value) const
Definition: rs_options.hpp:99
static cv::Mat frame_to_mat(const rs2::frame &f)
Definition: cv-helpers.hpp:11
const std::string & get_failed_function() const
Definition: rs_types.hpp:112
Definition: parser.hpp:150
::geometry_msgs::Point_< std::allocator< void > > Point
Definition: Point.h:57


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