YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
realsense2Driver.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3 * All rights reserved.
4 *
5 * This software may be modified and distributed under the terms of the
6 * BSD-3-Clause license. See the accompanying LICENSE file for details.
7 */
8
9#include <cmath>
10#include <algorithm>
11#include <iomanip>
12#include <cstdint>
13
15#include <yarp/os/Value.h>
16#include <yarp/sig/ImageUtils.h>
17
18#include <librealsense2/rsutil.h>
19#include "realsense2Driver.h"
20
21using namespace yarp::dev;
22using namespace yarp::sig;
23using namespace yarp::os;
24
25using namespace std;
26
27namespace {
28YARP_LOG_COMPONENT(REALSENSE2, "yarp.device.realsense2")
29}
30
31
32constexpr char accuracy [] = "accuracy";
33constexpr char clipPlanes [] = "clipPlanes";
34constexpr char depthRes [] = "depthResolution";
35constexpr char rgbRes [] = "rgbResolution";
36constexpr char framerate [] = "framerate";
37constexpr char enableEmitter [] = "enableEmitter";
38constexpr char needAlignment [] = "needAlignment";
39constexpr char alignmentFrame [] = "alignmentFrame";
40
41
53
54static const std::map<std::string, rs2_stream> stringRSStreamMap {
55 {"None", RS2_STREAM_ANY},
56 {"RGB", RS2_STREAM_COLOR},
57 {"Depth", RS2_STREAM_DEPTH}
58};
59
60
61static std::string get_device_information(const rs2::device& dev)
62{
63
64 std::stringstream ss;
65 ss << "Device information: " << std::endl;
66
67 for (int i = 0; i < static_cast<int>(RS2_CAMERA_INFO_COUNT); i++)
68 {
69 auto info_type = static_cast<rs2_camera_info>(i);
70 ss << " " << std::left << std::setw(20) << info_type << " : ";
71
72 if (dev.supports(info_type))
73 ss << dev.get_info(info_type) << std::endl;
74 else
75 ss << "N/A" << std::endl;
76 }
77 return ss.str();
78}
79
80
81static void print_supported_options(const rs2::sensor& sensor)
82{
83 // Sensors usually have several options to control their properties
84 // such as Exposure, Brightness etc.
85
86 if (sensor.is<rs2::depth_sensor>())
87 yCInfo(REALSENSE2) << "Depth sensor supports the following options:\n";
88 else if (sensor.get_stream_profiles()[0].stream_type() == RS2_STREAM_COLOR)
89 yCInfo(REALSENSE2) << "RGB camera supports the following options:\n";
90 else
91 yCInfo(REALSENSE2) << "Sensor supports the following options:\n";
92
93 // The following loop shows how to iterate over all available options
94 // Starting from 0 until RS2_OPTION_COUNT (exclusive)
95 for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
96 {
97 auto option_type = static_cast<rs2_option>(i);
98 //SDK enum types can be streamed to get a string that represents them
99
100 // To control an option, use the following api:
101
102 // First, verify that the sensor actually supports this option
103 if (sensor.supports(option_type))
104 {
105 std::cout << " " << option_type;
106 std::cout << std::endl;
107
108 // Get a human readable description of the option
109 const char* description = sensor.get_option_description(option_type);
110 std::cout << " Description : " << description << std::endl;
111
112 // Get the current value of the option
113 float current_value = sensor.get_option(option_type);
114 std::cout << " Current Value : " << current_value << std::endl;
115 }
116 }
117
118 std::cout<<std::endl;
119}
120
121static bool isSupportedFormat(const rs2::sensor &sensor, const int width, const int height, const int fps, bool verbose = false)
122{
123 bool ret = false;
124 std::vector<rs2::stream_profile> stream_profiles = sensor.get_stream_profiles();
125
126 std::map<std::pair<rs2_stream, int>, int> unique_streams;
127 for (auto&& sp : stream_profiles)
128 {
129 unique_streams[std::make_pair(sp.stream_type(), sp.stream_index())]++;
130 }
131
132 if (verbose)
133 {
134 std::cout << "Sensor consists of " << unique_streams.size() << " streams: " << std::endl;
135 for (size_t i = 0; i < unique_streams.size(); i++)
136 {
137 auto it = unique_streams.begin();
138 std::advance(it, i);
139 std::cout << " - " << it->first.first << " #" << it->first.second << std::endl;
140 }
141 std::cout << "Sensor provides the following stream profiles:" << std::endl;
142 }
143
144 //Next, we go over all the stream profiles and print the details of each one
145
146 int profile_num = 0;
147 for (const rs2::stream_profile& stream_profile : stream_profiles)
148 {
149 if (verbose)
150 {
152
153 int stream_index = stream_profile.stream_index();
154
155 std::cout << std::setw(3) << profile_num << ": " << stream_data_type << " #" << stream_index;
156 }
157
158 // As noted, a stream is an abstraction.
159 // In order to get additional data for the specific type of a
160 // stream, a mechanism of "Is" and "As" is provided:
161 if (stream_profile.is<rs2::video_stream_profile>()) //"Is" will test if the type tested is of the type given
162 {
163 // "As" will try to convert the instance to the given type
164 rs2::video_stream_profile video_stream_profile = stream_profile.as<rs2::video_stream_profile>();
165
166 // After using the "as" method we can use the new data type
167 // for additinal operations:
168 if (verbose)
169 {
170 std::cout << " (Video Stream: " << video_stream_profile.format() << " " <<
171 video_stream_profile.width() << "x" << video_stream_profile.height() << "@ " << video_stream_profile.fps() << "Hz)";
172 std::cout << std::endl;
173 }
174
175 if(video_stream_profile.width() == width && video_stream_profile.height() == height && video_stream_profile.fps() == fps)
176 ret=true;
177 }
178 profile_num++;
179 }
180
181
182 return ret;
183}
184
185static bool optionPerc2Value(rs2_option option,const rs2::sensor* sensor, const float& perc, float& value)
186{
187 if (!sensor)
188 {
189 return false;
190 }
191 try
192 {
193 rs2::option_range optionRange = sensor->get_option_range(option);
194 value =(float) (perc * (optionRange.max - optionRange.min) + optionRange.min);
195
196 }
197 catch (const rs2::error& e)
198 {
199 // Some options can only be set while the camera is streaming,
200 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
201 yCError(REALSENSE2) << "Failed to get option " << option << " range. (" << e.what() << ")";
202 return false;
203 }
204
205 return true;
206}
207
208static bool optionValue2Perc(rs2_option option,const rs2::sensor* sensor, float& perc, const float& value)
209{
210 if (!sensor)
211 {
212 return false;
213 }
214 try
215 {
216 rs2::option_range optionRange = sensor->get_option_range(option);
217 perc =(float) ((value - optionRange.min) / (optionRange.max - optionRange.min));
218
219 }
220 catch (const rs2::error& e)
221 {
222 // Some options can only be set while the camera is streaming,
223 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
224 yCError(REALSENSE2) << "Failed to get option " << option << " range. (" << e.what() << ")";
225 return false;
226 }
227
228 return true;
229}
230
231
232
233static bool setOption(rs2_option option,const rs2::sensor* sensor, float value)
234{
235
236 if (!sensor)
237 {
238 return false;
239 }
240
241 // First, verify that the sensor actually supports this option
242 if (!sensor->supports(option))
243 {
244 yCError(REALSENSE2) << "The option" << rs2_option_to_string(option) << "is not supported by this sensor";
245 return false;
246 }
247
248 // To set an option to a different value, we can call set_option with a new value
249 try
250 {
251 sensor->set_option(option, value);
252 }
253 catch (const rs2::error& e)
254 {
255 // Some options can only be set while the camera is streaming,
256 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
257 yCError(REALSENSE2) << "Failed to set option " << rs2_option_to_string(option) << ". (" << e.what() << ")";
258 return false;
259 }
260 return true;
261}
262
263static bool getOption(rs2_option option,const rs2::sensor *sensor, float &value)
264{
265 if (!sensor)
266 {
267 return false;
268 }
269
270 // First, verify that the sensor actually supports this option
271 if (!sensor->supports(option))
272 {
273 yCError(REALSENSE2) << "The option" << rs2_option_to_string(option) << "is not supported by this sensor";
274 return false;
275 }
276
277 // To set an option to a different value, we can call set_option with a new value
278 try
279 {
280 value = sensor->get_option(option);
281 }
282 catch (const rs2::error& e)
283 {
284 // Some options can only be set while the camera is streaming,
285 // and generally the hardware might fail so it is good practice to catch exceptions from set_option
286 yCError(REALSENSE2) << "Failed to get option " << rs2_option_to_string(option) << ". (" << e.what() << ")";
287 return false;
288 }
289 return true;
290}
291
292static int pixFormatToCode(const rs2_format p)
293{
294 switch(p)
295 {
296 case (RS2_FORMAT_RGB8):
297 return VOCAB_PIXEL_RGB;
298
299 case (RS2_FORMAT_BGR8):
300 return VOCAB_PIXEL_BGR;
301
302 case (RS2_FORMAT_Z16):
303 return VOCAB_PIXEL_MONO16;
304
306 return VOCAB_PIXEL_MONO16;
307
308 case (RS2_FORMAT_RGBA8):
309 return VOCAB_PIXEL_RGBA;
310
311 case (RS2_FORMAT_BGRA8):
312 return VOCAB_PIXEL_BGRA;
313
314 case (RS2_FORMAT_Y8):
315 return VOCAB_PIXEL_MONO;
316
317 case (RS2_FORMAT_Y16):
318 return VOCAB_PIXEL_MONO16;;
319
320 case (RS2_FORMAT_RAW16):
321 return VOCAB_PIXEL_MONO16;
322
323 case (RS2_FORMAT_RAW8):
324 return VOCAB_PIXEL_MONO;
325 default:
326 return VOCAB_PIXEL_INVALID;
327
328 }
329}
330
331static size_t bytesPerPixel(const rs2_format format)
332{
333 size_t bytes_per_pixel = 0;
334 switch (format)
335 {
336 case RS2_FORMAT_RAW8:
337 case RS2_FORMAT_Y8:
338 bytes_per_pixel = 1;
339 break;
340 case RS2_FORMAT_Z16:
342 case RS2_FORMAT_Y16:
343 case RS2_FORMAT_RAW16:
344 bytes_per_pixel = 2;
345 break;
346 case RS2_FORMAT_RGB8:
347 case RS2_FORMAT_BGR8:
348 bytes_per_pixel = 3;
349 break;
350 case RS2_FORMAT_RGBA8:
351 case RS2_FORMAT_BGRA8:
352 bytes_per_pixel = 4;
353 break;
354 default:
355 break;
356 }
357 return bytes_per_pixel;
358}
359
361{
362 switch (dist)
363 {
365 return YarpDistortion::YARP_PLUMB_BOB;
366 default:
367 /*
368 * If the coefficient are all zero the image is undistorted. For now is set to plumb bob since all the devices that uses it are configured with plumb bob.
369 * An issue will be open to fix this bug and set it to undistorted.
370 */
371 if (values.coeffs[0] == 0.0 && values.coeffs[1] == 0.0 && values.coeffs[2] == 0.0 && values.coeffs[3] == 0.0 && values.coeffs[4] == 0.0)
372 {
373 return YarpDistortion::YARP_DISTORTION_NONE;
374 }
375 else
376 {
377 return YarpDistortion::YARP_UNSUPPORTED;
378 }
379 }
380}
381
382static void settingErrorMsg(const string& error, bool& ret)
383{
384 yCError(REALSENSE2) << error.c_str();
385 ret = false;
386}
387
388static bool setIntrinsic(Property& intrinsic, const rs2_intrinsics &values)
389{
391 params.focalLengthX = values.fx;
392 params.focalLengthY = values.fy;
393 params.principalPointX = values.ppx;
394 params.principalPointY = values.ppy;
395 // distortion model
396 params.distortionModel.type = rsDistToYarpDist(values.model, values);
397 params.distortionModel.k1 = values.coeffs[0];
398 params.distortionModel.k2 = values.coeffs[1];
399 params.distortionModel.t1 = values.coeffs[2];
400 params.distortionModel.t2 = values.coeffs[3];
401 params.distortionModel.k3 = values.coeffs[4];
402 params.toProperty(intrinsic);
403 return true;
404}
405
407{
408
409 if (extrinsic.cols() != 4 || extrinsic.rows() != 4)
410 {
411 yCError(REALSENSE2) << "Extrinsic matrix is not 4x4";
412 return false;
413 }
414
415 extrinsic.eye();
416
417 for (size_t j=0; j<extrinsic.rows() - 1; j++)
418 {
419 for (size_t i=0; i<extrinsic.cols() - 1; i++)
420 {
421 extrinsic[j][i] = values.rotation[i + j*extrinsic.cols()];
422 }
423 }
424
425 extrinsic[0][3] = values.translation[0];
426 extrinsic[1][3] = values.translation[1];
427 extrinsic[2][3] = values.translation[2];
428
429 return false;
430}
431
432realsense2Driver::realsense2Driver() : m_depth_sensor(nullptr), m_color_sensor(nullptr),
433 m_paramParser(), m_verbose(false),
434 m_initialized(false), m_stereoMode(false),
435 m_needAlignment(true), m_fps(0),
436 m_scale(0.0)
437{
438 // realsense SDK already provides them
442
443
451}
452
454{
455 try
456 {
457 m_profile = m_pipeline.start(m_cfg);
458 }
459 catch (const rs2::error& e)
460 {
461 yCError(REALSENSE2) << "Failed to start the pipeline:"<< "(" << e.what() << ")";
462 m_lastError = e.what();
463 return false;
464 }
465 return true;
466}
467
469{
470 try
471 {
472 m_pipeline.stop();
473 }
474 catch (const rs2::error& e)
475 {
476 yCError(REALSENSE2) << "Failed to stop the pipeline:"<< "(" << e.what() << ")";
477 m_lastError = e.what();
478 return false;
479 }
480 return true;
481}
482
484{
485 std::lock_guard<std::mutex> guard(m_mutex);
486 if (!pipelineShutdown())
487 return false;
488
489 return pipelineStartup();
490
491}
492
494{
497
500 }
501 else
502 {
503 if (m_initialized)
504 {
505 fallback();
506 }
507
508 }
509
510 if (!pipelineRestart())
511 return false;
512
513 m_fps = _fps;
514
516
517 return true;
518
519}
520
522{
525 yCWarning(REALSENSE2)<<"Format not supported, use --verbose for more details. Setting the fallback format";
526 std::cout<<"COLOR: "<<m_color_intrin.width<<"x"<<m_color_intrin.height<<" fps: "<<m_fps<<std::endl;
527 std::cout<<"DEPTH: "<<m_depth_intrin.width<<"x"<<m_depth_intrin.height<<" fps: "<<m_fps<<std::endl;
528}
529
531{
532 // Print available devices
533 auto devices = m_ctx.query_devices();
534 if (devices.size() == 0)
535 {
536 yCError(REALSENSE2) << "No device connected, please connect a RealSense device";
537 return false;
538 }
539
540 std::string selectedcamera_type;
541
542 //Check serialnumbers
543 yCInfo(REALSENSE2) << "Found the following " << devices.size() << " devices:";
544 for (auto&& dev : devices)
545 {
546 std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
547 std::string name = dev.get_info(RS2_CAMERA_INFO_NAME);
548 yCInfo(REALSENSE2) << "Device: " << name << " | Serial: " << serial;
549 if (m_serialnumber == serial)
550 {
551 selectedcamera_type = name;
552 }
553 }
554
555 if (m_serialnumber!="")
556 {
557 //Enable only the selected device
558 m_cfg.enable_device(m_serialnumber);
559 }
560 else
561 {
562 //No serialnumber chosen, just get the first available device.
563 //Using the device_hub we can block the program until a device connects
564 rs2::device_hub device_hub(m_ctx);
565 yCInfo(REALSENSE2) << "Waiting for device to become avilable...";
566 m_device = device_hub.wait_for_device();
567 yCInfo(REALSENSE2) << "...device ready";
568
569 // Get the type of the chosen camera
570 selectedcamera_type= std::string(m_device.get_info(RS2_CAMERA_INFO_NAME));
571 }
572
573 //If it is a D405, set the flag. D405 cameras are handled differently
574 bool is_d405 = (selectedcamera_type.find("D405") != std::string::npos);
575
576 // Extract RGB and depth resolution
577 double colorW;
578 double colorH;
579 double depthW;
580 double depthH;
581 if (is_d405)
582 {
583 if (!params_map[depthRes].isSetting)
584 {
585 yCError(REALSENSE2)<<"Missing depthResolution from [SETTINGS]";
586 return false;
587 }
588
589 // A D405 camera inherits the RGB resolution from the that of the depth sensor
590 colorW = depthW = params_map[depthRes].val[0].asFloat64();
591 colorH = depthH = params_map[depthRes].val[1].asFloat64();
592 }
593 else
594 {
595 if (!params_map[rgbRes].isSetting)
596 {
597 yCError(REALSENSE2)<<"Missing rgbResolution from [SETTINGS]";
598 return false;
599 }
600 if (!params_map[depthRes].isSetting)
601 {
602 yCError(REALSENSE2)<<"Missing depthResolution from [SETTINGS]";
603 return false;
604 }
605
606 colorW = params_map[rgbRes].val[0].asFloat64();
607 colorH = params_map[rgbRes].val[1].asFloat64();
608 depthW = params_map[depthRes].val[0].asFloat64();
609 depthH = params_map[depthRes].val[1].asFloat64();
610 }
611
614 if (m_stereoMode) {
617 }
618 if (!pipelineStartup())
619 return false;
620 m_initialized = true;
621
622 // Update the selected device
623 m_device = m_profile.get_device();
624 if (m_verbose)
626
627 // Camera warmup - Dropped frames to allow stabilization
628 yCInfo(REALSENSE2) << "Sensor warm-up...";
629 for (int i = 0; i < 30; i++)
630 {
631 try
632 {
633 m_pipeline.wait_for_frames();
634 }
635 catch (const rs2::error& e)
636 {
637 yCError(REALSENSE2) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
638 m_lastError = e.what();
639 }
640 }
641 yCInfo(REALSENSE2) << "Device ready!";
642
643 // Given a device, we can query its sensors using:
644 m_sensors = m_device.query_sensors();
645
646 yCInfo(REALSENSE2) << "Device consists of" << m_sensors.size() << "sensors. More infos using --verbose option";
647 if (m_verbose)
648 {
649 for (const auto & m_sensor : m_sensors)
650 {
652 }
653 }
654
655 if (is_d405)
656 {
657 // A D405 camera only exposes rs2::depth_sensor depth sensor although it also streams RGB images
658 // Hence, we use the pointer to the depth sensor to access the RGB-related options
659 for (auto & m_sensor : m_sensors)
660 {
661 if (m_sensor.is<rs2::depth_sensor>())
663 }
665 }
666 else
667 {
668 for (auto & m_sensor : m_sensors)
669 {
670 if (m_sensor.is<rs2::depth_sensor>())
672 else if (m_sensor.get_stream_profiles()[0].stream_type() == RS2_STREAM_COLOR)
674 }
675 }
676
677 // Retrieve depth scaling factor
679 {
680 yCError(REALSENSE2) << "Failed to retrieve scale";
681 return false;
682 }
683
684 // Get stream intrinsics & extrinsics
686 return true;
687}
688
690{
691 rs2::pipeline_profile pipeline_profile = m_pipeline.get_active_profile();
692 rs2::video_stream_profile depth_stream_profile = rs2::video_stream_profile(pipeline_profile.get_stream(RS2_STREAM_DEPTH));
693 rs2::video_stream_profile color_stream_profile = rs2::video_stream_profile(pipeline_profile.get_stream(RS2_STREAM_COLOR));
694
695 m_depth_intrin = depth_stream_profile.get_intrinsics();
696 m_color_intrin = color_stream_profile.get_intrinsics();
699
700 if (m_stereoMode) {
701 rs2::video_stream_profile infrared_stream_profile = rs2::video_stream_profile(pipeline_profile.get_stream(RS2_STREAM_INFRARED));
703 }
704
705}
706
707
709{
710 bool ret = true;
711 //ACCURACY
712 if (params_map[accuracy].isSetting && ret)
713 {
714 if (!params_map[accuracy].val[0].isFloat64() )
715 settingErrorMsg("Param " + params_map[accuracy].name + " is not a double as it should be.", ret);
716
717 if (! setDepthAccuracy(params_map[accuracy].val[0].asFloat64() ) )
718 settingErrorMsg("Setting param " + params_map[accuracy].name + " failed... quitting.", ret);
719 }
720
721 //CLIP_PLANES
722 if (params_map[clipPlanes].isSetting && ret)
723 {
724 if (!params_map[clipPlanes].val[0].isFloat64() )
725 settingErrorMsg("Param " + params_map[clipPlanes].name + " is not a double as it should be.", ret);
726
727 if (!params_map[clipPlanes].val[1].isFloat64() )
728 settingErrorMsg("Param " + params_map[clipPlanes].name + " is not a double as it should be.", ret);
729
730 if (! setDepthClipPlanes(params_map[clipPlanes].val[0].asFloat64(), params_map[clipPlanes].val[1].asFloat64() ) )
731 settingErrorMsg("Setting param " + params_map[clipPlanes].name + " failed... quitting.", ret);
732 }
733
734 //FRAMERATE
735 if (params_map[framerate].isSetting && ret)
736 {
737 if (!params_map[framerate].val[0].isInt32() )
738 settingErrorMsg("Param " + params_map[framerate].name + " is not a int as it should be.", ret);
739 else
740 m_fps = params_map[framerate].val[0].asInt32();
741 }
742 else
743 {
744 yCWarning(REALSENSE2) << "Framerate not specified... setting 30 fps by default";
745 m_fps = 30;
746 }
747
748 //EMITTER
749 if (params_map[enableEmitter].isSetting && ret)
750 {
751 Value& v = params_map[enableEmitter].val[0];
752
753 if (!v.isBool())
754 {
755 settingErrorMsg("Param " + params_map[enableEmitter].name + " is not a bool as it should be.", ret);
756 return false;
757 }
759 {
760 settingErrorMsg("Setting param " + params_map[enableEmitter].name + " failed... quitting.", ret);
761 }
762 }
763
764 //ALIGNMENT
765 if (params_map[needAlignment].isSetting && ret)
766 {
767 yCWarning(REALSENSE2) << "needAlignment parameter is deprecated, use alignmentFrame instead.";
768 Value& v = params_map[needAlignment].val[0];
769 if (!v.isBool())
770 {
771 settingErrorMsg("Param " + params_map[needAlignment].name + " is not a bool as it should be.", ret);
772 return false;
773 }
774
777 }
778
779 if (params_map[alignmentFrame].isSetting && ret)
780 {
781 Value& v = params_map[alignmentFrame].val[0];
782 auto alignmentFrameStr = v.asString();
783 if (!v.isString())
784 {
785 settingErrorMsg("Param " + params_map[alignmentFrame].name + " is not a string as it should be.", ret);
786 return false;
787 }
788
790 settingErrorMsg("Value "+alignmentFrameStr+" not allowed for " + params_map[alignmentFrame].name + " see documentation for supported values.", ret);
791 return false;
792 }
793
795 }
796
797 //DEPTH_RES
798 if (params_map[depthRes].isSetting && ret)
799 {
800 Value p1, p2;
801 p1 = params_map[depthRes].val[0];
802 p2 = params_map[depthRes].val[1];
803
804 if (!p1.isInt32() || !p2.isInt32() )
805 {
806 settingErrorMsg("Param " + params_map[depthRes].name + " is not a int as it should be.", ret);
807 }
808
809 if (! setDepthResolution(p1.asInt32(), p2.asInt32()))
810 {
811 settingErrorMsg("Setting param " + params_map[depthRes].name + " failed... quitting.", ret);
812 }
813 }
814
815 //RGB_RES
816 if (params_map[rgbRes].isSetting && ret)
817 {
818 Value p1, p2;
819 p1 = params_map[rgbRes].val[0];
820 p2 = params_map[rgbRes].val[1];
821
822 if (!p1.isInt32() || !p2.isInt32() )
823 {
824 settingErrorMsg("Param " + params_map[rgbRes].name + " is not a int as it should be.", ret);
825 }
826
827 if (! setRgbResolution(p1.asInt32(), p2.asInt32()))
828 {
829 settingErrorMsg("Setting param " + params_map[rgbRes].name + " failed... quitting.", ret);
830 }
831 }
832
833 return ret;
834}
835
836
838{
839 std::vector<RGBDSensorParamParser::RGBDParam*> params;
840 params.reserve(params_map.size());
841 for (auto& p:params_map)
842 {
843 params.push_back(&(p.second));
844 }
845 //Manage depth quantization parameter
846 if(config.check("QUANT_PARAM")) {
848 quantCfg.fromString(config.findGroup("QUANT_PARAM").toString());
850 if (quantCfg.check("depth_quant")) {
851 m_depthDecimalNum = quantCfg.find("depth_quant").asInt32();
852 }
853 }
854
855 if(config.check("rotateImage180")){
856 m_rotateImage180 = config.find("rotateImage180").asBool();
857 if (m_rotateImage180) {
858 yCInfo(REALSENSE2) << "parameter rotateImage180 enabled, the image is rotated";
859 }
860 }
861
862 // Manage depth hole-filling filter parameters
863 if (config.check("DEPTH_FILTER")) {
865 filterCfg.fromString(config.findGroup("DEPTH_FILTER").toString());
866 if (filterCfg.check("enableHoleFilling") && filterCfg.find("enableHoleFilling").asBool()) {
867 m_enableHoleFilling = true;
868 int mode = 1; // default: farest_from_around
869 if (filterCfg.check("holeFillingMode")) {
870 mode = filterCfg.find("holeFillingMode").asInt32();
871 }
872 m_holeFillingFilter.set_option(RS2_OPTION_HOLES_FILL, static_cast<float>(mode));
873 yCInfo(REALSENSE2) << "Depth hole-filling filter enabled with mode" << mode;
874 }
875 }
876
877 if(config.check("rotateIRImage180")){
878 m_rotateIRImage180 = config.find("rotateIRImage180").asBool();
879 if (m_rotateIRImage180) {
880 yCInfo(REALSENSE2) << "parameter rotateIRImage180 enabled, infrared images are rotated";
881 }
882 }
883 m_verbose = config.check("verbose");
884
885 if (config.check("serialnumber")) {
886 m_serialnumber = config.find("serialnumber").toString();
887 yCInfo(REALSENSE2) << "Desired device serialnumber:" << m_serialnumber;
888 }
889
890 if (config.check("stereoMode")) {
891 m_stereoMode = config.find("stereoMode").asBool();
892 }
893
894 if (!m_paramParser.parseParam(config, params))
895 {
896 yCError(REALSENSE2) << "Failed to parse the parameters";
897 return false;
898 }
899
901 {
902 yCError(REALSENSE2) << "Failed to initialize the realsense device";
903 return false;
904 }
905
906 // setting Parameters
907 return setParams();
908
909}
910
912{
914 return true;
915}
916
918{
919 return m_color_intrin.height;
920}
921
923{
924 return m_color_intrin.width;
925}
926
928{
929 yCWarning(REALSENSE2) << "getRgbSupportedConfigurations not implemented yet";
930 return false;
931}
932
933bool realsense2Driver::getRgbResolution(int &width, int &height)
934{
935 width = m_color_intrin.width;
936 height = m_color_intrin.height;
937 return true;
938}
939
940bool realsense2Driver::setDepthResolution(int width, int height)
941{
944 else
945 {
946 if (m_initialized)
947 {
948 fallback();
949 return false;
950 }
951 }
952
953 if (!pipelineRestart())
954 return false;
955
957 return true;
958}
959
960bool realsense2Driver::setRgbResolution(int width, int height)
961{
962 bool fail = true;
965 fail = false;
966 if (m_stereoMode)
967 {
969 {
972 }
973 else
974 {
975 fail = true;
976 }
977 }
978 }
979
980 if (m_initialized && fail)
981 {
982 fallback();
983 return false;
984 }
985
986 if (!pipelineRestart())
987 return false;
988
990 return true;
991}
992
993
994bool realsense2Driver::setRgbFOV(double horizontalFov, double verticalFov)
995{
996 // It seems to be not available...
997 return false;
998}
999
1000bool realsense2Driver::setDepthFOV(double horizontalFov, double verticalFov)
1001{
1002 // It seems to be not available...
1003 return false;
1004}
1005
1007{
1008 std::lock_guard<std::mutex> guard(m_mutex);
1010 if (ok) {
1011 m_scale = accuracy;
1012 }
1013 return ok;
1014}
1015
1016bool realsense2Driver::getRgbFOV(double &horizontalFov, double &verticalFov)
1017{
1018 float fov[2];
1019 rs2_fov(&m_color_intrin, fov);
1020 horizontalFov = fov[0];
1021 verticalFov = fov[1];
1022 return true;
1023}
1024
1026{
1027 yCWarning(REALSENSE2) << "Mirroring not supported";
1028 return false;
1029}
1030
1032{
1033 yCWarning(REALSENSE2) << "Mirroring not supported";
1034 return false;
1035}
1036
1038{
1039 return setIntrinsic(intrinsic, m_color_intrin);
1040}
1041
1043{
1044 return m_depth_intrin.height;
1045}
1046
1048{
1049 return m_depth_intrin.width;
1050}
1051
1052bool realsense2Driver::getDepthFOV(double& horizontalFov, double& verticalFov)
1053{
1054 float fov[2];
1055 rs2_fov(&m_depth_intrin, fov);
1056 horizontalFov = fov[0];
1057 verticalFov = fov[1];
1058 return true;
1059}
1060
1062{
1063 return setIntrinsic(intrinsic, m_depth_intrin);;
1064}
1065
1067{
1068 float accuracy = 0.0;
1070 {
1071 return accuracy;
1072 }
1073 return 0.0;
1074}
1075
1076bool realsense2Driver::getDepthClipPlanes(double& nearPlane, double& farPlane)
1077{
1078 if (params_map[clipPlanes].isDescription)
1079 {
1080 nearPlane = params_map[clipPlanes].val[0].asFloat64();
1081 farPlane = params_map[clipPlanes].val[1].asFloat64();
1082 return true;
1083 }
1084
1085 bool ret = getOption(RS2_OPTION_MIN_DISTANCE, m_depth_sensor, (float&) nearPlane);
1086 ret &= getOption(RS2_OPTION_MAX_DISTANCE, m_depth_sensor, (float&) farPlane);
1087 return ret;
1088}
1089
1090bool realsense2Driver::setDepthClipPlanes(double nearPlane, double farPlane)
1091{
1092 if (params_map[clipPlanes].isDescription)
1093 {
1094 return false;
1095 }
1096 bool ret = setOption(RS2_OPTION_MIN_DISTANCE, m_depth_sensor, nearPlane);
1098 return ret;
1099}
1100
1102{
1103 yCWarning(REALSENSE2) << "Mirroring not supported";
1104 return false;
1105}
1106
1108{
1109 yCWarning(REALSENSE2) << "Mirroring not supported";
1110 return false;
1111}
1112
1117
1119{
1120 std::lock_guard<std::mutex> guard(m_mutex);
1121 rs2::frameset data;
1122 try
1123 {
1124 data = m_pipeline.wait_for_frames();
1125 }
1126 catch (const rs2::error& e)
1127 {
1128 yCError(REALSENSE2) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
1129 m_lastError = e.what();
1130 return false;
1131 }
1133 {
1134 rs2::align align(m_alignment_stream);
1135 data = align.process(data);
1136 }
1137 return getImage(rgbImage, timeStamp, data);
1138}
1139
1141{
1142 std::lock_guard<std::mutex> guard(m_mutex);
1143 rs2::frameset data;
1144 try
1145 {
1146 data = m_pipeline.wait_for_frames();
1147 }
1148 catch (const rs2::error& e)
1149 {
1150 yCError(REALSENSE2) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
1151 m_lastError = e.what();
1152 return false;
1153 }
1155 {
1156 rs2::align align(m_alignment_stream);
1157 data = align.process(data);
1158 }
1160 {
1161 data = data.apply_filter(m_holeFillingFilter);
1162 }
1163 return getImage(depthImage, timeStamp, data);
1164}
1165
1167{
1168 rs2::video_frame color_frm = sourceFrame.get_color_frame();
1169 rs2_format format = color_frm.get_profile().format();
1170
1171 int pixCode = pixFormatToCode(format);
1172 size_t mem_to_wrt = color_frm.get_width() * color_frm.get_height() * bytesPerPixel(format);
1173
1175 {
1176 yCError(REALSENSE2) << "Pixel Format not recognized";
1177 return false;
1178 }
1179
1180 Frame.setPixelCode(pixCode);
1181 Frame.resize(m_color_intrin.width, m_color_intrin.height);
1182
1183 if ((size_t) Frame.getRawImageSize() != mem_to_wrt)
1184 {
1185 yCError(REALSENSE2) << "Device and local copy data size doesn't match";
1186 return false;
1187 }
1188 if (m_rotateImage180) {
1189 for (int i = 0; i < (color_frm.get_width() * color_frm.get_height()); i++) {
1190 for (size_t pixelIndex = 0; pixelIndex < bytesPerPixel(format); pixelIndex++) {
1191 ((char *)Frame.getRawImage())[i * bytesPerPixel(format) + pixelIndex] = ((char *)color_frm.get_data())[
1192 ( Frame.getRawImageSize() -(( i-1) * bytesPerPixel(format))) + pixelIndex];
1193 }
1194 }
1195 } else {
1196 memcpy((void*)Frame.getRawImage(), (void*)color_frm.get_data(), mem_to_wrt);
1197 }
1199 if (timeStamp != nullptr)
1200 {
1201 *timeStamp = m_rgb_stamp;
1202 }
1203 return true;
1204}
1205
1206bool realsense2Driver::getImage(depthImage& Frame, Stamp *timeStamp, const rs2::frameset &sourceFrame)
1207{
1208 rs2::depth_frame depth_frm = sourceFrame.get_depth_frame();
1209 rs2_format format = depth_frm.get_profile().format();
1210
1211 int pixCode = pixFormatToCode(format);
1212
1213 int w = depth_frm.get_width();
1214 int h = depth_frm.get_height();
1215
1217 {
1218 yCError(REALSENSE2) << "Pixel Format not recognized";
1219 return false;
1220 }
1221
1222 Frame.resize(w, h);
1223
1224 float* rawImage = &Frame.pixel(0,0);
1225 const auto * rawImageRs =(const uint16_t *) depth_frm.get_data();
1226 double nearPlane;
1227 double farPlane;
1228 int intTemp;
1229 getDepthClipPlanes(nearPlane, farPlane);
1230 float powCoeff = pow(10.0f, (float) m_depthDecimalNum);
1231 for(int i = 0; i < w * h; i++)
1232 {
1233 if (m_rotateImage180) {
1234 rawImage[i] = m_scale * rawImageRs[(w * h) - i -1];
1235 }else {
1236 rawImage[i] = m_scale * rawImageRs[i];
1237 }
1239 intTemp = (int) (rawImage[i] * powCoeff);
1240 rawImage[i] = ((float) intTemp) / powCoeff;
1241 }
1242 }
1243
1245 if (timeStamp != nullptr)
1246 {
1247 *timeStamp = m_depth_stamp;
1248 }
1249 return true;
1250}
1251
1253{
1254 std::lock_guard<std::mutex> guard(m_mutex);
1255 rs2::frameset data;
1256 try
1257 {
1258 data = m_pipeline.wait_for_frames();
1259 }
1260 catch (const rs2::error& e)
1261 {
1262 yCError(REALSENSE2) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
1263 m_lastError = e.what();
1264 return false;
1265 }
1266 if (m_alignment_stream != RS2_STREAM_ANY) // RS2_STREAM_ANY is used as no-alignment-needed value.
1267 {
1268 rs2::align align(m_alignment_stream);
1269 data = align.process(data);
1270 }
1272 {
1273 data = data.apply_filter(m_holeFillingFilter);
1274 }
1275 return getImage(colorFrame, colorStamp, data) && getImage(depthFrame, depthStamp, data);
1276}
1277
1282
1284{
1285 return m_lastError;
1286}
1287
1289{
1291 camera->busType = BUS_USB;
1292 return true;
1293}
1294
1295bool realsense2Driver::hasFeature(int feature, bool* hasFeature)
1296{
1298 f = static_cast<cameraFeature_id_t>(feature);
1300 {
1301 return false;
1302 }
1303
1304 *hasFeature = std::find(m_supportedFeatures.begin(), m_supportedFeatures.end(), f) != m_supportedFeatures.end();
1305
1306 return true;
1307}
1308
1309bool realsense2Driver::setFeature(int feature, double value)
1310{
1311 bool b = false;
1312 if (!hasFeature(feature, &b) || !b)
1313 {
1314 yCError(REALSENSE2) << "Feature not supported!";
1315 return false;
1316 }
1317
1318 float valToSet = 0.0;
1319 b = false;
1320 auto f = static_cast<cameraFeature_id_t>(feature);
1321 switch(f)
1322 {
1325 {
1327 if (m_stereoMode)
1328 {
1330 {
1332 }
1333 }
1334 }
1335 break;
1336 case YARP_FEATURE_GAIN:
1338 {
1340 if (m_stereoMode)
1341 {
1343 {
1345 }
1346 }
1347 }
1348 break;
1350 {
1351 b = setFramerate((int) value);
1352 break;
1353 }
1357 break;
1361 break;
1362 case YARP_FEATURE_HUE:
1365 break;
1369 break;
1370 default:
1371 yCError(REALSENSE2) << "Feature not supported!";
1372 return false;
1373 }
1374 if (!b)
1375 {
1376 yCError(REALSENSE2) << "Something went wrong setting the feature requested, run the device with --verbose for the supported options";
1377 if (m_verbose)
1378 {
1380 }
1381 return false;
1382 }
1383 return true;
1384}
1385
1386bool realsense2Driver::getFeature(int feature, double *value)
1387{
1388 bool b = false;
1389 if (!hasFeature(feature, &b) || !b)
1390 {
1391 yCError(REALSENSE2) << "Feature not supported!";
1392 return false;
1393 }
1394
1395 float valToGet = 0.0;
1396 b = false;
1397
1398 auto f = static_cast<cameraFeature_id_t>(feature);
1399 switch(f)
1400 {
1404 break;
1405 case YARP_FEATURE_GAIN:
1408 break;
1410 {
1411 b = true;
1412 *value = (double) m_fps;
1413 break;
1414 }
1418 break;
1422 break;
1423 case YARP_FEATURE_HUE:
1426 break;
1430 break;
1431 default:
1432 yCError(REALSENSE2) << "Feature not supported!";
1433 return false;
1434 }
1435 if (!b)
1436 {
1437 yCError(REALSENSE2) << "Something went wrong setting the feature requested, run the device with --verbose for the supported options";
1438 if (m_verbose)
1439 {
1441 }
1442 return false;
1443 }
1444 return true;
1445}
1446
1447bool realsense2Driver::setFeature(int feature, double value1, double value2)
1448{
1449 yCError(REALSENSE2) << "No 2-valued feature are supported";
1450 return false;
1451}
1452
1453bool realsense2Driver::getFeature(int feature, double *value1, double *value2)
1454{
1455 yCError(REALSENSE2) << "No 2-valued feature are supported";
1456 return false;
1457}
1458
1459bool realsense2Driver::hasOnOff( int feature, bool *HasOnOff)
1460{
1461 bool b;
1462 if (!hasFeature(feature, &b) || !b)
1463 {
1464 yCError(REALSENSE2) << "Feature not supported!";
1465 return false;
1466 }
1467
1468 auto f = static_cast<cameraFeature_id_t>(feature);
1470 {
1471 *HasOnOff = true;
1472 return true;
1473 }
1474 *HasOnOff = false;
1475 return true;
1476}
1477
1478bool realsense2Driver::setActive( int feature, bool onoff)
1479{
1480 bool b;
1481 if (!hasFeature(feature, &b) || !b)
1482 {
1483 yCError(REALSENSE2) << "Feature not supported!";
1484 return false;
1485 }
1486
1487 if (!hasOnOff(feature, &b) || !b)
1488 {
1489 yCError(REALSENSE2) << "Feature does not have OnOff.. call hasOnOff() to know if a specific feature support OnOff mode";
1490 return false;
1491 }
1492
1493 switch(feature)
1494 {
1497 return b;
1500 return b;
1501 default:
1502 return false;
1503 }
1504
1505 return true;
1506}
1507
1508bool realsense2Driver::getActive( int feature, bool *isActive)
1509{
1510 bool b;
1511 if (!hasFeature(feature, &b) || !b)
1512 {
1513 yCError(REALSENSE2) << "Feature not supported!";
1514 return false;
1515 }
1516
1517 if (!hasOnOff(feature, &b) || !b)
1518 {
1519 yCError(REALSENSE2) << "Feature does not have OnOff.. call hasOnOff() to know if a specific feature support OnOff mode";
1520 return false;
1521 }
1522 float response = 0.0;
1523 switch(feature)
1524 {
1526 b = getOption(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, m_color_sensor, response); //TODO check if this exotic conversion works
1527 *isActive = (bool) response;
1528 return b;
1530 b = getOption(RS2_OPTION_ENABLE_AUTO_EXPOSURE, m_color_sensor, response); //TODO check if this exotic conversion works
1531 *isActive = (bool) response;
1532 return b;
1533 default:
1534 return false;
1535 }
1536
1537 return true;
1538}
1539
1540bool realsense2Driver::hasAuto(int feature, bool *hasAuto)
1541{
1542 bool b;
1543 if (!hasFeature(feature, &b) || !b)
1544 {
1545 yCError(REALSENSE2) << "Feature not supported!";
1546 return false;
1547 }
1548
1549 auto f = static_cast<cameraFeature_id_t>(feature);
1551 {
1552 *hasAuto = true;
1553 return true;
1554 }
1555 *hasAuto = false;
1556 return true;
1557}
1558
1559bool realsense2Driver::hasManual( int feature, bool* hasManual)
1560{
1561 bool b;
1562 if (!hasFeature(feature, &b) || !b)
1563 {
1564 yCError(REALSENSE2) << "Feature not supported!";
1565 return false;
1566 }
1567
1568 auto f = static_cast<cameraFeature_id_t>(feature);
1571 {
1572 *hasManual = true;
1573 return true;
1574 }
1575 *hasManual = false;
1576 return true;
1577}
1578
1579bool realsense2Driver::hasOnePush(int feature, bool* hasOnePush)
1580{
1581 bool b;
1582 if (!hasFeature(feature, &b) || !b)
1583 {
1584 yCError(REALSENSE2) << "Feature not supported!";
1585 return false;
1586 }
1587
1588 return hasAuto(feature, hasOnePush);
1589}
1590
1592{
1593 bool b;
1594 if (!hasFeature(feature, &b) || !b)
1595 {
1596 yCError(REALSENSE2) << "Feature not supported!";
1597 return false;
1598 }
1599 float one = 1.0;
1600 float zero = 0.0;
1601
1602 auto f = static_cast<cameraFeature_id_t>(feature);
1604 {
1605 switch(mode)
1606 {
1607 case MODE_AUTO:
1609 case MODE_MANUAL:
1611 case MODE_UNKNOWN:
1612 return false;
1613 default:
1614 return false;
1615 }
1616 return true;
1617 }
1618
1619 if (f == YARP_FEATURE_EXPOSURE)
1620 {
1621 switch(mode)
1622 {
1623 case MODE_AUTO:
1625 case MODE_MANUAL:
1627 case MODE_UNKNOWN:
1628 return false;
1629 default:
1630 return false;
1631 }
1632 return true;
1633 }
1634
1635
1636 yCError(REALSENSE2) << "Feature does not have both auto and manual mode";
1637 return false;
1638}
1639
1641{
1642 bool b;
1643 if (!hasFeature(feature, &b) || !b)
1644 {
1645 yCError(REALSENSE2) << "Feature not supported!";
1646 return false;
1647 }
1648 float res = 0.0;
1649 bool ret = true;
1650 auto f = static_cast<cameraFeature_id_t>(feature);
1652 {
1654 }
1655
1656 if (f == YARP_FEATURE_EXPOSURE)
1657 {
1659 }
1660
1661 if (res == 0.0)
1662 {
1663 *mode = MODE_MANUAL;
1664 }
1665 else if (res == 1.0)
1666 {
1667 *mode = MODE_AUTO;
1668 }
1669 else
1670 {
1671 *mode = MODE_UNKNOWN;
1672 }
1673 return ret;
1674}
1675
1677{
1678 bool b;
1679 if (!hasFeature(feature, &b) || !b)
1680 {
1681 yCError(REALSENSE2) << "Feature not supported!";
1682 return false;
1683 }
1684
1685 if (!hasOnePush(feature, &b) || !b)
1686 {
1687 yCError(REALSENSE2) << "Feature doesn't have OnePush";
1688 return false;
1689 }
1690
1691 setMode(feature, MODE_AUTO);
1692 setMode(feature, MODE_MANUAL);
1693
1694 return true;
1695}
1696
1698{
1699 if (!m_stereoMode)
1700 {
1701 yCError(REALSENSE2)<<"Infrared stereo stream not enabled";
1702 return false;
1703 }
1704
1705 image.resize(width(), height());
1706 std::lock_guard<std::mutex> guard(m_mutex);
1707 rs2::frameset data;
1708 try
1709 {
1710 data = m_pipeline.wait_for_frames();
1711 }
1712 catch (const rs2::error& e)
1713 {
1714 yCError(REALSENSE2) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
1715 m_lastError = e.what();
1716 return false;
1717 }
1718
1719 rs2::video_frame frm1 = data.get_infrared_frame(1);
1720 rs2::video_frame frm2 = data.get_infrared_frame(2);
1721
1722 int pixCode = pixFormatToCode(frm1.get_profile().format());
1723
1725 {
1726 yCError(REALSENSE2) << "Expecting Pixel Format MONO";
1727 return false;
1728 }
1729
1730 // Wrap rs images with yarp ones.
1732 imgL.setExternal((unsigned char*) (frm1.get_data()), frm1.get_width(), frm1.get_height());
1733 imgR.setExternal((unsigned char*) (frm2.get_data()), frm2.get_width(), frm2.get_height());
1734
1736 {
1738 rotatedL.resize(imgL.width(), imgL.height());
1739 rotatedR.resize(imgR.width(), imgR.height());
1740 for (size_t y = 0; y < imgL.height(); y++)
1741 {
1742 for (size_t x = 0; x < imgL.width(); x++)
1743 {
1744 rotatedL.pixel(x, y) = imgL.pixel(imgL.width() - x - 1, imgL.height() - y - 1);
1745 rotatedR.pixel(x, y) = imgR.pixel(imgR.width() - x - 1, imgR.height() - y - 1);
1746 }
1747 }
1748 // A 180-degree correction also reverses the physical left/right sensor order.
1750 }
1751
1752 return utils::horzConcat(imgL, imgR, image);
1753}
1754
1756{
1757 return m_infrared_intrin.height;
1758}
1759
1761{
1762 return m_infrared_intrin.width*2;
1763}
@ VOCAB_PIXEL_RGBA
Definition Image.h:45
@ VOCAB_PIXEL_MONO16
Definition Image.h:43
@ VOCAB_PIXEL_BGRA
Definition Image.h:46
@ VOCAB_PIXEL_BGR
Definition Image.h:49
@ VOCAB_PIXEL_INVALID
Definition Image.h:41
@ VOCAB_PIXEL_MONO
Definition Image.h:42
@ VOCAB_PIXEL_RGB
Definition Image.h:44
yarp::os::Stamp m_depth_stamp
rs2::pipeline m_pipeline
bool setActive(int feature, bool onoff) override
bool setDepthClipPlanes(double nearPlane, double farPlane) override
Set the clipping planes of the sensor.
rs2_intrinsics m_infrared_intrin
RGBDSensor_status getSensorStatus() override
bool hasManual(int feature, bool *hasManual) override
rs2_intrinsics m_color_intrin
bool getRgbMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool getDepthImage(depthImage &depthImage, Stamp *timeStamp=nullptr) override
rs2::sensor * m_depth_sensor
bool getActive(int feature, bool *isActive) override
std::vector< cameraFeature_id_t > m_supportedFeatures
bool close() override
Close the DeviceDriver.
bool setFramerate(const int _fps)
bool hasAuto(int feature, bool *hasAuto) override
int height() const override
Return the height of each frame.
bool setDepthFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the depth camera.
bool hasFeature(int feature, bool *hasFeature) override
bool getCameraDescription(CameraDescriptor *camera) override
bool getRgbSupportedConfigurations(yarp::sig::VectorOf< yarp::dev::CameraConfig > &configurations) override
bool getMode(int feature, FeatureMode *mode) override
bool setDepthResolution(int width, int height) override
Set the resolution of the depth image from the camera.
bool setRgbFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the rgb camera.
rs2::pipeline_profile m_profile
rs2_intrinsics m_depth_intrin
rs2_stream m_alignment_stream
int getDepthHeight() override
Return the height of each frame.
bool setRgbMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
rs2_extrinsics m_depth_to_color
double getDepthAccuracy() override
bool setDepthMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool hasOnOff(int feature, bool *HasOnOff) override
bool hasOnePush(int feature, bool *hasOnePush) override
std::string getLastErrorMsg(Stamp *timeStamp=NULL) override
int getDepthWidth() override
Return the height of each frame.
bool getRgbResolution(int &width, int &height) override
Get the resolution of the rgb image from the camera.
bool getDepthFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the depth camera.
bool setFeature(int feature, double value) override
bool setOnePush(int feature) override
bool setDepthAccuracy(double accuracy) override
Set the minimum detectable variation in distance [meter] when possible.
rs2::sensor * m_color_sensor
bool getRgbFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the rgb camera.
bool getRgbImage(FlexImage &rgbImage, Stamp *timeStamp=nullptr) override
Get the rgb frame from the device.
rs2_extrinsics m_color_to_depth
bool getImages(FlexImage &colorFrame, depthImage &depthFrame, Stamp *colorStamp=NULL, Stamp *depthStamp=NULL) override
yarp::os::Stamp m_rgb_stamp
bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override
Get the extrinsic parameters from the device.
bool getRgbIntrinsicParam(Property &intrinsic) override
Get the intrinsic parameters of the rgb camera.
int getRgbHeight() override
Return the height of each frame.
bool getDepthIntrinsicParam(Property &intrinsic) override
Get the intrinsic parameters of the depth camera.
std::string m_serialnumber
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelMono > &image) override
Get an image from the frame grabber.
int width() const override
Return the width of each frame.
int getRgbWidth() override
Return the width of each frame.
yarp::dev::RGBDSensorParamParser m_paramParser
rs2::hole_filling_filter m_holeFillingFilter
bool setMode(int feature, FeatureMode mode) override
std::vector< rs2::sensor > m_sensors
bool getFeature(int feature, double *value) override
std::string m_lastError
bool setRgbResolution(int width, int height) override
Set the resolution of the rgb image from the camera.
bool getDepthClipPlanes(double &nearPlane, double &farPlane) override
Get the clipping planes of the sensor.
bool getDepthMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool parseParam(const yarp::os::Searchable &config, std::vector< RGBDParam * > &params)
parseParam, parse the params stored in a Searchable.
yarp::sig::IntrinsicParams rgbIntrinsic
yarp::sig::IntrinsicParams depthIntrinsic
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
Definition Property.h:33
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
A single value (typically within a Bottle).
Definition Value.h:44
virtual bool isString() const
Checks if value is a string.
Definition Value.cpp:156
virtual bool isBool() const
Checks if value is a boolean.
Definition Value.cpp:114
virtual bool asBool() const
Get boolean value.
Definition Value.cpp:192
virtual std::string asString() const
Get string value.
Definition Value.cpp:246
Image class with user control of representation details.
Definition Image.h:361
A class for a Matrix.
Definition Matrix.h:39
static std::map< std::string, RGBDSensorParamParser::RGBDParam > params_map
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13
bool horzConcat(const yarp::sig::Image &inImgL, const yarp::sig::Image &inImgR, yarp::sig::Image &outImg)
Concatenate horizontally two images of the same size in one with double width.
YarpDistortion
The YarpDistortion enum to define the type of the distortion model of the camera.
constexpr char clipPlanes[]
static bool setOption(rs2_option option, const rs2::sensor *sensor, float value)
static int pixFormatToCode(const rs2_format p)
static void print_supported_options(const rs2::sensor &sensor)
static const std::map< std::string, rs2_stream > stringRSStreamMap
constexpr char needAlignment[]
static bool getOption(rs2_option option, const rs2::sensor *sensor, float &value)
static std::string get_device_information(const rs2::device &dev)
static bool optionValue2Perc(rs2_option option, const rs2::sensor *sensor, float &perc, const float &value)
static std::map< std::string, RGBDSensorParamParser::RGBDParam > params_map
static YarpDistortion rsDistToYarpDist(const rs2_distortion dist, const rs2_intrinsics &values)
static bool setExtrinsicParam(Matrix &extrinsic, const rs2_extrinsics &values)
constexpr char enableEmitter[]
static bool setIntrinsic(Property &intrinsic, const rs2_intrinsics &values)
constexpr char framerate[]
constexpr char alignmentFrame[]
constexpr char rgbRes[]
static size_t bytesPerPixel(const rs2_format format)
static void settingErrorMsg(const string &error, bool &ret)
static bool optionPerc2Value(rs2_option option, const rs2::sensor *sensor, const float &perc, float &value)
constexpr char depthRes[]
static bool isSupportedFormat(const rs2::sensor &sensor, const int width, const int height, const int fps, bool verbose=false)
constexpr char accuracy[]
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).
double focalLengthY
Result of the product of the physical focal length(mm) and the size sy of the individual imager eleme...
DistortionModel distortionModel
Distortion model of the image.
double focalLengthX
Result of the product of the physical focal length(mm) and the size sx of the individual imager eleme...
void toProperty(yarp::os::Property &intrinsic) const
toProperty, convert the struct to a Property.
double principalPointX
Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge.
double principalPointY
Vertical coordinate of the principal point of the image, as a pixel offset from the top edge.