Visual Servoing Platform  version 3.6.1 under development (2024-06-12)
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 
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 
268 void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
269 {
270  if (m_serial_no.empty()) {
272  "RealSense Camera - Multiple cameras detected (%d) but "
273  "no input serial number specified. Exiting!",
274  m_num_devices);
275  }
276 
277  m_serial_no = serial_no;
278 }
279 
291 {
292  auto intrinsics = this->getIntrinsics(stream);
293 
294  vpCameraParameters cam;
295  double u0 = intrinsics.ppx;
296  double v0 = intrinsics.ppy;
297  double px = intrinsics.fx;
298  double py = intrinsics.fy;
300  double kdu = intrinsics.coeffs[0];
301  cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
302  }
303  else {
304  cam.initPersProjWithoutDistortion(px, py, u0, v0);
305  }
306  return cam;
307 }
308 
315 rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
316 {
317  if (!m_device) {
318  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
319  "retrieve intrinsics. Exiting!");
320  }
321 
322  return m_device->get_stream_intrinsics(stream);
323 }
324 
332 rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const
333 {
334  if (!m_device) {
335  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
336  "retrieve extrinsics. Exiting!");
337  }
338  if (!m_device->is_stream_enabled(from)) {
340  "RealSense Camera - stream (%d) is not enabled to "
341  "retrieve extrinsics. Exiting!",
342  (int)from);
343  }
344  if (!m_device->is_stream_enabled(to)) {
346  "RealSense Camera - stream (%d) is not enabled to "
347  "retrieve extrinsics. Exiting!",
348  (int)to);
349  }
350  return m_device->get_extrinsics(from, to);
351 }
352 
360 vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const
361 {
362  rs::extrinsics extrinsics = this->getExtrinsics(from, to);
365  for (unsigned int i = 0; i < 3; i++) {
366  t[i] = extrinsics.translation[i];
367  for (unsigned int j = 0; j < 3; j++)
368  R[i][j] = extrinsics.rotation[j * 3 + i]; // rotation is column-major order
369  }
370  vpHomogeneousMatrix fMt(t, R);
371  return fMt;
372 }
373 
379 {
380  if (m_device == nullptr) {
381  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
382  }
383  if (!m_device->is_streaming()) {
384  open();
385  }
386 
387  m_device->wait_for_frames();
388 
389  // Retrieve grey image
390  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
391 }
392 
400 void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
401 {
402  if (m_device == nullptr) {
403  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
404  }
405  if (!m_device->is_streaming()) {
406  open();
407  }
408 
409  m_device->wait_for_frames();
410 
411  // Retrieve grey image
412  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
413 
414  // Retrieve point cloud
415  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
416 }
417 
424 void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
425 {
426  if (m_device == nullptr) {
427  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
428  }
429  if (!m_device->is_streaming()) {
430  open();
431  }
432 
433  m_device->wait_for_frames();
434 
435  // Retrieve point cloud
436  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
437 }
438 
444 {
445  if (m_device == nullptr) {
446  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
447  }
448  if (!m_device->is_streaming()) {
449  open();
450  }
451 
452  m_device->wait_for_frames();
453 
454  // Retrieve color image
455  vp_rs_get_color_impl(m_device, m_intrinsics, color);
456 }
457 
468  std::vector<vpColVector> &pointcloud)
469 {
470  if (m_device == nullptr) {
471  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
472  }
473  if (!m_device->is_streaming()) {
474  open();
475  }
476 
477  m_device->wait_for_frames();
478 
479  // Retrieve color image
480  vp_rs_get_color_impl(m_device, m_intrinsics, color);
481 
482  // Retrieve infrared image
483  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
484 
485  // Retrieve depth image
486  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
487 
488  // Retrieve point cloud
489  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
490 }
491 
502  std::vector<vpColVector> &pointcloud)
503 {
504  if (m_device == nullptr) {
505  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
506  }
507  if (!m_device->is_streaming()) {
508  open();
509  }
510 
511  m_device->wait_for_frames();
512 
513  // Retrieve grey image
514  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
515 
516  // Retrieve infrared image
517  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
518 
519  // Retrieve depth image
520  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
521 
522  // Retrieve point cloud
523  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
524 }
525 
533 void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
534 {
535  if (m_device == nullptr) {
536  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
537  }
538  if (!m_device->is_streaming()) {
539  open();
540  }
541 
542  m_device->wait_for_frames();
543 
544  // Retrieve color image
545  vp_rs_get_color_impl(m_device, m_intrinsics, color);
546 
547  // Retrieve point cloud
548  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
549 }
550 
568 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
569  std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
570  unsigned char *const data_infrared2, const rs::stream &stream_color,
571  const rs::stream &stream_depth, const rs::stream &stream_infrared,
572  const rs::stream &stream_infrared2)
573 {
574  if (m_device == nullptr) {
575  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
576  }
577 
578  if (!m_device->is_streaming()) {
579  open();
580  }
581 
582  m_device->wait_for_frames();
583 
584  if (data_image != nullptr) {
585  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
586  }
587 
588  if (data_depth != nullptr) {
589  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
590 
591  if (data_pointCloud != nullptr) {
592  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
593  }
594  }
595 
596  if (data_infrared != nullptr) {
597  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
598  }
599 
600  if (data_infrared2 != nullptr) {
601  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
602  }
603 }
604 
610 void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
611 {
612  m_useStreamPresets[stream] = true;
613  m_streamPresets[stream] = preset;
614 }
615 
643 void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
644 {
645  m_useStreamPresets[stream] = false;
646  m_streamParams[stream] = params;
647 }
648 
672 void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; }
673 
674 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
679 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
680 {
681  if (m_device == nullptr) {
682  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
683  }
684  if (!m_device->is_streaming()) {
685  open();
686  }
687 
688  m_device->wait_for_frames();
689 
690  // Retrieve point cloud
691  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
692 }
693 
698 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
699 {
700  if (m_device == nullptr) {
701  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
702  }
703  if (!m_device->is_streaming()) {
704  open();
705  }
706 
707  m_device->wait_for_frames();
708 
709  // Retrieve point cloud
710  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
711 }
712 
718 void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
719 {
720  if (m_device == nullptr) {
721  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
722  }
723  if (!m_device->is_streaming()) {
724  open();
725  }
726 
727  m_device->wait_for_frames();
728 
729  // Retrieve grey image
730  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
731 
732  // Retrieve point cloud
733  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
734 }
735 
741 void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
742 {
743  if (m_device == nullptr) {
744  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
745  }
746  if (!m_device->is_streaming()) {
747  open();
748  }
749 
750  m_device->wait_for_frames();
751 
752  // Retrieve color image
753  vp_rs_get_color_impl(m_device, m_intrinsics, color);
754 
755  // Retrieve point cloud
756  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
757 }
758 
767  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
768 {
769  if (m_device == nullptr) {
770  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
771  }
772  if (!m_device->is_streaming()) {
773  open();
774  }
775 
776  m_device->wait_for_frames();
777 
778  // Retrieve color image
779  vp_rs_get_color_impl(m_device, m_intrinsics, color);
780 
781  // Retrieve infrared image
782  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
783 
784  // Retrieve depth image
785  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
786 
787  // Retrieve point cloud
788  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
789 }
790 
799  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
800 {
801  if (m_device == nullptr) {
802  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
803  }
804  if (!m_device->is_streaming()) {
805  open();
806  }
807 
808  m_device->wait_for_frames();
809 
810  // Retrieve grey image
811  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
812 
813  // Retrieve infrared image
814  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
815 
816  // Retrieve depth image
817  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
818 
819  // Retrieve point cloud
820  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
821 }
822 
831  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
832 {
833  if (m_device == nullptr) {
834  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
835  }
836  if (!m_device->is_streaming()) {
837  open();
838  }
839 
840  m_device->wait_for_frames();
841 
842  // Retrieve color image
843  vp_rs_get_color_impl(m_device, m_intrinsics, color);
844 
845  // Retrieve infrared image
846  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
847 
848  // Retrieve depth image
849  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
850 
851  // Retrieve point cloud
852  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
853 }
854 
863  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
864 {
865  if (m_device == nullptr) {
866  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
867  }
868  if (!m_device->is_streaming()) {
869  open();
870  }
871 
872  m_device->wait_for_frames();
873 
874  // Retrieve grey image
875  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
876 
877  // Retrieve infrared image
878  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
879 
880  // Retrieve depth image
881  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
882 
883  // Retrieve point cloud
884  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
885 }
886 
906 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
907  std::vector<vpColVector> *const data_pointCloud,
908  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud, unsigned char *const data_infrared,
909  unsigned char *const data_infrared2, const rs::stream &stream_color,
910  const rs::stream &stream_depth, const rs::stream &stream_infrared,
911  const rs::stream &stream_infrared2)
912 {
913  if (m_device == nullptr) {
914  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
915  }
916 
917  if (!m_device->is_streaming()) {
918  open();
919  }
920 
921  m_device->wait_for_frames();
922 
923  if (data_image != nullptr) {
924  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
925  }
926 
927  if (data_depth != nullptr) {
928  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
929  }
930 
931  if (data_pointCloud != nullptr) {
932  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
933  }
934 
935  if (pointcloud != nullptr) {
936  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth);
937  }
938 
939  if (data_infrared != nullptr) {
940  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
941  }
942 
943  if (data_infrared2 != nullptr) {
944  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
945  }
946 }
947 
966 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
967  std::vector<vpColVector> *const data_pointCloud,
968  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud, unsigned char *const data_infrared,
969  unsigned char *const data_infrared2, const rs::stream &stream_color,
970  const rs::stream &stream_depth, const rs::stream &stream_infrared,
971  const rs::stream &stream_infrared2)
972 {
973  if (m_device == nullptr) {
974  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
975  }
976 
977  if (!m_device->is_streaming()) {
978  open();
979  }
980 
981  m_device->wait_for_frames();
982 
983  if (data_image != nullptr) {
984  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
985  }
986 
987  if (data_depth != nullptr) {
988  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
989  }
990 
991  if (data_pointCloud != nullptr) {
992  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
993  }
994 
995  if (pointcloud != nullptr) {
996  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
997  stream_depth);
998  }
999 
1000  if (data_infrared != nullptr) {
1001  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
1002  }
1003 
1004  if (data_infrared2 != nullptr) {
1005  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1006  }
1007 }
1008 #endif
1009 
1030 std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
1031 {
1032  os << "Device name: " << rs.getHandler()->get_name() << std::endl;
1033  std::streamsize ss = std::cout.precision();
1034  for (int i = 0; i < 4; ++i) {
1035  auto stream = rs::stream(i);
1036  if (!rs.getHandler()->is_stream_enabled(stream))
1037  continue;
1038  auto intrin = rs.getHandler()->get_stream_intrinsics(stream);
1039  std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
1040  std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
1041  << ", distortion = " << intrin.model() << std::endl;
1042  }
1043  std::cout.precision(ss);
1044 
1045  return os;
1046 }
1048 #elif !defined(VISP_BUILD_SHARED_LIBS)
1049 // Work around to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has symbols
1050 void dummy_vpRealSense() { };
1051 #endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:604
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:449
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:452
rs::device * m_device
Definition: vpRealSense.h:447
rs::device * getHandler() const
Get access to device handler.
Definition: vpRealSense.h:394
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:448
void acquire(std::vector< vpColVector > &pointcloud)
std::map< rs::stream, bool > m_useStreamPresets
Definition: vpRealSense.h:453
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:450
rs::context m_context
Definition: vpRealSense.h:446
std::map< rs::stream, rs::preset > m_streamPresets
Definition: vpRealSense.h:454
float m_invalidDepthValue
Definition: vpRealSense.h:456
rs::intrinsics getIntrinsics(const rs::stream &stream) const
std::map< rs::stream, vpRsStreamParams > m_streamParams
Definition: vpRealSense.h:455
float m_max_Z
Maximal Z depth in meter.
Definition: vpRealSense.h:451
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.