Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRealSense.h
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#ifndef _vpRealSense_h_
37#define _vpRealSense_h_
38
39#include <map>
40#include <stdint.h>
41
42#include <visp3/core/vpCameraParameters.h>
43#include <visp3/core/vpColVector.h>
44#include <visp3/core/vpConfig.h>
45#include <visp3/core/vpException.h>
46#include <visp3/core/vpHomogeneousMatrix.h>
47#include <visp3/core/vpImage.h>
48
49#if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
50
51#include <librealsense/rs.hpp>
52
53#ifdef VISP_HAVE_PCL
54#include <pcl/common/projection_matrix.h>
55#include <pcl/point_types.h>
56#endif
57
331class VISP_EXPORT vpRealSense
332{
333public:
334 vpRealSense();
335 virtual ~vpRealSense();
336
337 void acquire(std::vector<vpColVector> &pointcloud);
338#ifdef VISP_HAVE_PCL
339 void acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
340 void acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
341#endif
342 void acquire(vpImage<unsigned char> &grey); // tested
343 void acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud);
344 void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
345 std::vector<vpColVector> &pointcloud);
346#ifdef VISP_HAVE_PCL
347 void acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
348 void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
349 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
350 void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
351 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
352#endif
353
354 void acquire(vpImage<vpRGBa> &color); // tested
355 void acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud);
356 void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
357 std::vector<vpColVector> &pointcloud);
358
359 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
360 std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
361 unsigned char *const data_infrared2 = NULL, const rs::stream &stream_color = rs::stream::color,
362 const rs::stream &stream_depth = rs::stream::depth,
363 const rs::stream &stream_infrared = rs::stream::infrared,
364 const rs::stream &stream_infrared2 = rs::stream::infrared2);
365
366#ifdef VISP_HAVE_PCL
367 void acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
368 void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
369 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
370 void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
371 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
372
373 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
374 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
375 unsigned char *const data_infrared, unsigned char *const data_infrared2 = NULL,
376 const rs::stream &stream_color = rs::stream::color, const rs::stream &stream_depth = rs::stream::depth,
377 const rs::stream &stream_infrared = rs::stream::infrared,
378 const rs::stream &stream_infrared2 = rs::stream::infrared2);
379 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
380 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
381 unsigned char *const data_infrared, unsigned char *const data_infrared2 = NULL,
382 const rs::stream &stream_color = rs::stream::color, const rs::stream &stream_depth = rs::stream::depth,
383 const rs::stream &stream_infrared = rs::stream::infrared,
384 const rs::stream &stream_infrared2 = rs::stream::infrared2);
385#endif
386
387 void close();
388
389 vpCameraParameters getCameraParameters(
390 const rs::stream &stream,
393 rs::device *getHandler() const { return m_device; }
394
395 rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const;
396 rs::intrinsics getIntrinsics(const rs::stream &stream) const;
397
401 inline float getInvalidDepthValue() const { return m_invalidDepthValue; }
402
404 int getNumDevices() const { return m_context.get_device_count(); }
407 std::string getSerialNumber() const { return m_serial_no; }
408 vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const;
409
410 void open();
411
412 void setDeviceBySerialNumber(const std::string &serial_no);
413
414 friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRealSense &rs);
415
419 rs::format m_streamFormat;
421
423 : m_streamWidth(640), m_streamHeight(480), m_streamFormat(rs::format::rgba8), m_streamFramerate(30)
424 {
425 }
426
427 vpRsStreamParams(const int streamWidth, const int streamHeight, const rs::format &streamFormat,
428 const int streamFramerate)
429 : m_streamWidth(streamWidth), m_streamHeight(streamHeight), m_streamFormat(streamFormat),
430 m_streamFramerate(streamFramerate)
431 {
432 }
433 };
434
435 void setEnableStream(const rs::stream &stream, bool status);
436
440 inline void setInvalidDepthValue(float value) { m_invalidDepthValue = value; }
441
442 void setStreamSettings(const rs::stream &stream, const rs::preset &preset);
443 void setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params);
444
445protected:
446 rs::context m_context;
447 rs::device *m_device;
449 std::string m_serial_no;
450 std::map<rs::stream, rs::intrinsics> m_intrinsics;
451 float m_max_Z;
452 std::map<rs::stream, bool> m_enableStreams;
453 std::map<rs::stream, bool> m_useStreamPresets;
454 std::map<rs::stream, rs::preset> m_streamPresets;
455 std::map<rs::stream, vpRsStreamParams> m_streamParams;
457
458 void initStream();
459};
460
461#endif
462#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
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
int getNumDevices() const
Get number of devices that are detected.
void setInvalidDepthValue(float value)
std::map< rs::stream, bool > m_enableStreams
rs::device * m_device
float getInvalidDepthValue() const
rs::device * getHandler() const
Get access to device handler.
std::map< rs::stream, bool > m_useStreamPresets
std::string getSerialNumber() const
std::map< rs::stream, rs::intrinsics > m_intrinsics
rs::context m_context
std::map< rs::stream, rs::preset > m_streamPresets
float m_invalidDepthValue
std::map< rs::stream, vpRsStreamParams > m_streamParams
float m_max_Z
Maximal Z depth in meter.
vpRsStreamParams(const int streamWidth, const int streamHeight, const rs::format &streamFormat, const int streamFramerate)