Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
readRealSenseData.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 *****************************************************************************/
32 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 
43 #if defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
44 #include <condition_variable>
45 #include <fstream>
46 #include <mutex>
47 #include <queue>
48 #include <thread>
49 
50 #ifdef VISP_HAVE_PCL
51 #include <pcl/common/common.h>
52 #include <pcl/io/pcd_io.h>
53 #include <pcl/visualization/cloud_viewer.h>
54 
55 #define USE_PCL_VIEWER
56 #endif
57 
58 #include <visp3/core/vpImageConvert.h>
59 #include <visp3/core/vpIoTools.h>
60 #include <visp3/gui/vpDisplayGDI.h>
61 #include <visp3/gui/vpDisplayX.h>
62 #include <visp3/io/vpImageIo.h>
63 #include <visp3/io/vpParseArgv.h>
64 #include <visp3/io/vpVideoWriter.h>
65 
66 #define GETOPTARGS "ci:bodh"
67 
68 namespace
69 {
70 
71 void usage(const char *name, const char *badparam)
72 {
73  fprintf(stdout, "\n\
74  Read RealSense data.\n\
75  \n\
76  %s\
77  OPTIONS: \n\
78  -i <directory> \n\
79  Input directory.\n\
80  \n\
81  -c \n\
82  Click enable.\n\
83  \n\
84  -b \n\
85  Pointcloud is in binary format.\n\
86  \n\
87  -o \n\
88  Save color and depth side by side to image sequence.\n\
89  \n\
90  -d \n\
91  Display depth in color.\n\
92  \n\
93  -h \n\
94  Print the help.\n\n",
95  name);
96 
97  if (badparam)
98  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
99 }
100 
101 bool getOptions(int argc, char **argv, std::string &input_directory, bool &click, bool &pointcloud_binary_format,
102  bool &save_video, bool &color_depth)
103 {
104  const char *optarg;
105  const char **argv1 = (const char **)argv;
106  int c;
107  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
108 
109  switch (c) {
110  case 'i':
111  input_directory = optarg;
112  break;
113  case 'c':
114  click = true;
115  break;
116  case 'b':
117  pointcloud_binary_format = true;
118  break;
119  case 'o':
120  save_video = true;
121  break;
122  case 'd':
123  color_depth = true;
124  break;
125 
126  case 'h':
127  usage(argv[0], nullptr);
128  return false;
129  break;
130 
131  default:
132  usage(argv[0], optarg);
133  return false;
134  break;
135  }
136  }
137 
138  if ((c == 1) || (c == -1)) {
139  // standalone param or error
140  usage(argv[0], nullptr);
141  std::cerr << "ERROR: " << std::endl;
142  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
143  return false;
144  }
145 
146  return true;
147 }
148 
149 #ifdef USE_PCL_VIEWER
150 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
151 bool cancelled = false, update_pointcloud = false;
152 
153 class ViewerWorker
154 {
155 public:
156  explicit ViewerWorker(std::mutex &mutex) : m_mutex(mutex) { }
157 
158  void run()
159  {
160  pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
161 
162  bool local_update = false, local_cancelled = false;
163  pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
164  viewer->setBackgroundColor(0, 0, 0);
165  viewer->initCameraParameters();
166  viewer->setPosition(640 + 80, 480 + 80);
167  viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
168  viewer->setSize(640, 480);
169 
170  bool first_init = true;
171  while (!local_cancelled) {
172  {
173  std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
174 
175  if (lock.owns_lock()) {
176  local_update = update_pointcloud;
177  update_pointcloud = false;
178  local_cancelled = cancelled;
179  local_pointcloud = pointcloud->makeShared();
180  }
181  }
182 
183  if (local_update && !local_cancelled) {
184  local_update = false;
185 
186  if (first_init) {
187  viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
188  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
189  first_init = false;
190  }
191  else {
192  viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
193  }
194  }
195 
196  viewer->spinOnce(10);
197  }
198 
199  std::cout << "End of point cloud display thread" << std::endl;
200  }
201 
202 private:
203  std::mutex &m_mutex;
204 };
205 #endif
206 
207 bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color, vpImage<uint16_t> &I_depth_raw,
208  bool pointcloud_binary_format
209 #ifdef USE_PCL_VIEWER
210  ,
211  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
212 #endif
213 )
214 {
215  char buffer[FILENAME_MAX];
216  std::stringstream ss;
217  ss << input_directory << "/color_image_%04d.jpg";
218  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
219  std::string filename_color = buffer;
220 
221  ss.str("");
222  ss << input_directory << "/depth_image_%04d.bin";
223  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
224  std::string filename_depth = buffer;
225 
226  ss.str("");
227  ss << input_directory << "/point_cloud_%04d" << (pointcloud_binary_format ? ".bin" : ".pcd");
228  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
229  std::string filename_pointcloud = buffer;
230 
231  if (!vpIoTools::checkFilename(filename_color) && !vpIoTools::checkFilename(filename_depth) &&
232  !vpIoTools::checkFilename(filename_pointcloud)) {
233  std::cerr << "End of sequence." << std::endl;
234  return false;
235  }
236 
237  // Read color
238  if (vpIoTools::checkFilename(filename_color))
239  vpImageIo::read(I_color, filename_color);
240 
241  // Read raw depth
242  std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
243  if (file_depth.is_open()) {
244  unsigned int height = 0, width = 0;
245  vpIoTools::readBinaryValueLE(file_depth, height);
246  vpIoTools::readBinaryValueLE(file_depth, width);
247  I_depth_raw.resize(height, width);
248 
249  uint16_t depth_value = 0;
250  for (unsigned int i = 0; i < height; i++) {
251  for (unsigned int j = 0; j < width; j++) {
252  vpIoTools::readBinaryValueLE(file_depth, depth_value);
253  I_depth_raw[i][j] = depth_value;
254  }
255  }
256  }
257 
258  // Read pointcloud
259 #ifdef USE_PCL_VIEWER
260  if (pointcloud_binary_format) {
261  std::ifstream file_pointcloud(filename_pointcloud.c_str(), std::ios::in | std::ios::binary);
262  if (!file_pointcloud.is_open()) {
263  std::cerr << "Cannot read pointcloud file: " << filename_pointcloud << std::endl;
264  }
265 
266  uint32_t height = 0, width = 0;
267  char is_dense = 1;
268  vpIoTools::readBinaryValueLE(file_pointcloud, height);
269  vpIoTools::readBinaryValueLE(file_pointcloud, width);
270  file_pointcloud.read((char *)(&is_dense), sizeof(is_dense));
271 
272  point_cloud->width = width;
273  point_cloud->height = height;
274  point_cloud->is_dense = (is_dense != 0);
275  point_cloud->resize((size_t)width * height);
276 
277  float x = 0.0f, y = 0.0f, z = 0.0f;
278  for (uint32_t i = 0; i < height; i++) {
279  for (uint32_t j = 0; j < width; j++) {
280  vpIoTools::readBinaryValueLE(file_pointcloud, x);
281  vpIoTools::readBinaryValueLE(file_pointcloud, y);
282  vpIoTools::readBinaryValueLE(file_pointcloud, z);
283 
284  point_cloud->points[(size_t)(i * width + j)].x = x;
285  point_cloud->points[(size_t)(i * width + j)].y = y;
286  point_cloud->points[(size_t)(i * width + j)].z = z;
287  }
288  }
289  }
290  else {
291  if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
292  std::cerr << "Cannot read PCD: " << filename_pointcloud << std::endl;
293  }
294  }
295 #endif
296 
297  return true;
298 }
299 } // Namespace
300 
301 int main(int argc, char *argv[])
302 {
303  std::string input_directory = "";
304  bool click = false;
305  bool pointcloud_binary_format = false;
306  bool save_video = false;
307  bool color_depth = false;
308 
309  // Read the command line options
310  if (!getOptions(argc, argv, input_directory, click, pointcloud_binary_format, save_video, color_depth)) {
311  return EXIT_FAILURE;
312  }
313 
314  vpImage<vpRGBa> I_color(480, 640), I_depth_color(480, 640);
315  vpImage<uint16_t> I_depth_raw(480, 640);
316  vpImage<unsigned char> I_depth(480, 640);
317 
318 #ifdef VISP_HAVE_X11
319  vpDisplayX d1, d2;
320 #else
321  vpDisplayGDI d1, d2;
322 #endif
323  bool init_display = false;
324 
325 #ifdef USE_PCL_VIEWER
326  std::mutex mutex;
327  ViewerWorker viewer(mutex);
328  std::thread viewer_thread(&ViewerWorker::run, &viewer);
329 #endif
330 
331  vpVideoWriter writer;
332  vpImage<vpRGBa> O;
333  if (save_video) {
334  std::string output_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
335  vpIoTools::makeDirectory(output_directory);
336  writer.setFileName(output_directory + "/%04d.png");
337  }
338 
339  int cpt_frame = 0;
340  bool quit = false;
341  while (!quit) {
342  double t = vpTime::measureTimeMs();
343 
344 #ifdef USE_PCL_VIEWER
345  {
346  std::lock_guard<std::mutex> lock(mutex);
347  update_pointcloud = true;
348  quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud);
349  }
350 #else
351  quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format);
352 #endif
353 
354  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
355  if (color_depth)
356  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_color);
357 
358  if (!init_display) {
359  init_display = true;
360  d1.init(I_color, 0, 0, "Color image");
361  if (color_depth)
362  d2.init(I_depth_color, I_color.getWidth() + 10, 0, "Depth image");
363  else
364  d2.init(I_depth, I_color.getWidth() + 10, 0, "Depth image");
365  }
366 
367  vpDisplay::display(I_color);
368  if (color_depth)
369  vpDisplay::display(I_depth_color);
370  else
371  vpDisplay::display(I_depth);
372 
373  std::stringstream ss;
374  ss << "Frame: " << cpt_frame;
375  vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
376  if (color_depth)
377  vpDisplay::displayText(I_depth_color, 20, 20, ss.str(), vpColor::red);
378  else
379  vpDisplay::displayText(I_depth, 20, 20, ss.str(), vpColor::red);
380 
381  vpDisplay::flush(I_color);
382  if (color_depth)
383  vpDisplay::flush(I_depth_color);
384  else
385  vpDisplay::flush(I_depth);
386 
387  if (save_video) {
388  if (O.getSize() == 0) {
389  O.resize(I_color.getHeight(), I_color.getWidth() + I_depth_color.getWidth());
390  writer.open(O);
391  }
392 
393  O.insert(I_color, vpImagePoint());
394  if (!color_depth)
395  vpImageConvert::convert(I_depth, I_depth_color);
396  O.insert(I_depth_color, vpImagePoint(0, I_color.getWidth()));
397  writer.saveFrame(O);
398  }
399 
401  if (vpDisplay::getClick(I_color, button, click)) {
402  switch (button) {
404  if (!quit)
405  quit = !click;
406  break;
407 
409  click = !click;
410  break;
411 
412  default:
413  break;
414  }
415  }
416 
417  vpTime::wait(t, 30);
418  cpt_frame++;
419  }
420 
421 #ifdef USE_PCL_VIEWER
422  {
423  std::lock_guard<std::mutex> lock(mutex);
424  cancelled = true;
425  }
426  viewer_thread.join();
427 #endif
428 
429  return EXIT_SUCCESS;
430 }
431 #else
432 int main()
433 {
434  std::cerr << "Enable C++11 or higher (cmake -DUSE_CXX_STANDARD=11) and install X11 or GDI!" << std::endl;
435  return EXIT_SUCCESS;
436 }
437 #endif
static const vpColor red
Definition: vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
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 void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:143
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:780
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1336
unsigned int getSize() const
Definition: vpImage.h:223
unsigned int getHeight() const
Definition: vpImage.h:184
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:1204
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:2470
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:972
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that enables to write easily a video file or a sequence of images.
void saveFrame(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void open(vpImage< vpRGBa > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()