Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRealSense2_D435_opencv.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test Intel RealSense acquisition with librealsense2 (OpenCV demo).
33 *
34*****************************************************************************/
40#include <iostream>
41
42#include <visp3/core/vpConfig.h>
43
44#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
45 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
46
47#include <visp3/core/vpImage.h>
48#include <visp3/core/vpImageConvert.h>
49#include <visp3/core/vpMeterPixelConversion.h>
50#include <visp3/sensor/vpRealSense2.h>
51
52#include <opencv2/core.hpp>
53#include <opencv2/highgui.hpp>
54
55namespace
56{
57struct float3 {
58 float x, y, z;
59 float3() : x(0), y(0), z(0) {}
60 float3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
61};
62
63void getPointcloud(const rs2::depth_frame &depth_frame, std::vector<float3> &pointcloud)
64{
65 auto vf = depth_frame.as<rs2::video_frame>();
66 const int width = vf.get_width();
67 const int height = vf.get_height();
68 pointcloud.resize((size_t)(width * height));
69
70 rs2::pointcloud pc;
71 rs2::points points = pc.calculate(depth_frame);
72 auto vertices = points.get_vertices();
73 for (size_t i = 0; i < points.size(); i++) {
74 float3 pcl;
75 if (vertices[i].z > std::numeric_limits<float>::epsilon()) {
76 pcl.x = vertices[i].x;
77 pcl.y = vertices[i].y;
78 pcl.z = vertices[i].z;
79 }
80
81 pointcloud[i] = pcl;
82 }
83}
84
85void createDepthHist(std::vector<uint32_t> &histogram, const std::vector<float3> &pointcloud, float depth_scale)
86{
87 std::fill(histogram.begin(), histogram.end(), 0);
88
89 for (size_t i = 0; i < pointcloud.size(); i++) {
90 const float3 &pt = pointcloud[i];
91 ++histogram[static_cast<uint32_t>(pt.z * depth_scale)];
92 }
93
94 for (int i = 2; i < 0x10000; i++)
95 histogram[i] += histogram[i - 1]; // Build a cumulative histogram for
96 // the indices in [1,0xFFFF]
97}
98
99unsigned char getDepthColor(const std::vector<uint32_t> &histogram, float z, float depth_scale)
100{
101 // 0-255 based on histogram location
102 return static_cast<unsigned char>(histogram[static_cast<uint32_t>(z * depth_scale)] * 255 / histogram[0xFFFF]);
103}
104
105void getNativeFrame(const rs2::frame &frame, unsigned char *const data)
106{
107 auto vf = frame.as<rs2::video_frame>();
108 int size = vf.get_width() * vf.get_height();
109
110 switch (frame.get_profile().format()) {
111 case RS2_FORMAT_RGB8:
112 case RS2_FORMAT_BGR8:
113 memcpy(data, frame.get_data(), size * 3);
114 break;
115
116 case RS2_FORMAT_RGBA8:
117 case RS2_FORMAT_BGRA8:
118 memcpy(data, frame.get_data(), size * 4);
119 break;
120
121 case RS2_FORMAT_Y16:
122 case RS2_FORMAT_Z16:
123 memcpy(data, frame.get_data(), size * 2);
124 break;
125
126 case RS2_FORMAT_Y8:
127 memcpy(data, frame.get_data(), size);
128 break;
129
130 default:
131 break;
132 }
133}
134
135void frame_to_mat(const rs2::frame &f, cv::Mat &img)
136{
137 auto vf = f.as<rs2::video_frame>();
138 const int w = vf.get_width();
139 const int h = vf.get_height();
140 const int size = w * h;
141
142 if (f.get_profile().format() == RS2_FORMAT_BGR8) {
143 memcpy(static_cast<void *>(img.ptr<cv::Vec3b>()), f.get_data(), size * 3);
144 } else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
145 cv::Mat tmp(h, w, CV_8UC3, const_cast<void *>(f.get_data()), cv::Mat::AUTO_STEP);
146 cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
147 } else if (f.get_profile().format() == RS2_FORMAT_Y8) {
148 memcpy(img.ptr<uchar>(), f.get_data(), size);
149 }
150}
151} // namespace
152
153int main()
154{
155 const int width = 640, height = 480, fps = 60;
156 vpRealSense2 rs;
157 rs2::config config;
158 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);
159 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
160 config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, fps);
161 config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, fps);
162 rs.open(config);
163
164 rs2::pipeline_profile &profile = rs.getPipelineProfile();
165 rs2::pipeline &pipe = rs.getPipeline();
166 float depth_scale = 1 / rs.getDepthScale();
167
168 // initialize the image sizes
169 // width and height can also be used instead
170 auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
171 cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
172
173 auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
174 cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3);
175 rs2::colorizer color_map;
176
177 auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
178 cv::Mat mat_infrared1(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
179 cv::Mat mat_infrared2(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
180
181 std::vector<float3> pointcloud;
182 cv::Mat mat_pointcloud(depth_profile.height(), depth_profile.width(), CV_8UC1);
183 std::vector<uint32_t> histogram(0x10000);
184
185 vpCameraParameters cam_projection = rs.getCameraParameters(RS2_STREAM_DEPTH);
186
187 std::vector<double> time_vector;
188 vpChrono chrono;
189 while (true) {
190 chrono.start();
191
192 auto data = pipe.wait_for_frames();
193 frame_to_mat(data.get_color_frame(), mat_color);
194#if (RS2_API_VERSION >= ((2 * 10000) + (16 * 100) + 0))
195 frame_to_mat(data.get_depth_frame().apply_filter(color_map), mat_depth);
196#else
197 frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
198#endif
199
200 cv::imshow("OpenCV color", mat_color);
201 cv::imshow("OpenCV depth", mat_depth);
202
203#if (RS2_API_VERSION >= ((2 * 10000) + (10 * 100) + 0))
204 // rs2::frameset::get_infrared_frame() introduced in librealsense 2.10.0
205 frame_to_mat(data.get_infrared_frame(1), mat_infrared1);
206 frame_to_mat(data.get_infrared_frame(2), mat_infrared2);
207
208 cv::imshow("OpenCV infrared left", mat_infrared1);
209 cv::imshow("OpenCV infrared right", mat_infrared2);
210#endif
211
212 getPointcloud(data.get_depth_frame(), pointcloud);
213 createDepthHist(histogram, pointcloud, depth_scale);
214
215 mat_pointcloud = 0;
216 for (size_t i = 0; i < pointcloud.size(); i++) {
217 const float3 &pt = pointcloud[i];
218 float Z = pt.z;
219 if (Z > 1e-2) {
220 double x = pt.x / Z;
221 double y = pt.y / Z;
222
223 vpImagePoint imPt;
224 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
225 int u = std::min(static_cast<int>(width - 1), static_cast<int>(std::max(0.0, imPt.get_u())));
226 int v = std::min(static_cast<int>(height - 1), static_cast<int>(std::max(0.0, imPt.get_v())));
227 unsigned char depth_viz = getDepthColor(histogram, Z, depth_scale);
228 mat_pointcloud.at<uchar>(v, u) = depth_viz;
229 }
230 }
231 cv::imshow("OpenCV projected pointcloud", mat_pointcloud);
232
233 chrono.stop();
234 time_vector.push_back(chrono.getDurationMs());
235 if (cv::waitKey(5) == 27) {
236 break;
237 }
238 }
239
240 std::cout << "Acquisition - Mean time: " << vpMath::getMean(time_vector)
241 << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
242
243 return EXIT_SUCCESS;
244}
245#else
246int main()
247{
248#if !defined(VISP_HAVE_REALSENSE2)
249 std::cout << "Install librealsense2 to make this test work." << std::endl;
250#endif
251#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
252 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11) "
253 "to make this test work"
254 << std::endl;
255#endif
256#if !(VISP_HAVE_OPENCV_VERSION >= 0x030000)
257 std::cout << "Install OpenCV version >= 3 to make this test work." << std::endl;
258#endif
259 return EXIT_SUCCESS;
260}
261#endif
Generic class defining intrinsic camera parameters.
void start(bool reset=true)
Definition vpTime.cpp:397
void stop()
Definition vpTime.cpp:412
double getDurationMs()
Definition vpTime.cpp:386
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_u() const
double get_v() const
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.
bool open(const rs2::config &cfg=rs2::config())
float getDepthScale()
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.