Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRealSense2.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 * librealSense2 interface.
33 *
34*****************************************************************************/
35
36#ifndef _vpRealSense2_h_
37#define _vpRealSense2_h_
38
39#include <visp3/core/vpConfig.h>
40
41#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
42
43#include <librealsense2/rs.hpp>
44#include <librealsense2/rsutil.h>
45
46#ifdef VISP_HAVE_PCL
47#include <pcl/common/common_headers.h>
48#endif
49
50#include <visp3/core/vpCameraParameters.h>
51#include <visp3/core/vpImage.h>
52
287class VISP_EXPORT vpRealSense2
288{
289public:
290 vpRealSense2();
291 virtual ~vpRealSense2();
292
293 void acquire(vpImage<unsigned char> &grey, double *ts = NULL);
294 void acquire(vpImage<vpRGBa> &color, double *ts = NULL);
295 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
296 std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
297 rs2::align *const align_to = NULL, double *ts = NULL);
298 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
299 std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared1,
300 unsigned char *const data_infrared2, rs2::align *const align_to, double *ts = NULL);
301#if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
302 void acquire(vpImage<unsigned char> *left, vpImage<unsigned char> *right, double *ts = NULL);
304 vpColVector *odo_vel, vpColVector *odo_acc, unsigned int *confidence = NULL, double *ts = NULL);
306 vpColVector *odo_vel, vpColVector *odo_acc, vpColVector *imu_vel, vpColVector *imu_acc,
307 unsigned int *tracker_confidence = NULL, double *ts = NULL);
308#endif
309
310#ifdef VISP_HAVE_PCL
311 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
312 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
313 unsigned char *const data_infrared = NULL, rs2::align *const align_to = NULL, double *ts = NULL);
314 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
315 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
316 unsigned char *const data_infrared1, unsigned char *const data_infrared2, rs2::align *const align_to,
317 double *ts = NULL);
318
319 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
320 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
321 unsigned char *const data_infrared = NULL, rs2::align *const align_to = NULL, double *ts = NULL);
322 void acquire(unsigned char *const data_image, unsigned char *const data_depth,
323 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
324 unsigned char *const data_infrared1, unsigned char *const data_infrared2, rs2::align *const align_to,
325 double *ts = NULL);
326#endif
327
328 void close();
329
330 vpCameraParameters getCameraParameters(
331 const rs2_stream &stream,
333 int index = -1) const;
334
335 float getDepthScale();
336
337#if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
338 void getIMUAcceleration(vpColVector *imu_acc, double *ts);
339 void getIMUData(vpColVector *imu_vel, vpColVector *imu_acc, double *ts);
340 void getIMUVelocity(vpColVector *imu_vel, double *ts);
341#endif
342
343 rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index = -1) const;
344
348 inline float getInvalidDepthValue() const { return m_invalidDepthValue; }
349
352 inline float getMaxZ() const { return m_max_Z; }
353
354#if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
355 unsigned int getOdometryData(vpHomogeneousMatrix *cMw, vpColVector *odo_vel, vpColVector *odo_acc, double *ts = NULL);
356#endif
357
359 rs2::pipeline &getPipeline() { return m_pipe; }
360
362 rs2::pipeline_profile &getPipelineProfile() { return m_pipelineProfile; }
363
364 std::string getProductLine();
365
366 vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index = -1) const;
367
368 bool open(const rs2::config &cfg = rs2::config());
369 bool open(const rs2::config &cfg, std::function<void(rs2::frame)> &callback);
370
371 friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRealSense2 &rs);
372
376 inline void setInvalidDepthValue(float value) { m_invalidDepthValue = value; }
377
380 inline void setMaxZ(const float maxZ) { m_max_Z = maxZ; }
381
382protected:
385 float m_max_Z;
386 rs2::pipeline m_pipe;
387 rs2::pipeline_profile m_pipelineProfile;
388 rs2::pointcloud m_pointcloud;
389 rs2::points m_points;
393 std::string m_product_line;
394 bool m_init;
395
396 void getColorFrame(const rs2::frame &frame, vpImage<vpRGBa> &color);
397 void getGreyFrame(const rs2::frame &frame, vpImage<unsigned char> &grey);
398 void getNativeFrameData(const rs2::frame &frame, unsigned char *const data);
399 void getPointcloud(const rs2::depth_frame &depth_frame, std::vector<vpColVector> &pointcloud);
400#ifdef VISP_HAVE_PCL
401 void getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
402 void getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame,
403 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
404#endif
405};
406
407#endif
408#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a rotation vector as quaternion angle minimal representation.
void setInvalidDepthValue(float value)
void setMaxZ(const float maxZ)
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.
vpQuaternionVector m_quat
rs2::pipeline m_pipe
vpRotationMatrix m_rot
rs2::pointcloud m_pointcloud
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.
vpTranslationVector m_pos
std::string m_product_line
rs2::points m_points
float getInvalidDepthValue() const
float m_invalidDepthValue
rs2::pipeline_profile m_pipelineProfile
float getMaxZ() const
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.