Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
grabRealSense.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 RealSense RGB-D sensor.
33 *
34*****************************************************************************/
35
42#include <iostream>
43
44#include <visp3/core/vpImageConvert.h>
45#include <visp3/core/vpMutex.h>
46#include <visp3/core/vpThread.h>
47#include <visp3/gui/vpDisplayGDI.h>
48#include <visp3/gui/vpDisplayX.h>
49#include <visp3/sensor/vpRealSense.h>
50
51#if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
52
53// Using a thread to display the pointcloud with PCL produces a segfault on
54// OSX
55#if (!defined(__APPLE__) && !defined(__MACH__)) // Not OSX
56#if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
57#define USE_THREAD
58#endif
59#endif
60
61#ifdef VISP_HAVE_PCL
62#include <pcl/visualization/cloud_viewer.h>
63#include <pcl/visualization/pcl_visualizer.h>
64#endif
65
66#ifdef VISP_HAVE_PCL
67#ifdef USE_THREAD
68// Shared vars
69typedef enum { capture_waiting, capture_started, capture_stopped } t_CaptureState;
70t_CaptureState s_capture_state = capture_waiting;
71vpMutex s_mutex_capture;
72
73vpThread::Return displayPointcloudFunction(vpThread::Args args)
74{
75 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *)args);
76
77 pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
78 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
79 viewer->setBackgroundColor(0, 0, 0);
80 viewer->initCameraParameters();
81 viewer->setPosition(640 + 80, 480 + 80);
82 viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
83 viewer->setSize(640, 480);
84
85 t_CaptureState capture_state_;
86
87 do {
88 s_mutex_capture.lock();
89 capture_state_ = s_capture_state;
90 s_mutex_capture.unlock();
91
92 if (capture_state_ == capture_started) {
93 static bool update = false;
94 if (!update) {
95 viewer->addPointCloud<pcl::PointXYZRGB>(pointcloud_, rgb, "sample cloud");
96 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
97 update = true;
98 }
99 else {
100 viewer->updatePointCloud<pcl::PointXYZRGB>(pointcloud_, rgb, "sample cloud");
101 }
102
103 viewer->spinOnce(10);
104 }
105 } while (capture_state_ != capture_stopped);
106
107 std::cout << "End of point cloud display thread" << std::endl;
108 return EXIT_SUCCESS;
109}
110#endif
111#endif
112#endif
113
114int main()
115{
116#if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
117 try {
118 vpRealSense rs;
119 rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30));
120 rs.open();
121
123 << std::endl;
125 << std::endl;
126 std::cout << "Extrinsics cMd: \n" << rs.getTransformation(rs::stream::color, rs::stream::depth) << std::endl;
127 std::cout << "Extrinsics dMc: \n" << rs.getTransformation(rs::stream::depth, rs::stream::color) << std::endl;
128 std::cout << "Extrinsics cMi: \n" << rs.getTransformation(rs::stream::color, rs::stream::infrared) << std::endl;
129 std::cout << "Extrinsics dMi: \n" << rs.getTransformation(rs::stream::depth, rs::stream::infrared) << std::endl;
130
131 vpImage<vpRGBa> color((unsigned int)rs.getIntrinsics(rs::stream::color).height,
132 (unsigned int)rs.getIntrinsics(rs::stream::color).width);
133
134 vpImage<unsigned char> infrared_display((unsigned int)rs.getIntrinsics(rs::stream::infrared).height,
135 (unsigned int)rs.getIntrinsics(rs::stream::infrared).width);
136 vpImage<uint16_t> infrared_y16;
137 rs::device *dev = rs.getHandler();
138 bool use_infrared_y16 = dev->get_stream_format(rs::stream::infrared) == rs::format::y16;
139
140 vpImage<vpRGBa> depth_display((unsigned int)rs.getIntrinsics(rs::stream::depth).height,
141 (unsigned int)rs.getIntrinsics(rs::stream::depth).width);
142 vpImage<uint16_t> depth(depth_display.getHeight(), depth_display.getWidth());
143
144#ifdef VISP_HAVE_PCL
145 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
146
147#ifdef USE_THREAD
148 vpThread thread_display_pointcloud(displayPointcloudFunction, (vpThread::Args)&pointcloud);
149#else
150 pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
151 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
152 viewer->setBackgroundColor(0, 0, 0);
153 viewer->addCoordinateSystem(1.0);
154 viewer->initCameraParameters();
155 viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
156#endif
157
158#else
159 std::vector<vpColVector> pointcloud;
160#endif
161
162#if defined(VISP_HAVE_X11)
163 vpDisplayX dc(color, 10, 10, "Color image");
164 vpDisplayX di(infrared_display, (int)color.getWidth() + 80, 10, "Infrared image");
165 vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 80, "Depth image");
166#elif defined(VISP_HAVE_GDI)
167 vpDisplayGDI dc(color, 10, 10, "Color image");
168 vpDisplayGDI di(infrared_display, color.getWidth() + 80, 10, "Infrared image");
169 vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80, "Depth image");
170#else
171 std::cout << "No image viewer is available..." << std::endl;
172#endif
173
174 while (1) {
175 double t = vpTime::measureTimeMs();
176
177 if (use_infrared_y16) {
178 rs.acquire(color, infrared_y16, depth, pointcloud);
179 vpImageConvert::convert(infrared_y16, infrared_display);
180 }
181 else {
182#ifdef VISP_HAVE_PCL
183 rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL, pointcloud,
184 (unsigned char *)infrared_display.bitmap);
185#else
186 rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, NULL,
187 (unsigned char *)infrared_display.bitmap);
188#endif
189 }
190
191#ifdef VISP_HAVE_PCL
192#ifdef USE_THREAD
193 {
194 vpMutex::vpScopedLock lock(s_mutex_capture);
195 s_capture_state = capture_started;
196 }
197#else
198 static bool update = false;
199 if (!update) {
200 viewer->addPointCloud<pcl::PointXYZRGB>(pointcloud, rgb, "sample cloud");
201 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
202 viewer->setPosition(color.getWidth() + 80, color.getHeight() + 80);
203 update = true;
204 }
205 else {
206 viewer->updatePointCloud<pcl::PointXYZRGB>(pointcloud, rgb, "sample cloud");
207 }
208
209 viewer->spinOnce(10);
210#endif
211#endif
212
213 vpImageConvert::createDepthHistogram(depth, depth_display);
214
215 vpDisplay::display(color);
216 vpDisplay::display(infrared_display);
217 vpDisplay::display(depth_display);
218
219 vpDisplay::displayText(color, 15, 15, "Click to quit", vpColor::red);
220 if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared_display, false) ||
221 vpDisplay::getClick(depth_display, false)) {
222 break;
223 }
224 vpDisplay::flush(color);
225 vpDisplay::flush(infrared_display);
226 vpDisplay::flush(depth_display);
227
228 std::cout << "Loop time: " << vpTime::measureTimeMs() - t << std::endl;
229 }
230
231 std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
232
233#ifdef VISP_HAVE_PCL
234#ifdef USE_THREAD
235 {
236 vpMutex::vpScopedLock lock(s_mutex_capture);
237 s_capture_state = capture_stopped;
238 }
239#endif
240#endif
241
242 rs.close();
243 }
244 catch (const vpException &e) {
245 std::cerr << "RealSense error " << e.what() << std::endl;
246 }
247 catch (const rs::error &e) {
248 std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args()
249 << "): " << e.what() << std::endl;
250 }
251 catch (const std::exception &e) {
252 std::cerr << e.what() << std::endl;
253 }
254
255#elif !defined(VISP_HAVE_REALSENSE)
256 std::cout << "This deprecated example is only working with librealsense 1.x" << std::endl;
257#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
258 std::cout << "You do not build ViSP with c++11 or higher compiler flag" << std::endl;
259 std::cout << "Tip:" << std::endl;
260 std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
261 << std::endl;
262#endif
263 return EXIT_SUCCESS;
264}
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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
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)
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * what() const
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Class that allows protection by mutex.
Definition vpMutex.h:166
void unlock()
Definition vpMutex.h:106
void lock()
Definition vpMutex.h:90
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
rs::device * getHandler() const
Get access to device handler.
void acquire(std::vector< vpColVector > &pointcloud)
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
rs::intrinsics getIntrinsics(const rs::stream &stream) const
void * Args
Definition vpThread.h:72
void * Return
Definition vpThread.h:73
VISP_EXPORT double measureTimeMs()