Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
vpRealSense.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  * RealSense SDK wrapper.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <iomanip>
40 #include <iostream>
41 
42 #include <visp3/core/vpImageConvert.h>
43 #include <visp3/sensor/vpRealSense.h>
44 
45 #if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
46 
47 #include "vpRealSense_impl.h"
48 
53  : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(),
54  m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
55 {
56  initStream();
57 }
58 
64 
66 {
67  // General default presets
68  // Color stream
69  m_useStreamPresets[rs::stream::color] = true;
70  m_streamPresets[rs::stream::color] = rs::preset::best_quality;
71  m_streamParams[rs::stream::color] = vpRsStreamParams(640, 480, rs::format::rgba8, 60);
72 
73  // Depth stream
74  m_useStreamPresets[rs::stream::depth] = true;
75  m_streamPresets[rs::stream::depth] = rs::preset::best_quality;
76  m_streamParams[rs::stream::depth] = vpRsStreamParams(640, 480, rs::format::z16, 60);
77 
78  // Infrared stream
79  m_useStreamPresets[rs::stream::infrared] = true;
80  m_streamPresets[rs::stream::infrared] = rs::preset::best_quality;
81  m_streamParams[rs::stream::infrared] = vpRsStreamParams(640, 480, rs::format::y16, 60);
82 
83  // Infrared stream 2
84  m_useStreamPresets[rs::stream::infrared2] = true;
85  m_streamPresets[rs::stream::infrared2] = rs::preset::best_quality;
86  m_streamParams[rs::stream::infrared2] = vpRsStreamParams(640, 480, rs::format::y16, 60);
87 
88  // Enable all streams
89  m_enableStreams[rs::stream::color] = true;
90  m_enableStreams[rs::stream::depth] = true;
91  m_enableStreams[rs::stream::infrared] = true;
92  m_enableStreams[rs::stream::infrared2] = true;
93 }
94 
99 {
100  m_num_devices = m_context.get_device_count();
101 
102  if (m_num_devices == 0)
103  throw vpException(vpException::fatalError, "RealSense Camera - No device detected.");
104 
105  std::vector<rs::device *> detected_devices;
106  rs::device *device;
107  for (int i = 0; i < m_num_devices; ++i) {
108  device = m_context.get_device(i);
109  std::string serial_no = device->get_serial();
110  if (serial_no.compare(m_serial_no) == 0) {
111  m_device = device;
112  }
113  detected_devices.push_back(device);
114  }
115 
116  // Exit with error if no serial number is specified and multiple cameras are
117  // detected.
118  if ((m_serial_no.empty()) && (m_num_devices > 1)) {
120  "RealSense Camera - Multiple cameras detected (%d) but "
121  "no input serial number specified. Exiting!",
122  m_num_devices);
123  }
124  // Exit with error if no camera is detected that matches the input serial
125  // number.
126  if ((!m_serial_no.empty()) && (m_device == NULL)) {
128  "RealSense Camera - No camera detected with input "
129  "serial_no \"%s\" Exiting!",
130  m_serial_no.c_str());
131  }
132 
133  // At this point, m_device will be null if no input serial number was
134  // specified and only one camera is connected. This is a valid use case and
135  // the code will proceed.
136  m_device = m_context.get_device(0);
137  m_serial_no = m_device->get_serial();
138 
139  std::cout << "RealSense Camera - Connecting to camera with Serial No: " << m_serial_no << std::endl;
140 
141  // Enable only infrared2 stream if supported
142  m_enableStreams[rs::stream::infrared2] = m_enableStreams[rs::stream::infrared2]
143  ? m_device->supports(rs::capabilities::infrared2)
144  : m_enableStreams[rs::stream::infrared2];
145 
146  if (m_device->is_streaming()) {
147  m_device->stop();
148  }
149 
150  for (int j = 0; j < 4; j++) {
151  auto s = (rs::stream)j;
152  auto capabilities = (rs::capabilities)j;
153  if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
154  m_device->disable_stream(s);
155  }
156  }
157 
158  if (m_enableStreams[rs::stream::color]) {
159  if (m_useStreamPresets[rs::stream::color]) {
160  m_device->enable_stream(rs::stream::color, m_streamPresets[rs::stream::color]);
161  } else {
162  m_device->enable_stream(rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth,
163  m_streamParams[rs::stream::color].m_streamHeight,
164  m_streamParams[rs::stream::color].m_streamFormat,
165  m_streamParams[rs::stream::color].m_streamFramerate);
166  }
167  }
168 
169  if (m_enableStreams[rs::stream::depth]) {
170  if (m_useStreamPresets[rs::stream::depth]) {
171  m_device->enable_stream(rs::stream::depth, m_streamPresets[rs::stream::depth]);
172  } else {
173  m_device->enable_stream(rs::stream::depth, m_streamParams[rs::stream::depth].m_streamWidth,
174  m_streamParams[rs::stream::depth].m_streamHeight,
175  m_streamParams[rs::stream::depth].m_streamFormat,
176  m_streamParams[rs::stream::depth].m_streamFramerate);
177  }
178  }
179 
180  if (m_enableStreams[rs::stream::infrared]) {
181  if (m_useStreamPresets[rs::stream::infrared]) {
182  m_device->enable_stream(rs::stream::infrared, m_streamPresets[rs::stream::infrared]);
183  } else {
184  m_device->enable_stream(rs::stream::infrared, m_streamParams[rs::stream::infrared].m_streamWidth,
185  m_streamParams[rs::stream::infrared].m_streamHeight,
186  m_streamParams[rs::stream::infrared].m_streamFormat,
187  m_streamParams[rs::stream::infrared].m_streamFramerate);
188  }
189  }
190 
191  if (m_enableStreams[rs::stream::infrared2]) {
192  if (m_useStreamPresets[rs::stream::infrared2]) {
193  m_device->enable_stream(rs::stream::infrared2, m_streamPresets[rs::stream::infrared2]);
194  } else {
195  m_device->enable_stream(rs::stream::infrared2, m_streamParams[rs::stream::infrared2].m_streamWidth,
196  m_streamParams[rs::stream::infrared2].m_streamHeight,
197  m_streamParams[rs::stream::infrared2].m_streamFormat,
198  m_streamParams[rs::stream::infrared2].m_streamFramerate);
199  }
200  }
201 
202  // Compute field of view for each enabled stream
203  m_intrinsics.clear();
204  for (int i = 0; i < 4; ++i) {
205  auto stream = rs::stream(i);
206  if (!m_device->is_stream_enabled(stream))
207  continue;
208  auto intrin = m_device->get_stream_intrinsics(stream);
209 
210  m_intrinsics[stream] = intrin;
211  }
212 
213  // Add synthetic stream intrinsics
214  if (m_enableStreams[rs::stream::color]) {
215  m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color);
216 
217  if (m_enableStreams[rs::stream::depth]) {
218  m_intrinsics[rs::stream::color_aligned_to_depth] =
219  m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
220  m_intrinsics[rs::stream::depth_aligned_to_color] =
221  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
222  m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
223  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
224  }
225  }
226 
227  if (m_enableStreams[rs::stream::depth] && m_enableStreams[rs::stream::infrared2]) {
228  m_intrinsics[rs::stream::depth_aligned_to_infrared2] =
229  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
230  m_intrinsics[rs::stream::infrared2_aligned_to_depth] =
231  m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
232  }
233 
234  // Start device
235  m_device->start();
236 }
237 
242 {
243  if (m_device) {
244  if (m_device->is_streaming()) {
245  m_device->stop();
246  }
247  m_device = NULL;
248  }
249 }
250 
266 void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
267 {
268  if (m_serial_no.empty()) {
270  "RealSense Camera - Multiple cameras detected (%d) but "
271  "no input serial number specified. Exiting!",
272  m_num_devices);
273  }
274 
275  m_serial_no = serial_no;
276 }
277 
289 {
290  auto intrinsics = this->getIntrinsics(stream);
291 
292  vpCameraParameters cam;
293  double u0 = intrinsics.ppx;
294  double v0 = intrinsics.ppy;
295  double px = intrinsics.fx;
296  double py = intrinsics.fy;
298  double kdu = intrinsics.coeffs[0];
299  cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
300  } else {
301  cam.initPersProjWithoutDistortion(px, py, u0, v0);
302  }
303  return cam;
304 }
305 
312 rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
313 {
314  if (!m_device) {
315  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
316  "retrieve intrinsics. Exiting!");
317  }
318 
319  return m_device->get_stream_intrinsics(stream);
320 }
321 
329 rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const
330 {
331  if (!m_device) {
332  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
333  "retrieve extrinsics. Exiting!");
334  }
335  if (!m_device->is_stream_enabled(from)) {
337  "RealSense Camera - stream (%d) is not enabled to "
338  "retrieve extrinsics. Exiting!",
339  (int)from);
340  }
341  if (!m_device->is_stream_enabled(to)) {
343  "RealSense Camera - stream (%d) is not enabled to "
344  "retrieve extrinsics. Exiting!",
345  (int)to);
346  }
347  return m_device->get_extrinsics(from, to);
348 }
349 
357 vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const
358 {
359  rs::extrinsics extrinsics = this->getExtrinsics(from, to);
362  for (unsigned int i = 0; i < 3; i++) {
363  t[i] = extrinsics.translation[i];
364  for (unsigned int j = 0; j < 3; j++)
365  R[i][j] = extrinsics.rotation[j * 3 + i]; // rotation is column-major order
366  }
367  vpHomogeneousMatrix fMt(t, R);
368  return fMt;
369 }
370 
376 {
377  if (m_device == NULL) {
378  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
379  }
380  if (!m_device->is_streaming()) {
381  open();
382  }
383 
384  m_device->wait_for_frames();
385 
386  // Retrieve grey image
387  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
388 }
389 
397 void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
398 {
399  if (m_device == NULL) {
400  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
401  }
402  if (!m_device->is_streaming()) {
403  open();
404  }
405 
406  m_device->wait_for_frames();
407 
408  // Retrieve grey image
409  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
410 
411  // Retrieve point cloud
412  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
413 }
414 
421 void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
422 {
423  if (m_device == NULL) {
424  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
425  }
426  if (!m_device->is_streaming()) {
427  open();
428  }
429 
430  m_device->wait_for_frames();
431 
432  // Retrieve point cloud
433  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
434 }
435 
441 {
442  if (m_device == NULL) {
443  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
444  }
445  if (!m_device->is_streaming()) {
446  open();
447  }
448 
449  m_device->wait_for_frames();
450 
451  // Retrieve color image
452  vp_rs_get_color_impl(m_device, m_intrinsics, color);
453 }
454 
465  std::vector<vpColVector> &pointcloud)
466 {
467  if (m_device == NULL) {
468  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
469  }
470  if (!m_device->is_streaming()) {
471  open();
472  }
473 
474  m_device->wait_for_frames();
475 
476  // Retrieve color image
477  vp_rs_get_color_impl(m_device, m_intrinsics, color);
478 
479  // Retrieve infrared image
480  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
481 
482  // Retrieve depth image
483  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
484 
485  // Retrieve point cloud
486  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
487 }
488 
499  std::vector<vpColVector> &pointcloud)
500 {
501  if (m_device == NULL) {
502  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
503  }
504  if (!m_device->is_streaming()) {
505  open();
506  }
507 
508  m_device->wait_for_frames();
509 
510  // Retrieve grey image
511  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
512 
513  // Retrieve infrared image
514  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
515 
516  // Retrieve depth image
517  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
518 
519  // Retrieve point cloud
520  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
521 }
522 
530 void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
531 {
532  if (m_device == NULL) {
533  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
534  }
535  if (!m_device->is_streaming()) {
536  open();
537  }
538 
539  m_device->wait_for_frames();
540 
541  // Retrieve color image
542  vp_rs_get_color_impl(m_device, m_intrinsics, color);
543 
544  // Retrieve point cloud
545  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
546 }
547 
565 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
566  std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
567  unsigned char *const data_infrared2, const rs::stream &stream_color,
568  const rs::stream &stream_depth, const rs::stream &stream_infrared,
569  const rs::stream &stream_infrared2)
570 {
571  if (m_device == NULL) {
572  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
573  }
574 
575  if (!m_device->is_streaming()) {
576  open();
577  }
578 
579  m_device->wait_for_frames();
580 
581  if (data_image != NULL) {
582  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
583  }
584 
585  if (data_depth != NULL) {
586  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
587 
588  if (data_pointCloud != NULL) {
589  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
590  }
591  }
592 
593  if (data_infrared != NULL) {
594  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
595  }
596 
597  if (data_infrared2 != NULL) {
598  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
599  }
600 }
601 
607 void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
608 {
609  m_useStreamPresets[stream] = true;
610  m_streamPresets[stream] = preset;
611 }
612 
640 void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
641 {
642  m_useStreamPresets[stream] = false;
643  m_streamParams[stream] = params;
644 }
645 
669 void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; }
670 
671 #ifdef VISP_HAVE_PCL
676 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
677 {
678  if (m_device == NULL) {
679  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
680  }
681  if (!m_device->is_streaming()) {
682  open();
683  }
684 
685  m_device->wait_for_frames();
686 
687  // Retrieve point cloud
688  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
689 }
690 
695 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
696 {
697  if (m_device == NULL) {
698  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
699  }
700  if (!m_device->is_streaming()) {
701  open();
702  }
703 
704  m_device->wait_for_frames();
705 
706  // Retrieve point cloud
707  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
708 }
709 
715 void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
716 {
717  if (m_device == NULL) {
718  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
719  }
720  if (!m_device->is_streaming()) {
721  open();
722  }
723 
724  m_device->wait_for_frames();
725 
726  // Retrieve grey image
727  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
728 
729  // Retrieve point cloud
730  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
731 }
732 
738 void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
739 {
740  if (m_device == NULL) {
741  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
742  }
743  if (!m_device->is_streaming()) {
744  open();
745  }
746 
747  m_device->wait_for_frames();
748 
749  // Retrieve color image
750  vp_rs_get_color_impl(m_device, m_intrinsics, color);
751 
752  // Retrieve point cloud
753  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
754 }
755 
764  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
765 {
766  if (m_device == NULL) {
767  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
768  }
769  if (!m_device->is_streaming()) {
770  open();
771  }
772 
773  m_device->wait_for_frames();
774 
775  // Retrieve color image
776  vp_rs_get_color_impl(m_device, m_intrinsics, color);
777 
778  // Retrieve infrared image
779  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
780 
781  // Retrieve depth image
782  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
783 
784  // Retrieve point cloud
785  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
786 }
787 
796  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
797 {
798  if (m_device == NULL) {
799  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
800  }
801  if (!m_device->is_streaming()) {
802  open();
803  }
804 
805  m_device->wait_for_frames();
806 
807  // Retrieve grey image
808  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
809 
810  // Retrieve infrared image
811  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
812 
813  // Retrieve depth image
814  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
815 
816  // Retrieve point cloud
817  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
818 }
819 
828  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
829 {
830  if (m_device == NULL) {
831  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
832  }
833  if (!m_device->is_streaming()) {
834  open();
835  }
836 
837  m_device->wait_for_frames();
838 
839  // Retrieve color image
840  vp_rs_get_color_impl(m_device, m_intrinsics, color);
841 
842  // Retrieve infrared image
843  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
844 
845  // Retrieve depth image
846  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
847 
848  // Retrieve point cloud
849  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
850 }
851 
860  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
861 {
862  if (m_device == NULL) {
863  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
864  }
865  if (!m_device->is_streaming()) {
866  open();
867  }
868 
869  m_device->wait_for_frames();
870 
871  // Retrieve grey image
872  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
873 
874  // Retrieve infrared image
875  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
876 
877  // Retrieve depth image
878  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
879 
880  // Retrieve point cloud
881  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
882 }
883 
903 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
904  std::vector<vpColVector> *const data_pointCloud,
905  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud, unsigned char *const data_infrared,
906  unsigned char *const data_infrared2, const rs::stream &stream_color,
907  const rs::stream &stream_depth, const rs::stream &stream_infrared,
908  const rs::stream &stream_infrared2)
909 {
910  if (m_device == NULL) {
911  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
912  }
913 
914  if (!m_device->is_streaming()) {
915  open();
916  }
917 
918  m_device->wait_for_frames();
919 
920  if (data_image != NULL) {
921  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
922  }
923 
924  if (data_depth != NULL) {
925  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
926  }
927 
928  if (data_pointCloud != NULL) {
929  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
930  }
931 
932  if (pointcloud != NULL) {
933  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth);
934  }
935 
936  if (data_infrared != NULL) {
937  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
938  }
939 
940  if (data_infrared2 != NULL) {
941  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
942  }
943 }
944 
963 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
964  std::vector<vpColVector> *const data_pointCloud,
965  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud, unsigned char *const data_infrared,
966  unsigned char *const data_infrared2, const rs::stream &stream_color,
967  const rs::stream &stream_depth, const rs::stream &stream_infrared,
968  const rs::stream &stream_infrared2)
969 {
970  if (m_device == NULL) {
971  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
972  }
973 
974  if (!m_device->is_streaming()) {
975  open();
976  }
977 
978  m_device->wait_for_frames();
979 
980  if (data_image != NULL) {
981  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
982  }
983 
984  if (data_depth != NULL) {
985  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
986  }
987 
988  if (data_pointCloud != NULL) {
989  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
990  }
991 
992  if (pointcloud != NULL) {
993  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
994  stream_depth);
995  }
996 
997  if (data_infrared != NULL) {
998  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
999  }
1000 
1001  if (data_infrared2 != NULL) {
1002  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1003  }
1004 }
1005 #endif
1006 
1027 std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
1028 {
1029  os << "Device name: " << rs.getHandler()->get_name() << std::endl;
1030  std::streamsize ss = std::cout.precision();
1031  for (int i = 0; i < 4; ++i) {
1032  auto stream = rs::stream(i);
1033  if (!rs.getHandler()->is_stream_enabled(stream))
1034  continue;
1035  auto intrin = rs.getHandler()->get_stream_intrinsics(stream);
1036  std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
1037  std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
1038  << ", distortion = " << intrin.model() << std::endl;
1039  }
1040  std::cout.precision(ss);
1041 
1042  return os;
1043 }
1044 
1045 #elif !defined(VISP_BUILD_SHARED_LIBS)
1046 // Work arround to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has no
1047 // symbols
1048 void dummy_vpRealSense(){};
1049 #endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:494
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::string m_serial_no
Definition: vpRealSense.h:452
void setDeviceBySerialNumber(const std::string &serial_no)
virtual ~vpRealSense()
Definition: vpRealSense.cpp:63
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void initStream()
Definition: vpRealSense.cpp:65
std::map< rs::stream, bool > m_enableStreams
Definition: vpRealSense.h:455
rs::device * m_device
Definition: vpRealSense.h:450
rs::device * getHandler() const
Get access to device handler.
Definition: vpRealSense.h:396
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
int m_num_devices
Definition: vpRealSense.h:451
void acquire(std::vector< vpColVector > &pointcloud)
std::map< rs::stream, bool > m_useStreamPresets
Definition: vpRealSense.h:456
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
void setEnableStream(const rs::stream &stream, bool status)
std::map< rs::stream, rs::intrinsics > m_intrinsics
Definition: vpRealSense.h:453
rs::context m_context
Definition: vpRealSense.h:449
std::map< rs::stream, rs::preset > m_streamPresets
Definition: vpRealSense.h:457
float m_invalidDepthValue
Definition: vpRealSense.h:459
rs::intrinsics getIntrinsics(const rs::stream &stream) const
std::map< rs::stream, vpRsStreamParams > m_streamParams
Definition: vpRealSense.h:458
float m_max_Z
Maximal Z depth in meter.
Definition: vpRealSense.h:454
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.