Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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!",
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
263void 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!",
270 }
271
272 m_serial_no = serial_no;
273}
274
286{
287 auto intrinsics = this->getIntrinsics(stream);
288
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
309rs::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
326rs::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
354vpHomogeneousMatrix 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
394void 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
418void 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
527void 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
562void 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
604void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
605{
606 m_useStreamPresets[stream] = true;
607 m_streamPresets[stream] = preset;
608}
609
637void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
638{
639 m_useStreamPresets[stream] = false;
640 m_streamParams[stream] = params;
641}
642
666void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; }
667
668#ifdef VISP_HAVE_PCL
673void 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
692void 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
712void 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
735void 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
900void 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
960void 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
1024std::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
1045void dummy_vpRealSense(){};
1046#endif
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
std::string m_serial_no
void setDeviceBySerialNumber(const std::string &serial_no)
virtual ~vpRealSense()
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void initStream()
std::map< rs::stream, bool > m_enableStreams
rs::device * m_device
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
rs::device * getHandler() const
Get access to device handler.
void acquire(std::vector< vpColVector > &pointcloud)
std::map< rs::stream, bool > m_useStreamPresets
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
rs::context m_context
std::map< rs::stream, rs::preset > m_streamPresets
float m_invalidDepthValue
rs::intrinsics getIntrinsics(const rs::stream &stream) const
std::map< rs::stream, vpRsStreamParams > m_streamParams
float m_max_Z
Maximal Z depth in meter.
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.