Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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
672 
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
rs::device * getHandler() const
Get access to device handler.
Definition: vpRealSense.h:395
void setEnableStream(const rs::stream &stream, bool status)
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpRealSense &rs)
Implementation of an homogeneous matrix and operations on such kind of matrices.
int m_num_devices
Definition: vpRealSense.h:450
void initStream()
Definition: vpRealSense.cpp:65
error that can be emited by ViSP classes.
Definition: vpException.h:71
void acquire(std::vector< vpColVector > &pointcloud)
std::string m_serial_no
Definition: vpRealSense.h:451
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of a rotation matrix and operations on such kind of matrices.
void setDeviceBySerialNumber(const std::string &serial_no)
std::map< rs::stream, vpRsStreamParams > m_streamParams
Definition: vpRealSense.h:457
std::map< rs::stream, bool > m_useStreamPresets
Definition: vpRealSense.h:455
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
Generic class defining intrinsic camera parameters.
rs::device * m_device
Definition: vpRealSense.h:449
std::map< rs::stream, rs::preset > m_streamPresets
Definition: vpRealSense.h:456
virtual ~vpRealSense()
Definition: vpRealSense.cpp:63
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
rs::context m_context
Definition: vpRealSense.h:448
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
float m_invalidDepthValue
Definition: vpRealSense.h:458
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
std::map< rs::stream, rs::intrinsics > m_intrinsics
Definition: vpRealSense.h:452
std::map< rs::stream, bool > m_enableStreams
Definition: vpRealSense.h:454
Class that consider the case of a translation vector.
float m_max_Z
Maximal Z depth in meter.
Definition: vpRealSense.h:453