Visual Servoing Platform version 3.5.0
testRealSense2_D435.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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.
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))
46
47#include <visp3/core/vpImage.h>
48#include <visp3/core/vpImageConvert.h>
49#include <visp3/gui/vpDisplayGDI.h>
50#include <visp3/gui/vpDisplayX.h>
51#include <visp3/sensor/vpRealSense2.h>
52
53int main(int argc, char *argv[])
54{
55 bool show_info = false;
56
57 for (int i = 1; i < argc; i++) {
58 if (std::string(argv[i]) == "--show_info") {
59 show_info = true;
60 }
61 }
62
63 if (show_info) {
64 vpRealSense2 rs;
65 rs.open();
66 std::cout << "RealSense:\n" << rs << std::endl;
67 return EXIT_SUCCESS;
68 }
69
70 int width = 1280, height = 720, fps = 30;
71 vpRealSense2 rs;
72 rs2::config config;
73 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
74 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
75 config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, fps);
76 config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, fps);
77 rs.open(config);
78
79 vpImage<vpRGBa> color(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
80 vpImage<vpRGBa> depth_color(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
81 vpImage<uint16_t> depth_raw(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
82 vpImage<unsigned char> infrared1(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
83 vpImage<unsigned char> infrared2(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
84
85#ifdef VISP_HAVE_X11
86 vpDisplayX d1, d2, d3, d4;
87#else
88 vpDisplayGDI d1, d2, d3, d4;
89#endif
90 d1.init(color, 0, 0, "Color");
91 d2.init(depth_color, color.getWidth(), 0, "Depth");
92 d3.init(infrared1, 0, color.getHeight() + 100, "Infrared left");
93 d4.init(infrared2, color.getWidth(), color.getHeight() + 100, "Infrared right");
94
95 std::vector<vpColVector> pointcloud_colvector;
96
97 std::vector<double> time_vector;
98 double t_begin = vpTime::measureTimeMs();
99 while (vpTime::measureTimeMs() - t_begin < 10000) {
100 double t = vpTime::measureTimeMs();
101
102 rs.acquire(reinterpret_cast<unsigned char *>(color.bitmap),
103 reinterpret_cast<unsigned char *>(depth_raw.bitmap),
104 &pointcloud_colvector,
105 reinterpret_cast<unsigned char *>(infrared1.bitmap),
106 reinterpret_cast<unsigned char *>(infrared2.bitmap),
107 NULL);
108
109 vpImageConvert::createDepthHistogram(depth_raw, depth_color);
110
111 vpDisplay::display(color);
112 vpDisplay::display(depth_color);
113 vpDisplay::display(infrared1);
114 vpDisplay::display(infrared2);
115
116 vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
117 vpDisplay::displayText(depth_color, 20, 20, "Click to quit.", vpColor::red);
118 vpDisplay::displayText(infrared1, 20, 20, "Click to quit.", vpColor::red);
119 vpDisplay::displayText(infrared2, 20, 20, "Click to quit.", vpColor::red);
120
121 vpDisplay::flush(color);
122 vpDisplay::flush(depth_color);
123 vpDisplay::flush(infrared1);
124 vpDisplay::flush(infrared2);
125
126 time_vector.push_back(vpTime::measureTimeMs() - t);
127 if (vpDisplay::getClick(color, false) ||
128 vpDisplay::getClick(depth_color, false) ||
129 vpDisplay::getClick(infrared1, false) ||
130 vpDisplay::getClick(infrared2, false)) {
131 break;
132 }
133 }
134
135 // test open -> close -> open sequence
136 rs.close();
137 d1.close(color);
138 d2.close(depth_color);
139 d3.close(infrared1);
140 d4.close(infrared2);
141
142 std::cout << "Acquisition1 - Mean time: " << vpMath::getMean(time_vector)
143 << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
144
145 width = 640;
146 height = 480;
147 fps = 60;
148 config.disable_all_streams();
149 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
150 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
151 config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, fps);
152 rs.open(config);
153
154 color.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
155 depth_color.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
156 depth_raw.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
157 infrared1.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
158
159 d1.init(color, 0, 0, "Color");
160 d2.init(depth_color, color.getWidth(), 0, "Depth");
161 d3.init(infrared1, 0, color.getHeight() + 100, "Infrared");
162
163 time_vector.clear();
164 t_begin = vpTime::measureTimeMs();
165 while (vpTime::measureTimeMs() - t_begin < 10000) {
166 double t = vpTime::measureTimeMs();
167
168 rs.acquire(reinterpret_cast<unsigned char *>(color.bitmap),
169 reinterpret_cast<unsigned char *>(depth_raw.bitmap),
170 NULL,
171 reinterpret_cast<unsigned char *>(infrared1.bitmap));
172
173 vpImageConvert::createDepthHistogram(depth_raw, depth_color);
174
175 vpDisplay::display(color);
176 vpDisplay::display(depth_color);
177 vpDisplay::display(infrared1);
178
179 vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
180 vpDisplay::displayText(depth_color, 20, 20, "Click to quit.", vpColor::red);
181 vpDisplay::displayText(infrared1, 20, 20, "Click to quit.", vpColor::red);
182
183 vpDisplay::flush(color);
184 vpDisplay::flush(depth_color);
185 vpDisplay::flush(infrared1);
186
187 time_vector.push_back(vpTime::measureTimeMs() - t);
188 if (vpDisplay::getClick(color, false) ||
189 vpDisplay::getClick(depth_color, false) ||
190 vpDisplay::getClick(infrared1, false)) {
191 break;
192 }
193 }
194
195 std::cout << "Acquisition2 - Mean time: " << vpMath::getMean(time_vector)
196 << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
197
198 return EXIT_SUCCESS;
199}
200
201#else
202int main()
203{
204#if !defined(VISP_HAVE_REALSENSE2)
205 std::cout << "Install librealsense2 to make this test work." << std::endl;
206#endif
207#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
208 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11) "
209 "to make this test work"
210 << std::endl;
211#endif
212#if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
213 std::cout << "X11 or GDI are needed." << std::endl;
214#endif
215 return 0;
216}
217#endif
static const vpColor red
Definition: vpColor.h:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void close(vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()