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