Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRealSense2_D435.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.
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), reinterpret_cast<unsigned char *>(depth_raw.bitmap),
103 &pointcloud_colvector, reinterpret_cast<unsigned char *>(infrared1.bitmap),
104 reinterpret_cast<unsigned char *>(infrared2.bitmap), NULL);
105
106 vpImageConvert::createDepthHistogram(depth_raw, depth_color);
107
108 vpDisplay::display(color);
109 vpDisplay::display(depth_color);
110 vpDisplay::display(infrared1);
111 vpDisplay::display(infrared2);
112
113 vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
114 vpDisplay::displayText(depth_color, 20, 20, "Click to quit.", vpColor::red);
115 vpDisplay::displayText(infrared1, 20, 20, "Click to quit.", vpColor::red);
116 vpDisplay::displayText(infrared2, 20, 20, "Click to quit.", vpColor::red);
117
118 vpDisplay::flush(color);
119 vpDisplay::flush(depth_color);
120 vpDisplay::flush(infrared1);
121 vpDisplay::flush(infrared2);
122
123 time_vector.push_back(vpTime::measureTimeMs() - t);
124 if (vpDisplay::getClick(color, false) || vpDisplay::getClick(depth_color, false) ||
125 vpDisplay::getClick(infrared1, false) || vpDisplay::getClick(infrared2, false)) {
126 break;
127 }
128 }
129
130 // test open -> close -> open sequence
131 rs.close();
132 d1.close(color);
133 d2.close(depth_color);
134 d3.close(infrared1);
135 d4.close(infrared2);
136
137 std::cout << "Acquisition1 - Mean time: " << vpMath::getMean(time_vector)
138 << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
139
140 width = 640;
141 height = 480;
142 fps = 60;
143 config.disable_all_streams();
144 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
145 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
146 config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, fps);
147 rs.open(config);
148
149 color.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
150 depth_color.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
151 depth_raw.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
152 infrared1.init(static_cast<unsigned int>(height), static_cast<unsigned int>(width));
153
154 d1.init(color, 0, 0, "Color");
155 d2.init(depth_color, color.getWidth(), 0, "Depth");
156 d3.init(infrared1, 0, color.getHeight() + 100, "Infrared");
157
158 time_vector.clear();
159 t_begin = vpTime::measureTimeMs();
160 while (vpTime::measureTimeMs() - t_begin < 10000) {
161 double t = vpTime::measureTimeMs();
162
163 rs.acquire(reinterpret_cast<unsigned char *>(color.bitmap), reinterpret_cast<unsigned char *>(depth_raw.bitmap),
164 NULL, reinterpret_cast<unsigned char *>(infrared1.bitmap));
165
166 vpImageConvert::createDepthHistogram(depth_raw, depth_color);
167
168 vpDisplay::display(color);
169 vpDisplay::display(depth_color);
170 vpDisplay::display(infrared1);
171
172 vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
173 vpDisplay::displayText(depth_color, 20, 20, "Click to quit.", vpColor::red);
174 vpDisplay::displayText(infrared1, 20, 20, "Click to quit.", vpColor::red);
175
176 vpDisplay::flush(color);
177 vpDisplay::flush(depth_color);
178 vpDisplay::flush(infrared1);
179
180 time_vector.push_back(vpTime::measureTimeMs() - t);
181 if (vpDisplay::getClick(color, false) || vpDisplay::getClick(depth_color, false) ||
182 vpDisplay::getClick(infrared1, false)) {
183 break;
184 }
185 }
186
187 std::cout << "Acquisition2 - Mean time: " << vpMath::getMean(time_vector)
188 << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
189
190 return EXIT_SUCCESS;
191}
192
193#else
194int main()
195{
196#if !defined(VISP_HAVE_REALSENSE2)
197 std::cout << "Install librealsense2 to make this test work." << std::endl;
198#endif
199#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
200 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11) "
201 "to make this test work"
202 << std::endl;
203#endif
204#if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
205 std::cout << "X11 or GDI are needed." << std::endl;
206#endif
207 return EXIT_SUCCESS;
208}
209#endif
static const vpColor red
Definition vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()