rs-dnn.cpp
Go to the documentation of this file.
1 // This example is derived from the ssd_mobilenet_object_detection opencv demo
2 // and adapted to be used with Intel RealSense Cameras
3 // Please see https://github.com/opencv/opencv/blob/master/LICENSE
4 
5 #include <opencv2/dnn.hpp>
6 #include <librealsense2/rs.hpp>
7 #include "../cv-helpers.hpp"
8 
9 const size_t inWidth = 300;
10 const size_t inHeight = 300;
11 const float WHRatio = inWidth / (float)inHeight;
12 const float inScaleFactor = 0.007843f;
13 const float meanVal = 127.5;
14 const char* classNames[] = {"background",
15  "aeroplane", "bicycle", "bird", "boat",
16  "bottle", "bus", "car", "cat", "chair",
17  "cow", "diningtable", "dog", "horse",
18  "motorbike", "person", "pottedplant",
19  "sheep", "sofa", "train", "tvmonitor"};
20 
21 int main(int argc, char** argv) try
22 {
23  using namespace cv;
24  using namespace cv::dnn;
25  using namespace rs2;
26 
27  Net net = readNetFromCaffe("MobileNetSSD_deploy.prototxt",
28  "MobileNetSSD_deploy.caffemodel");
29 
30  // Start streaming from Intel RealSense Camera
31  pipeline pipe;
32  auto config = pipe.start();
33  auto profile = config.get_stream(RS2_STREAM_COLOR)
34  .as<video_stream_profile>();
36 
37  Size cropSize;
38  if (profile.width() / (float)profile.height() > WHRatio)
39  {
40  cropSize = Size(static_cast<int>(profile.height() * WHRatio),
41  profile.height());
42  }
43  else
44  {
45  cropSize = Size(profile.width(),
46  static_cast<int>(profile.width() / WHRatio));
47  }
48 
49  Rect crop(Point((profile.width() - cropSize.width) / 2,
50  (profile.height() - cropSize.height) / 2),
51  cropSize);
52 
53  const auto window_name = "Display Image";
54  namedWindow(window_name, WINDOW_AUTOSIZE);
55 
56  while (getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
57  {
58  // Wait for the next set of frames
59  auto data = pipe.wait_for_frames();
60  // Make sure the frames are spatially aligned
61  data = align_to.process(data);
62 
63  auto color_frame = data.get_color_frame();
64  auto depth_frame = data.get_depth_frame();
65 
66  // If we only received new depth frame,
67  // but the color did not update, continue
68  static int last_frame_number = 0;
69  if (color_frame.get_frame_number() == last_frame_number) continue;
70  last_frame_number = static_cast<int>(color_frame.get_frame_number());
71 
72  // Convert RealSense frame to OpenCV matrix:
73  auto color_mat = frame_to_mat(color_frame);
74  auto depth_mat = depth_frame_to_meters(depth_frame);
75 
76  Mat inputBlob = blobFromImage(color_mat, inScaleFactor,
77  Size(inWidth, inHeight), meanVal, false); //Convert Mat to batch of images
78  net.setInput(inputBlob, "data"); //set the network input
79  Mat detection = net.forward("detection_out"); //compute output
80 
81  Mat detectionMat(detection.size[2], detection.size[3], CV_32F, detection.ptr<float>());
82 
83  // Crop both color and depth frames
84  color_mat = color_mat(crop);
85  depth_mat = depth_mat(crop);
86 
87  float confidenceThreshold = 0.8f;
88  for(int i = 0; i < detectionMat.rows; i++)
89  {
90  float confidence = detectionMat.at<float>(i, 2);
91 
92  if(confidence > confidenceThreshold)
93  {
94  size_t objectClass = (size_t)(detectionMat.at<float>(i, 1));
95 
96  int xLeftBottom = static_cast<int>(detectionMat.at<float>(i, 3) * color_mat.cols);
97  int yLeftBottom = static_cast<int>(detectionMat.at<float>(i, 4) * color_mat.rows);
98  int xRightTop = static_cast<int>(detectionMat.at<float>(i, 5) * color_mat.cols);
99  int yRightTop = static_cast<int>(detectionMat.at<float>(i, 6) * color_mat.rows);
100 
101  Rect object((int)xLeftBottom, (int)yLeftBottom,
102  (int)(xRightTop - xLeftBottom),
103  (int)(yRightTop - yLeftBottom));
104 
105  object = object & Rect(0, 0, depth_mat.cols, depth_mat.rows);
106 
107  // Calculate mean depth inside the detection region
108  // This is a very naive way to estimate objects depth
109  // but it is intended to demonstrate how one might
110  // use depth data in general
111  Scalar m = mean(depth_mat(object));
112 
113  std::ostringstream ss;
114  ss << classNames[objectClass] << " ";
115  ss << std::setprecision(2) << m[0] << " meters away";
116  String conf(ss.str());
117 
118  rectangle(color_mat, object, Scalar(0, 255, 0));
119  int baseLine = 0;
120  Size labelSize = getTextSize(ss.str(), FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
121 
122  auto center = (object.br() + object.tl())*0.5;
123  center.x = center.x - labelSize.width / 2;
124 
125  rectangle(color_mat, Rect(Point(center.x, center.y - labelSize.height),
126  Size(labelSize.width, labelSize.height + baseLine)),
127  Scalar(255, 255, 255), FILLED);
128  putText(color_mat, ss.str(), center,
129  FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,0,0));
130  }
131  }
132 
133  imshow(window_name, color_mat);
134  if (waitKey(1) >= 0) break;
135  }
136 
137  return EXIT_SUCCESS;
138 }
139 catch (const rs2::error & e)
140 {
141  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
142  return EXIT_FAILURE;
143 }
144 catch (const std::exception& e)
145 {
146  std::cerr << e.what() << std::endl;
147  return EXIT_FAILURE;
148 }
frameset wait_for_frames(unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
int main(int argc, char **argv)
Definition: rs-dnn.cpp:21
const GLfloat * m
Definition: glext.h:6814
const float inScaleFactor
Definition: rs-dnn.cpp:12
Definition: cah-model.h:10
const size_t inHeight
Definition: rs-dnn.cpp:10
std::array< point3d, 4 > object
e
Definition: rmse.py:177
::std_msgs::String_< std::allocator< void > > String
Definition: String.h:47
static cv::Mat depth_frame_to_meters(const rs2::depth_frame &f)
Definition: cv-helpers.hpp:48
const float meanVal
Definition: rs-dnn.cpp:13
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
const size_t inWidth
Definition: rs-dnn.cpp:9
std::ostream & cerr()
const float WHRatio
Definition: rs-dnn.cpp:11
int i
pipeline_profile start()
static cv::Mat frame_to_mat(const rs2::frame &f)
Definition: cv-helpers.hpp:11
const char * classNames[]
Definition: rs-dnn.cpp:14
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