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