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