Visual Servoing Platform  version 3.0.1
grabRealSense.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test RealSense RGB-D sensor.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
44 #include <iostream>
45 
46 #include <visp3/sensor/vpRealSense.h>
47 #include <visp3/gui/vpDisplayX.h>
48 #include <visp3/gui/vpDisplayGDI.h>
49 #include <visp3/io/vpImageIo.h>
50 #include <visp3/core/vpImageConvert.h>
51 #include <visp3/core/vpMutex.h>
52 #include <visp3/core/vpThread.h>
53 
54 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
55 
56 // Using a thread to display the pointcloud with PCL produces a segfault on OSX
57 #if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX
58 # if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
59 # define USE_THREAD
60 # endif
61 #endif
62 
63 #ifdef VISP_HAVE_PCL
64 # include <pcl/visualization/cloud_viewer.h>
65 # include <pcl/visualization/pcl_visualizer.h>
66 #endif
67 
68 #ifdef VISP_HAVE_PCL
69 #ifdef USE_THREAD
70 // Shared vars
71 typedef enum {
72  capture_waiting,
73  capture_started,
74  capture_stopped
75 } t_CaptureState;
76 t_CaptureState s_capture_state = capture_waiting;
77 vpMutex s_mutex_capture;
78 
79 
80 vpThread::Return displayPointcloudFunction(vpThread::Args args)
81 {
82  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
83 
84  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
85  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
86  viewer->setBackgroundColor (0, 0, 0);
87  viewer->addCoordinateSystem (1.0);
88  viewer->initCameraParameters ();
89  viewer->setPosition(640+80, 480+80);
90  viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
91  viewer->setSize(640, 480);
92 
93  t_CaptureState capture_state_;
94 
95  do {
96  s_mutex_capture.lock();
97  capture_state_ = s_capture_state;
98  s_mutex_capture.unlock();
99 
100  if (capture_state_ == capture_started) {
101  static bool update = false;
102  if (! update) {
103  viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
104  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
105  update = true;
106  }
107  else {
108  viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
109  }
110 
111  viewer->spinOnce (10);
112  }
113  } while(capture_state_ != capture_stopped);
114 
115  std::cout << "End of point cloud display thread" << std::endl;
116  return 0;
117 }
118 #endif
119 #endif
120 #endif
121 
122 int main()
123 {
124 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
125  try {
126  vpRealSense rs;
127  //rs.setDeviceBySerialNumber("541142003219");
128 
129  rs.open();
130 
131  std::cout << rs.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithoutDistortion) << std::endl;
132  std::cout << rs.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
133  std::cout << "Extrinsics cMd: \n" << rs.getTransformation(rs::stream::color, rs::stream::depth) << std::endl;
134  std::cout << "Extrinsics dMc: \n" << rs.getTransformation(rs::stream::depth, rs::stream::color) << std::endl;
135  std::cout << "Extrinsics cMi: \n" << rs.getTransformation(rs::stream::color, rs::stream::infrared) << std::endl;
136  std::cout << "Extrinsics dMi: \n" << rs.getTransformation(rs::stream::depth, rs::stream::infrared) << std::endl;
137 
138  vpImage<vpRGBa> color((unsigned int) rs.getIntrinsics(rs::stream::color).height, (unsigned int) rs.getIntrinsics(rs::stream::color).width);
139  vpImage<uint16_t> infrared;
140  vpImage<unsigned char> infrared_display((unsigned int) rs.getIntrinsics(rs::stream::infrared).height, (unsigned int) rs.getIntrinsics(rs::stream::infrared).width);;
141  vpImage<uint16_t> depth;
142  vpImage<vpRGBa> depth_display((unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width);
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  rs.acquire(color, infrared, depth, pointcloud);
177 
178 #ifdef VISP_HAVE_PCL
179 #ifdef USE_THREAD
180  {
181  vpMutex::vpScopedLock lock(s_mutex_capture);
182  s_capture_state = capture_started;
183  }
184 #else
185  static bool update = false;
186  if (! update) {
187  viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
188  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
189  viewer->setPosition(color.getWidth()+80, color.getHeight()+80);
190  update = true;
191  }
192  else {
193  viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
194  }
195 
196  viewer->spinOnce (10);
197 #endif
198 #endif
199 
200  vpImageConvert::convert(infrared, infrared_display);
201  vpImageConvert::createDepthHistogram(depth, depth_display);
202 
203  vpDisplay::display(color);
204  vpDisplay::display(infrared_display);
205  vpDisplay::display(depth_display);
206 
207  vpDisplay::displayText(color, 15, 15, "Click to quit", vpColor::red);
208  if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared_display, false) || vpDisplay::getClick(depth_display, false)) {
209  break;
210  }
211  vpDisplay::flush(color);
212  vpDisplay::flush(infrared_display);
213  vpDisplay::flush(depth_display);
214 
215  std::cout << "Loop time: " << vpTime::measureTimeMs() - t << std::endl;
216  }
217 
218  std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
219 
220 #ifdef VISP_HAVE_PCL
221 #ifdef USE_THREAD
222  {
223  vpMutex::vpScopedLock lock(s_mutex_capture);
224  s_capture_state = capture_stopped;
225  }
226 #endif
227 #endif
228 
229  rs.close();
230  }
231  catch(const vpException &e) {
232  std::cerr << "RealSense error " << e.getStringMessage() << std::endl;
233  }
234  catch(const rs::error & e) {
235  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "): " << e.what() << std::endl;
236  }
237  catch(const std::exception & e) {
238  std::cerr << e.what() << std::endl;
239  }
240 
241 #elif !defined(VISP_HAVE_REALSENSE)
242  std::cout << "Install RealSense SDK to make this test working" << std::endl;
243 #elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
244  std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) to make this test working" << std::endl;
245 #endif
246  return 0;
247 }
248 
Class that allows protection by mutex.
Definition: vpMutex.h:163
void lock()
Definition: vpMutex.h:90
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Perspective projection without distortion model.
void * Return
Definition: vpThread.h:36
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:106
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
error that can be emited by ViSP classes.
Definition: vpException.h:73
void acquire(std::vector< vpColVector > &pointcloud)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static const vpColor red
Definition: vpColor.h:163
void * Args
Definition: vpThread.h:35
static void display(const vpImage< unsigned char > &I)
Perspective projection with distortion model.
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)
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).