Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpRealSense.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  * Description:
32  * RealSense SDK wrapper.
33  *
34 *****************************************************************************/
35 
36 #include <iomanip>
37 #include <iostream>
38 
39 #include <visp3/core/vpImageConvert.h>
40 #include <visp3/sensor/vpRealSense.h>
41 
42 #if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
43 
44 #include "vpRealSense_impl.h"
45 
46 BEGIN_VISP_NAMESPACE
51  : m_context(), m_device(nullptr), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(),
52  m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
53 {
54  initStream();
55 }
56 
62 
64 {
65  // General default presets
66  // Color stream
67  m_useStreamPresets[rs::stream::color] = true;
68  m_streamPresets[rs::stream::color] = rs::preset::best_quality;
69  m_streamParams[rs::stream::color] = vpRsStreamParams(640, 480, rs::format::rgba8, 60);
70 
71  // Depth stream
72  m_useStreamPresets[rs::stream::depth] = true;
73  m_streamPresets[rs::stream::depth] = rs::preset::best_quality;
74  m_streamParams[rs::stream::depth] = vpRsStreamParams(640, 480, rs::format::z16, 60);
75 
76  // Infrared stream
77  m_useStreamPresets[rs::stream::infrared] = true;
78  m_streamPresets[rs::stream::infrared] = rs::preset::best_quality;
79  m_streamParams[rs::stream::infrared] = vpRsStreamParams(640, 480, rs::format::y16, 60);
80 
81  // Infrared stream 2
82  m_useStreamPresets[rs::stream::infrared2] = true;
83  m_streamPresets[rs::stream::infrared2] = rs::preset::best_quality;
84  m_streamParams[rs::stream::infrared2] = vpRsStreamParams(640, 480, rs::format::y16, 60);
85 
86  // Enable all streams
87  m_enableStreams[rs::stream::color] = true;
88  m_enableStreams[rs::stream::depth] = true;
89  m_enableStreams[rs::stream::infrared] = true;
90  m_enableStreams[rs::stream::infrared2] = true;
91 }
92 
97 {
98  m_num_devices = m_context.get_device_count();
99 
100  if (m_num_devices == 0)
101  throw vpException(vpException::fatalError, "RealSense Camera - No device detected.");
102 
103  std::vector<rs::device *> detected_devices;
104  rs::device *device;
105  for (int i = 0; i < m_num_devices; ++i) {
106  device = m_context.get_device(i);
107  std::string serial_no = device->get_serial();
108  if (serial_no.compare(m_serial_no) == 0) {
109  m_device = device;
110  }
111  detected_devices.push_back(device);
112  }
113 
114  // Exit with error if no serial number is specified and multiple cameras are
115  // detected.
116  if ((m_serial_no.empty()) && (m_num_devices > 1)) {
118  "RealSense Camera - Multiple cameras detected (%d) but "
119  "no input serial number specified. Exiting!",
120  m_num_devices);
121  }
122  // Exit with error if no camera is detected that matches the input serial
123  // number.
124  if ((!m_serial_no.empty()) && (m_device == nullptr)) {
126  "RealSense Camera - No camera detected with input "
127  "serial_no \"%s\" Exiting!",
128  m_serial_no.c_str());
129  }
130 
131  // At this point, m_device will be null if no input serial number was
132  // specified and only one camera is connected. This is a valid use case and
133  // the code will proceed.
134  m_device = m_context.get_device(0);
135  m_serial_no = m_device->get_serial();
136 
137  std::cout << "RealSense Camera - Connecting to camera with Serial No: " << m_serial_no << std::endl;
138 
139  // Enable only infrared2 stream if supported
140  m_enableStreams[rs::stream::infrared2] = m_enableStreams[rs::stream::infrared2]
141  ? m_device->supports(rs::capabilities::infrared2)
142  : m_enableStreams[rs::stream::infrared2];
143 
144  if (m_device->is_streaming()) {
145  m_device->stop();
146  }
147 
148  for (int j = 0; j < 4; j++) {
149  auto s = (rs::stream)j;
150  auto capabilities = (rs::capabilities)j;
151  if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
152  m_device->disable_stream(s);
153  }
154  }
155 
156  if (m_enableStreams[rs::stream::color]) {
157  if (m_useStreamPresets[rs::stream::color]) {
158  m_device->enable_stream(rs::stream::color, m_streamPresets[rs::stream::color]);
159  }
160  else {
161  m_device->enable_stream(rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth,
162  m_streamParams[rs::stream::color].m_streamHeight,
163  m_streamParams[rs::stream::color].m_streamFormat,
164  m_streamParams[rs::stream::color].m_streamFramerate);
165  }
166  }
167 
168  if (m_enableStreams[rs::stream::depth]) {
169  if (m_useStreamPresets[rs::stream::depth]) {
170  m_device->enable_stream(rs::stream::depth, m_streamPresets[rs::stream::depth]);
171  }
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  }
184  else {
185  m_device->enable_stream(rs::stream::infrared, m_streamParams[rs::stream::infrared].m_streamWidth,
186  m_streamParams[rs::stream::infrared].m_streamHeight,
187  m_streamParams[rs::stream::infrared].m_streamFormat,
188  m_streamParams[rs::stream::infrared].m_streamFramerate);
189  }
190  }
191 
192  if (m_enableStreams[rs::stream::infrared2]) {
193  if (m_useStreamPresets[rs::stream::infrared2]) {
194  m_device->enable_stream(rs::stream::infrared2, m_streamPresets[rs::stream::infrared2]);
195  }
196  else {
197  m_device->enable_stream(rs::stream::infrared2, m_streamParams[rs::stream::infrared2].m_streamWidth,
198  m_streamParams[rs::stream::infrared2].m_streamHeight,
199  m_streamParams[rs::stream::infrared2].m_streamFormat,
200  m_streamParams[rs::stream::infrared2].m_streamFramerate);
201  }
202  }
203 
204  // Compute field of view for each enabled stream
205  m_intrinsics.clear();
206  for (int i = 0; i < 4; ++i) {
207  auto stream = rs::stream(i);
208  if (!m_device->is_stream_enabled(stream))
209  continue;
210  auto intrin = m_device->get_stream_intrinsics(stream);
211 
212  m_intrinsics[stream] = intrin;
213  }
214 
215  // Add synthetic stream intrinsics
216  if (m_enableStreams[rs::stream::color]) {
217  m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color);
218 
219  if (m_enableStreams[rs::stream::depth]) {
220  m_intrinsics[rs::stream::color_aligned_to_depth] =
221  m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
222  m_intrinsics[rs::stream::depth_aligned_to_color] =
223  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
224  m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
225  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
226  }
227  }
228 
229  if (m_enableStreams[rs::stream::depth] && m_enableStreams[rs::stream::infrared2]) {
230  m_intrinsics[rs::stream::depth_aligned_to_infrared2] =
231  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
232  m_intrinsics[rs::stream::infrared2_aligned_to_depth] =
233  m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
234  }
235 
236  // Start device
237  m_device->start();
238 }
239 
244 {
245  if (m_device) {
246  if (m_device->is_streaming()) {
247  m_device->stop();
248  }
249  m_device = nullptr;
250  }
251 }
252 
272 void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
273 {
274  if (m_serial_no.empty()) {
276  "RealSense Camera - Multiple cameras detected (%d) but "
277  "no input serial number specified. Exiting!",
278  m_num_devices);
279  }
280 
281  m_serial_no = serial_no;
282 }
283 
295 {
296  auto intrinsics = this->getIntrinsics(stream);
297 
298  vpCameraParameters cam;
299  double u0 = intrinsics.ppx;
300  double v0 = intrinsics.ppy;
301  double px = intrinsics.fx;
302  double py = intrinsics.fy;
304  double kdu = intrinsics.coeffs[0];
305  cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
306  }
307  else {
308  cam.initPersProjWithoutDistortion(px, py, u0, v0);
309  }
310  return cam;
311 }
312 
319 rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
320 {
321  if (!m_device) {
322  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
323  "retrieve intrinsics. Exiting!");
324  }
325 
326  return m_device->get_stream_intrinsics(stream);
327 }
328 
336 rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const
337 {
338  if (!m_device) {
339  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
340  "retrieve extrinsics. Exiting!");
341  }
342  if (!m_device->is_stream_enabled(from)) {
344  "RealSense Camera - stream (%d) is not enabled to "
345  "retrieve extrinsics. Exiting!",
346  (int)from);
347  }
348  if (!m_device->is_stream_enabled(to)) {
350  "RealSense Camera - stream (%d) is not enabled to "
351  "retrieve extrinsics. Exiting!",
352  (int)to);
353  }
354  return m_device->get_extrinsics(from, to);
355 }
356 
364 vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const
365 {
366  rs::extrinsics extrinsics = this->getExtrinsics(from, to);
369  for (unsigned int i = 0; i < 3; i++) {
370  t[i] = extrinsics.translation[i];
371  for (unsigned int j = 0; j < 3; j++)
372  R[i][j] = extrinsics.rotation[j * 3 + i]; // rotation is column-major order
373  }
374  vpHomogeneousMatrix fMt(t, R);
375  return fMt;
376 }
377 
383 {
384  if (m_device == nullptr) {
385  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
386  }
387  if (!m_device->is_streaming()) {
388  open();
389  }
390 
391  m_device->wait_for_frames();
392 
393  // Retrieve grey image
394  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
395 }
396 
404 void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
405 {
406  if (m_device == nullptr) {
407  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
408  }
409  if (!m_device->is_streaming()) {
410  open();
411  }
412 
413  m_device->wait_for_frames();
414 
415  // Retrieve grey image
416  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
417 
418  // Retrieve point cloud
419  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
420 }
421 
428 void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
429 {
430  if (m_device == nullptr) {
431  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
432  }
433  if (!m_device->is_streaming()) {
434  open();
435  }
436 
437  m_device->wait_for_frames();
438 
439  // Retrieve point cloud
440  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
441 }
442 
448 {
449  if (m_device == nullptr) {
450  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
451  }
452  if (!m_device->is_streaming()) {
453  open();
454  }
455 
456  m_device->wait_for_frames();
457 
458  // Retrieve color image
459  vp_rs_get_color_impl(m_device, m_intrinsics, color);
460 }
461 
472  std::vector<vpColVector> &pointcloud)
473 {
474  if (m_device == nullptr) {
475  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
476  }
477  if (!m_device->is_streaming()) {
478  open();
479  }
480 
481  m_device->wait_for_frames();
482 
483  // Retrieve color image
484  vp_rs_get_color_impl(m_device, m_intrinsics, color);
485 
486  // Retrieve infrared image
487  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
488 
489  // Retrieve depth image
490  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
491 
492  // Retrieve point cloud
493  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
494 }
495 
506  std::vector<vpColVector> &pointcloud)
507 {
508  if (m_device == nullptr) {
509  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
510  }
511  if (!m_device->is_streaming()) {
512  open();
513  }
514 
515  m_device->wait_for_frames();
516 
517  // Retrieve grey image
518  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
519 
520  // Retrieve infrared image
521  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
522 
523  // Retrieve depth image
524  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
525 
526  // Retrieve point cloud
527  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
528 }
529 
537 void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
538 {
539  if (m_device == nullptr) {
540  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
541  }
542  if (!m_device->is_streaming()) {
543  open();
544  }
545 
546  m_device->wait_for_frames();
547 
548  // Retrieve color image
549  vp_rs_get_color_impl(m_device, m_intrinsics, color);
550 
551  // Retrieve point cloud
552  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
553 }
554 
572 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
573  std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
574  unsigned char *const data_infrared2, const rs::stream &stream_color,
575  const rs::stream &stream_depth, const rs::stream &stream_infrared,
576  const rs::stream &stream_infrared2)
577 {
578  if (m_device == nullptr) {
579  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
580  }
581 
582  if (!m_device->is_streaming()) {
583  open();
584  }
585 
586  m_device->wait_for_frames();
587 
588  if (data_image != nullptr) {
589  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
590  }
591 
592  if (data_depth != nullptr) {
593  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
594 
595  if (data_pointCloud != nullptr) {
596  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
597  }
598  }
599 
600  if (data_infrared != nullptr) {
601  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
602  }
603 
604  if (data_infrared2 != nullptr) {
605  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
606  }
607 }
608 
614 void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
615 {
616  m_useStreamPresets[stream] = true;
617  m_streamPresets[stream] = preset;
618 }
619 
651 void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
652 {
653  m_useStreamPresets[stream] = false;
654  m_streamParams[stream] = params;
655 }
656 
684 void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; }
685 
686 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
691 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
692 {
693  if (m_device == nullptr) {
694  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
695  }
696  if (!m_device->is_streaming()) {
697  open();
698  }
699 
700  m_device->wait_for_frames();
701 
702  // Retrieve point cloud
703  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
704 }
705 
710 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
711 {
712  if (m_device == nullptr) {
713  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
714  }
715  if (!m_device->is_streaming()) {
716  open();
717  }
718 
719  m_device->wait_for_frames();
720 
721  // Retrieve point cloud
722  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
723 }
724 
730 void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
731 {
732  if (m_device == nullptr) {
733  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
734  }
735  if (!m_device->is_streaming()) {
736  open();
737  }
738 
739  m_device->wait_for_frames();
740 
741  // Retrieve grey image
742  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
743 
744  // Retrieve point cloud
745  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
746 }
747 
753 void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
754 {
755  if (m_device == nullptr) {
756  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
757  }
758  if (!m_device->is_streaming()) {
759  open();
760  }
761 
762  m_device->wait_for_frames();
763 
764  // Retrieve color image
765  vp_rs_get_color_impl(m_device, m_intrinsics, color);
766 
767  // Retrieve point cloud
768  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
769 }
770 
779  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
780 {
781  if (m_device == nullptr) {
782  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
783  }
784  if (!m_device->is_streaming()) {
785  open();
786  }
787 
788  m_device->wait_for_frames();
789 
790  // Retrieve color image
791  vp_rs_get_color_impl(m_device, m_intrinsics, color);
792 
793  // Retrieve infrared image
794  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
795 
796  // Retrieve depth image
797  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
798 
799  // Retrieve point cloud
800  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
801 }
802 
811  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
812 {
813  if (m_device == nullptr) {
814  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
815  }
816  if (!m_device->is_streaming()) {
817  open();
818  }
819 
820  m_device->wait_for_frames();
821 
822  // Retrieve grey image
823  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
824 
825  // Retrieve infrared image
826  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
827 
828  // Retrieve depth image
829  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
830 
831  // Retrieve point cloud
832  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
833 }
834 
843  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
844 {
845  if (m_device == nullptr) {
846  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
847  }
848  if (!m_device->is_streaming()) {
849  open();
850  }
851 
852  m_device->wait_for_frames();
853 
854  // Retrieve color image
855  vp_rs_get_color_impl(m_device, m_intrinsics, color);
856 
857  // Retrieve infrared image
858  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
859 
860  // Retrieve depth image
861  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
862 
863  // Retrieve point cloud
864  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
865 }
866 
875  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
876 {
877  if (m_device == nullptr) {
878  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
879  }
880  if (!m_device->is_streaming()) {
881  open();
882  }
883 
884  m_device->wait_for_frames();
885 
886  // Retrieve grey image
887  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
888 
889  // Retrieve infrared image
890  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
891 
892  // Retrieve depth image
893  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
894 
895  // Retrieve point cloud
896  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
897 }
898 
918 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
919  std::vector<vpColVector> *const data_pointCloud,
920  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud, unsigned char *const data_infrared,
921  unsigned char *const data_infrared2, const rs::stream &stream_color,
922  const rs::stream &stream_depth, const rs::stream &stream_infrared,
923  const rs::stream &stream_infrared2)
924 {
925  if (m_device == nullptr) {
926  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
927  }
928 
929  if (!m_device->is_streaming()) {
930  open();
931  }
932 
933  m_device->wait_for_frames();
934 
935  if (data_image != nullptr) {
936  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
937  }
938 
939  if (data_depth != nullptr) {
940  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
941  }
942 
943  if (data_pointCloud != nullptr) {
944  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
945  }
946 
947  if (pointcloud != nullptr) {
948  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth);
949  }
950 
951  if (data_infrared != nullptr) {
952  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
953  }
954 
955  if (data_infrared2 != nullptr) {
956  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
957  }
958 }
959 
978 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
979  std::vector<vpColVector> *const data_pointCloud,
980  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud, unsigned char *const data_infrared,
981  unsigned char *const data_infrared2, const rs::stream &stream_color,
982  const rs::stream &stream_depth, const rs::stream &stream_infrared,
983  const rs::stream &stream_infrared2)
984 {
985  if (m_device == nullptr) {
986  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
987  }
988 
989  if (!m_device->is_streaming()) {
990  open();
991  }
992 
993  m_device->wait_for_frames();
994 
995  if (data_image != nullptr) {
996  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
997  }
998 
999  if (data_depth != nullptr) {
1000  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
1001  }
1002 
1003  if (data_pointCloud != nullptr) {
1004  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
1005  }
1006 
1007  if (pointcloud != nullptr) {
1008  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
1009  stream_depth);
1010  }
1011 
1012  if (data_infrared != nullptr) {
1013  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
1014  }
1015 
1016  if (data_infrared2 != nullptr) {
1017  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1018  }
1019 }
1020 #endif
1021 
1046 std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
1047 {
1048  os << "Device name: " << rs.getHandler()->get_name() << std::endl;
1049  std::streamsize ss = std::cout.precision();
1050  for (int i = 0; i < 4; ++i) {
1051  auto stream = rs::stream(i);
1052  if (!rs.getHandler()->is_stream_enabled(stream))
1053  continue;
1054  auto intrin = rs.getHandler()->get_stream_intrinsics(stream);
1055  std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
1056  std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
1057  << ", distortion = " << intrin.model() << std::endl;
1058  }
1059  std::cout.precision(ss);
1060 
1061  return os;
1062 }
1063 END_VISP_NAMESPACE
1064 #elif !defined(VISP_BUILD_SHARED_LIBS)
1065 // Work around to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has symbols
1066 void dummy_vpRealSense() { };
1067 #endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:611
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::string m_serial_no
Definition: vpRealSense.h:477
void setDeviceBySerialNumber(const std::string &serial_no)
virtual ~vpRealSense()
Definition: vpRealSense.cpp:61
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void initStream()
Definition: vpRealSense.cpp:63
std::map< rs::stream, bool > m_enableStreams
Definition: vpRealSense.h:480
rs::device * m_device
Definition: vpRealSense.h:475
rs::device * getHandler() const
Get access to device handler.
Definition: vpRealSense.h:422
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:476
void acquire(std::vector< vpColVector > &pointcloud)
std::map< rs::stream, bool > m_useStreamPresets
Definition: vpRealSense.h:481
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:478
rs::context m_context
Definition: vpRealSense.h:474
std::map< rs::stream, rs::preset > m_streamPresets
Definition: vpRealSense.h:482
float m_invalidDepthValue
Definition: vpRealSense.h:484
rs::intrinsics getIntrinsics(const rs::stream &stream) const
std::map< rs::stream, vpRsStreamParams > m_streamParams
Definition: vpRealSense.h:483
float m_max_Z
Maximal Z depth in meter.
Definition: vpRealSense.h:479
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.