Visual Servoing Platform  version 3.6.0 under development (2023-09-25)
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(NULL), 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 == NULL)) {
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  } else {
159  m_device->enable_stream(rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth,
160  m_streamParams[rs::stream::color].m_streamHeight,
161  m_streamParams[rs::stream::color].m_streamFormat,
162  m_streamParams[rs::stream::color].m_streamFramerate);
163  }
164  }
165 
166  if (m_enableStreams[rs::stream::depth]) {
167  if (m_useStreamPresets[rs::stream::depth]) {
168  m_device->enable_stream(rs::stream::depth, m_streamPresets[rs::stream::depth]);
169  } else {
170  m_device->enable_stream(rs::stream::depth, m_streamParams[rs::stream::depth].m_streamWidth,
171  m_streamParams[rs::stream::depth].m_streamHeight,
172  m_streamParams[rs::stream::depth].m_streamFormat,
173  m_streamParams[rs::stream::depth].m_streamFramerate);
174  }
175  }
176 
177  if (m_enableStreams[rs::stream::infrared]) {
178  if (m_useStreamPresets[rs::stream::infrared]) {
179  m_device->enable_stream(rs::stream::infrared, m_streamPresets[rs::stream::infrared]);
180  } else {
181  m_device->enable_stream(rs::stream::infrared, m_streamParams[rs::stream::infrared].m_streamWidth,
182  m_streamParams[rs::stream::infrared].m_streamHeight,
183  m_streamParams[rs::stream::infrared].m_streamFormat,
184  m_streamParams[rs::stream::infrared].m_streamFramerate);
185  }
186  }
187 
188  if (m_enableStreams[rs::stream::infrared2]) {
189  if (m_useStreamPresets[rs::stream::infrared2]) {
190  m_device->enable_stream(rs::stream::infrared2, m_streamPresets[rs::stream::infrared2]);
191  } else {
192  m_device->enable_stream(rs::stream::infrared2, m_streamParams[rs::stream::infrared2].m_streamWidth,
193  m_streamParams[rs::stream::infrared2].m_streamHeight,
194  m_streamParams[rs::stream::infrared2].m_streamFormat,
195  m_streamParams[rs::stream::infrared2].m_streamFramerate);
196  }
197  }
198 
199  // Compute field of view for each enabled stream
200  m_intrinsics.clear();
201  for (int i = 0; i < 4; ++i) {
202  auto stream = rs::stream(i);
203  if (!m_device->is_stream_enabled(stream))
204  continue;
205  auto intrin = m_device->get_stream_intrinsics(stream);
206 
207  m_intrinsics[stream] = intrin;
208  }
209 
210  // Add synthetic stream intrinsics
211  if (m_enableStreams[rs::stream::color]) {
212  m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color);
213 
214  if (m_enableStreams[rs::stream::depth]) {
215  m_intrinsics[rs::stream::color_aligned_to_depth] =
216  m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
217  m_intrinsics[rs::stream::depth_aligned_to_color] =
218  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
219  m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
220  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
221  }
222  }
223 
224  if (m_enableStreams[rs::stream::depth] && m_enableStreams[rs::stream::infrared2]) {
225  m_intrinsics[rs::stream::depth_aligned_to_infrared2] =
226  m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
227  m_intrinsics[rs::stream::infrared2_aligned_to_depth] =
228  m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
229  }
230 
231  // Start device
232  m_device->start();
233 }
234 
239 {
240  if (m_device) {
241  if (m_device->is_streaming()) {
242  m_device->stop();
243  }
244  m_device = NULL;
245  }
246 }
247 
263 void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
264 {
265  if (m_serial_no.empty()) {
267  "RealSense Camera - Multiple cameras detected (%d) but "
268  "no input serial number specified. Exiting!",
269  m_num_devices);
270  }
271 
272  m_serial_no = serial_no;
273 }
274 
286 {
287  auto intrinsics = this->getIntrinsics(stream);
288 
289  vpCameraParameters cam;
290  double u0 = intrinsics.ppx;
291  double v0 = intrinsics.ppy;
292  double px = intrinsics.fx;
293  double py = intrinsics.fy;
295  double kdu = intrinsics.coeffs[0];
296  cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
297  } else {
298  cam.initPersProjWithoutDistortion(px, py, u0, v0);
299  }
300  return cam;
301 }
302 
309 rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
310 {
311  if (!m_device) {
312  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
313  "retrieve intrinsics. Exiting!");
314  }
315 
316  return m_device->get_stream_intrinsics(stream);
317 }
318 
326 rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const
327 {
328  if (!m_device) {
329  throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
330  "retrieve extrinsics. Exiting!");
331  }
332  if (!m_device->is_stream_enabled(from)) {
334  "RealSense Camera - stream (%d) is not enabled to "
335  "retrieve extrinsics. Exiting!",
336  (int)from);
337  }
338  if (!m_device->is_stream_enabled(to)) {
340  "RealSense Camera - stream (%d) is not enabled to "
341  "retrieve extrinsics. Exiting!",
342  (int)to);
343  }
344  return m_device->get_extrinsics(from, to);
345 }
346 
354 vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const
355 {
356  rs::extrinsics extrinsics = this->getExtrinsics(from, to);
359  for (unsigned int i = 0; i < 3; i++) {
360  t[i] = extrinsics.translation[i];
361  for (unsigned int j = 0; j < 3; j++)
362  R[i][j] = extrinsics.rotation[j * 3 + i]; // rotation is column-major order
363  }
364  vpHomogeneousMatrix fMt(t, R);
365  return fMt;
366 }
367 
373 {
374  if (m_device == NULL) {
375  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
376  }
377  if (!m_device->is_streaming()) {
378  open();
379  }
380 
381  m_device->wait_for_frames();
382 
383  // Retrieve grey image
384  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
385 }
386 
394 void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
395 {
396  if (m_device == NULL) {
397  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
398  }
399  if (!m_device->is_streaming()) {
400  open();
401  }
402 
403  m_device->wait_for_frames();
404 
405  // Retrieve grey image
406  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
407 
408  // Retrieve point cloud
409  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
410 }
411 
418 void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
419 {
420  if (m_device == NULL) {
421  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
422  }
423  if (!m_device->is_streaming()) {
424  open();
425  }
426 
427  m_device->wait_for_frames();
428 
429  // Retrieve point cloud
430  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
431 }
432 
438 {
439  if (m_device == NULL) {
440  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
441  }
442  if (!m_device->is_streaming()) {
443  open();
444  }
445 
446  m_device->wait_for_frames();
447 
448  // Retrieve color image
449  vp_rs_get_color_impl(m_device, m_intrinsics, color);
450 }
451 
462  std::vector<vpColVector> &pointcloud)
463 {
464  if (m_device == NULL) {
465  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
466  }
467  if (!m_device->is_streaming()) {
468  open();
469  }
470 
471  m_device->wait_for_frames();
472 
473  // Retrieve color image
474  vp_rs_get_color_impl(m_device, m_intrinsics, color);
475 
476  // Retrieve infrared image
477  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
478 
479  // Retrieve depth image
480  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
481 
482  // Retrieve point cloud
483  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
484 }
485 
496  std::vector<vpColVector> &pointcloud)
497 {
498  if (m_device == NULL) {
499  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
500  }
501  if (!m_device->is_streaming()) {
502  open();
503  }
504 
505  m_device->wait_for_frames();
506 
507  // Retrieve grey image
508  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
509 
510  // Retrieve infrared image
511  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
512 
513  // Retrieve depth image
514  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
515 
516  // Retrieve point cloud
517  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
518 }
519 
527 void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
528 {
529  if (m_device == NULL) {
530  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
531  }
532  if (!m_device->is_streaming()) {
533  open();
534  }
535 
536  m_device->wait_for_frames();
537 
538  // Retrieve color image
539  vp_rs_get_color_impl(m_device, m_intrinsics, color);
540 
541  // Retrieve point cloud
542  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
543 }
544 
562 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
563  std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
564  unsigned char *const data_infrared2, const rs::stream &stream_color,
565  const rs::stream &stream_depth, const rs::stream &stream_infrared,
566  const rs::stream &stream_infrared2)
567 {
568  if (m_device == NULL) {
569  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
570  }
571 
572  if (!m_device->is_streaming()) {
573  open();
574  }
575 
576  m_device->wait_for_frames();
577 
578  if (data_image != NULL) {
579  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
580  }
581 
582  if (data_depth != NULL) {
583  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
584 
585  if (data_pointCloud != NULL) {
586  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
587  }
588  }
589 
590  if (data_infrared != NULL) {
591  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
592  }
593 
594  if (data_infrared2 != NULL) {
595  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
596  }
597 }
598 
604 void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
605 {
606  m_useStreamPresets[stream] = true;
607  m_streamPresets[stream] = preset;
608 }
609 
637 void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
638 {
639  m_useStreamPresets[stream] = false;
640  m_streamParams[stream] = params;
641 }
642 
666 void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; }
667 
668 #ifdef VISP_HAVE_PCL
673 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
674 {
675  if (m_device == NULL) {
676  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
677  }
678  if (!m_device->is_streaming()) {
679  open();
680  }
681 
682  m_device->wait_for_frames();
683 
684  // Retrieve point cloud
685  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
686 }
687 
692 void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
693 {
694  if (m_device == NULL) {
695  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
696  }
697  if (!m_device->is_streaming()) {
698  open();
699  }
700 
701  m_device->wait_for_frames();
702 
703  // Retrieve point cloud
704  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
705 }
706 
712 void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
713 {
714  if (m_device == NULL) {
715  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
716  }
717  if (!m_device->is_streaming()) {
718  open();
719  }
720 
721  m_device->wait_for_frames();
722 
723  // Retrieve grey image
724  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
725 
726  // Retrieve point cloud
727  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
728 }
729 
735 void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
736 {
737  if (m_device == NULL) {
738  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
739  }
740  if (!m_device->is_streaming()) {
741  open();
742  }
743 
744  m_device->wait_for_frames();
745 
746  // Retrieve color image
747  vp_rs_get_color_impl(m_device, m_intrinsics, color);
748 
749  // Retrieve point cloud
750  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
751 }
752 
761  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
762 {
763  if (m_device == NULL) {
764  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
765  }
766  if (!m_device->is_streaming()) {
767  open();
768  }
769 
770  m_device->wait_for_frames();
771 
772  // Retrieve color image
773  vp_rs_get_color_impl(m_device, m_intrinsics, color);
774 
775  // Retrieve infrared image
776  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
777 
778  // Retrieve depth image
779  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
780 
781  // Retrieve point cloud
782  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
783 }
784 
793  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
794 {
795  if (m_device == NULL) {
796  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
797  }
798  if (!m_device->is_streaming()) {
799  open();
800  }
801 
802  m_device->wait_for_frames();
803 
804  // Retrieve grey image
805  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
806 
807  // Retrieve infrared image
808  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
809 
810  // Retrieve depth image
811  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
812 
813  // Retrieve point cloud
814  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
815 }
816 
825  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
826 {
827  if (m_device == NULL) {
828  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
829  }
830  if (!m_device->is_streaming()) {
831  open();
832  }
833 
834  m_device->wait_for_frames();
835 
836  // Retrieve color image
837  vp_rs_get_color_impl(m_device, m_intrinsics, color);
838 
839  // Retrieve infrared image
840  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
841 
842  // Retrieve depth image
843  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
844 
845  // Retrieve point cloud
846  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
847 }
848 
857  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
858 {
859  if (m_device == NULL) {
860  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
861  }
862  if (!m_device->is_streaming()) {
863  open();
864  }
865 
866  m_device->wait_for_frames();
867 
868  // Retrieve grey image
869  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
870 
871  // Retrieve infrared image
872  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);
873 
874  // Retrieve depth image
875  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);
876 
877  // Retrieve point cloud
878  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
879 }
880 
900 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
901  std::vector<vpColVector> *const data_pointCloud,
902  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud, unsigned char *const data_infrared,
903  unsigned char *const data_infrared2, const rs::stream &stream_color,
904  const rs::stream &stream_depth, const rs::stream &stream_infrared,
905  const rs::stream &stream_infrared2)
906 {
907  if (m_device == NULL) {
908  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
909  }
910 
911  if (!m_device->is_streaming()) {
912  open();
913  }
914 
915  m_device->wait_for_frames();
916 
917  if (data_image != NULL) {
918  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
919  }
920 
921  if (data_depth != NULL) {
922  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
923  }
924 
925  if (data_pointCloud != NULL) {
926  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
927  }
928 
929  if (pointcloud != NULL) {
930  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth);
931  }
932 
933  if (data_infrared != NULL) {
934  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
935  }
936 
937  if (data_infrared2 != NULL) {
938  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
939  }
940 }
941 
960 void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
961  std::vector<vpColVector> *const data_pointCloud,
962  pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud, unsigned char *const data_infrared,
963  unsigned char *const data_infrared2, const rs::stream &stream_color,
964  const rs::stream &stream_depth, const rs::stream &stream_infrared,
965  const rs::stream &stream_infrared2)
966 {
967  if (m_device == NULL) {
968  throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
969  }
970 
971  if (!m_device->is_streaming()) {
972  open();
973  }
974 
975  m_device->wait_for_frames();
976 
977  if (data_image != NULL) {
978  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
979  }
980 
981  if (data_depth != NULL) {
982  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
983  }
984 
985  if (data_pointCloud != NULL) {
986  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
987  }
988 
989  if (pointcloud != NULL) {
990  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
991  stream_depth);
992  }
993 
994  if (data_infrared != NULL) {
995  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
996  }
997 
998  if (data_infrared2 != NULL) {
999  vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1000  }
1001 }
1002 #endif
1003 
1024 std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
1025 {
1026  os << "Device name: " << rs.getHandler()->get_name() << std::endl;
1027  std::streamsize ss = std::cout.precision();
1028  for (int i = 0; i < 4; ++i) {
1029  auto stream = rs::stream(i);
1030  if (!rs.getHandler()->is_stream_enabled(stream))
1031  continue;
1032  auto intrin = rs.getHandler()->get_stream_intrinsics(stream);
1033  std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
1034  std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
1035  << ", distortion = " << intrin.model() << std::endl;
1036  }
1037  std::cout.precision(ss);
1038 
1039  return os;
1040 }
1041 
1042 #elif !defined(VISP_BUILD_SHARED_LIBS)
1043 // Work around to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has no
1044 // symbols
1045 void dummy_vpRealSense(){};
1046 #endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:529
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:449
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:452
rs::device * m_device
Definition: vpRealSense.h:447
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: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.