Visual Servoing Platform
version 3.3.0 under development (2020-02-17)
|
#include <visp3/sensor/vpRealSense2.h>
Public Member Functions | |
vpRealSense2 () | |
virtual | ~vpRealSense2 () |
void | acquire (vpImage< unsigned char > &grey) |
void | acquire (vpImage< vpRGBa > &color) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, unsigned char *const data_infrared, rs2::align *const align_to=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, unsigned char *const data_infrared1, unsigned char *const data_infrared2, rs2::align *const align_to) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud, unsigned char *const data_infrared=NULL, rs2::align *const align_to=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud, unsigned char *const data_infrared1, unsigned char *const data_infrared2, rs2::align *const align_to) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pointcloud, unsigned char *const data_infrared=NULL, rs2::align *const align_to=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pointcloud, unsigned char *const data_infrared1, unsigned char *const data_infrared2, rs2::align *const align_to) |
void | close () |
vpCameraParameters | getCameraParameters (const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const |
float | getDepthScale () |
rs2_intrinsics | getIntrinsics (const rs2_stream &stream) const |
float | getInvalidDepthValue () const |
float | getMaxZ () const |
rs2::pipeline & | getPipeline () |
rs2::pipeline_profile & | getPipelineProfile () |
vpHomogeneousMatrix | getTransformation (const rs2_stream &from, const rs2_stream &to) const |
void | open (const rs2::config &cfg=rs2::config()) |
void | setInvalidDepthValue (float value) |
void | setMaxZ (const float maxZ) |
Protected Member Functions | |
void | getColorFrame (const rs2::frame &frame, vpImage< vpRGBa > &color) |
void | getGreyFrame (const rs2::frame &frame, vpImage< unsigned char > &grey) |
void | getNativeFrameData (const rs2::frame &frame, unsigned char *const data) |
void | getPointcloud (const rs2::depth_frame &depth_frame, std::vector< vpColVector > &pointcloud) |
void | getPointcloud (const rs2::depth_frame &depth_frame, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud) |
void | getPointcloud (const rs2::depth_frame &depth_frame, const rs2::frame &color_frame, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pointcloud) |
Protected Attributes | |
float | m_depthScale |
float | m_invalidDepthValue |
float | m_max_Z |
rs2::pipeline | m_pipe |
rs2::pipeline_profile | m_pipelineProfile |
rs2::pointcloud | m_pointcloud |
rs2::points | m_points |
Friends | |
VISP_EXPORT std::ostream & | operator<< (std::ostream &os, const vpRealSense2 &rs) |
This class provides a lightweight wrapper over the Intel librealsense2 library https://github.com/IntelRealSense/librealsense. It allows to capture data from the Intel RealSense cameras.
The usage of vpRealSense2 class is enabled when librealsense2 3rd party is successfully installed.
Moreover, if Point Cloud Library (PCL) 3rd party is installed, we also propose interfaces to retrieve point cloud as pcl::PointCloud<pcl::PointXYZ> or pcl::PointCloud<pcl::PointXYZRGB> data structures.
To acquire images from the RealSense color camera and convert them into grey level images, a good starting is to use the following code that corresponds to the content of sample-realsense.cpp:
If you want to acquire color images, in the previous sample replace:
by
If you are interested in the point cloud and if ViSP is build with PCL support, you can start from the following example where we use PCL library to visualize the point cloud
If you want to change the default stream parameters, refer to the librealsense2 rs2::config
documentation. The following code allows to capture the color stream in 1920x1080:
This example shows how to get depth stream aligned on color stream:
References to rs2::pipeline_profile
and rs2::pipeline
can be retrieved with (rs.open() must be called before
):
Information about the sensor can be printed with:
Definition at line 282 of file vpRealSense2.h.
vpRealSense2::vpRealSense2 | ( | ) |
Default constructor.
Definition at line 72 of file vpRealSense2.cpp.
|
virtual |
Default destructor that stops the streaming.
Definition at line 82 of file vpRealSense2.cpp.
References close().
void vpRealSense2::acquire | ( | vpImage< unsigned char > & | grey | ) |
Acquire greyscale image from RealSense device.
grey | : Greyscale image. |
Definition at line 88 of file vpRealSense2.cpp.
References getGreyFrame(), and m_pipe.
Referenced by acquire().
Acquire color image from RealSense device.
color | : Color image. |
Definition at line 99 of file vpRealSense2.cpp.
References getColorFrame(), and m_pipe.
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
unsigned char *const | data_infrared, | ||
rs2::align *const | align_to = NULL |
||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
Definition at line 115 of file vpRealSense2.cpp.
References acquire().
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
unsigned char *const | data_infrared1, | ||
unsigned char *const | data_infrared2, | ||
rs2::align *const | align_to | ||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
data_infrared1 | : First infrared image buffer or NULL if not wanted. |
data_infrared2 | : Second infrared image buffer (if supported by the device) or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
The following code shows how to use this function to get color, infrared 1 and infrared 2 frames acquired by a D435 device:
Definition at line 178 of file vpRealSense2.cpp.
References getNativeFrameData(), getPointcloud(), and m_pipe.
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZ >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared = NULL , |
||
rs2::align *const | align_to = NULL |
||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format and without texture information) pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
Definition at line 232 of file vpRealSense2.cpp.
References acquire().
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZ >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared1, | ||
unsigned char *const | data_infrared2, | ||
rs2::align *const | align_to | ||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format and without texture information) pointer or NULL if not wanted. |
data_infrared1 | : First infrared image buffer or NULL if not wanted. |
data_infrared2 | : Second infrared image (if supported by the device) buffer or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
Definition at line 253 of file vpRealSense2.cpp.
References getNativeFrameData(), getPointcloud(), and m_pipe.
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared = NULL , |
||
rs2::align *const | align_to = NULL |
||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format and with texture information) pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
Definition at line 311 of file vpRealSense2.cpp.
References acquire().
void vpRealSense2::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared1, | ||
unsigned char *const | data_infrared2, | ||
rs2::align *const | align_to | ||
) |
Acquire data from RealSense device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format and with texture information) pointer or NULL if not wanted. |
data_infrared1 | : First infrared image buffer or NULL if not wanted. |
data_infrared2 | : Second infrared image (if supported by the device) buffer or NULL if not wanted. |
align_to | : Align to a reference stream or NULL if not wanted. Only depth and color streams can be aligned. |
Definition at line 332 of file vpRealSense2.cpp.
References getNativeFrameData(), getPointcloud(), and m_pipe.
void vpRealSense2::close | ( | ) |
librealsense documentation:
Stop the pipeline streaming. The pipeline stops delivering samples to the attached computer vision modules and processing blocks, stops the device streaming and releases the device resources used by the pipeline. It is the application's responsibility to release any frame reference it owns. The method takes effect only after
start()
was called, otherwise an exception is raised.
Definition at line 391 of file vpRealSense2.cpp.
References m_pipe.
Referenced by ~vpRealSense2().
vpCameraParameters vpRealSense2::getCameraParameters | ( | const rs2_stream & | stream, |
vpCameraParameters::vpCameraParametersProjType | type = vpCameraParameters::perspectiveProjWithDistortion |
||
) | const |
Return the camera parameters corresponding to a specific stream. This function has to be called after open().
stream | : stream for which camera intrinsic parameters are returned. |
type | : Indicate if the model should include distorsion parameters or not. |
Definition at line 401 of file vpRealSense2.cpp.
References vpCameraParameters::initPersProjWithDistortion(), vpCameraParameters::initPersProjWithoutDistortion(), m_pipelineProfile, and vpCameraParameters::perspectiveProjWithDistortion.
Definition at line 436 of file vpRealSense2.cpp.
References vpImageConvert::BGRToRGBa(), vpImage< Type >::bitmap, vpException::fatalError, vpImage< Type >::resize(), and vpImageConvert::RGBToRGBa().
Referenced by acquire().
float vpRealSense2::getDepthScale | ( | ) |
Get depth scale value used to convert all the uint16_t values contained in a depth frame into a distance in meter.
Definition at line 462 of file vpRealSense2.cpp.
References m_depthScale.
|
protected |
Definition at line 467 of file vpRealSense2.cpp.
References vpImageConvert::BGRToGrey(), vpImage< Type >::bitmap, vpException::fatalError, vpImage< Type >::resize(), vpImageConvert::RGBaToGrey(), and vpImageConvert::RGBToGrey().
Referenced by acquire().
rs2_intrinsics vpRealSense2::getIntrinsics | ( | const rs2_stream & | stream | ) | const |
Get intrinsic parameters corresponding to the stream. This function has to be called after open().
stream | : stream for which the camera intrinsic parameters are returned. |
Definition at line 430 of file vpRealSense2.cpp.
References m_pipelineProfile.
|
inline |
Get the value used when the pixel value (u, v) in the depth map is invalid for the point cloud. For instance, the Point Cloud Library (PCL) uses NAN values for points where the depth is invalid.
Definition at line 326 of file vpRealSense2.h.
|
inline |
Get the maximum Z value (used to discard bad reconstructed depth for pointcloud).
Definition at line 330 of file vpRealSense2.h.
|
protected |
Definition at line 488 of file vpRealSense2.cpp.
Referenced by acquire().
|
inline |
Get a reference to rs2::pipeline
.
Definition at line 333 of file vpRealSense2.h.
|
inline |
Get a reference to rs2::pipeline_profile
.
Definition at line 336 of file vpRealSense2.h.
|
protected |
Definition at line 518 of file vpRealSense2.cpp.
References vpException::fatalError, m_depthScale, m_invalidDepthValue, and m_max_Z.
Referenced by acquire().
|
protected |
Definition at line 571 of file vpRealSense2.cpp.
References vpException::fatalError, m_depthScale, m_invalidDepthValue, m_max_Z, m_pointcloud, and m_points.
|
protected |
Definition at line 637 of file vpRealSense2.cpp.
References vpException::fatalError, m_depthScale, m_invalidDepthValue, and m_max_Z.
vpHomogeneousMatrix vpRealSense2::getTransformation | ( | const rs2_stream & | from, |
const rs2_stream & | to | ||
) | const |
Get the extrinsic transformation from one stream to another. This function has to be called after open().
from,to | : streams for which the camera extrinsic parameters are returned. |
Definition at line 817 of file vpRealSense2.cpp.
References m_pipelineProfile.
void vpRealSense2::open | ( | const rs2::config & | cfg = rs2::config() | ) |
Open access to the RealSense device and start the streaming.
Definition at line 838 of file vpRealSense2.cpp.
References m_depthScale, m_pipe, and m_pipelineProfile.
|
inline |
Set the value used when the pixel value (u, v) in the depth map is invalid for the point cloud. For instance, the Point Cloud Library (PCL) uses NAN values for points where the depth is invalid.
Definition at line 347 of file vpRealSense2.h.
|
inline |
Set the maximum Z value (used to discard bad reconstructed depth for pointcloud).
Definition at line 351 of file vpRealSense2.h.
|
friend |
Return information from the sensor.
os | : Input stream. |
rs | : RealSense object. |
The following example shows how to use this method.
Definition at line 969 of file vpRealSense2.cpp.
|
protected |
Definition at line 354 of file vpRealSense2.h.
Referenced by getDepthScale(), getPointcloud(), and open().
|
protected |
Definition at line 355 of file vpRealSense2.h.
Referenced by getPointcloud().
|
protected |
Definition at line 356 of file vpRealSense2.h.
Referenced by getPointcloud().
|
protected |
Definition at line 357 of file vpRealSense2.h.
|
protected |
Definition at line 358 of file vpRealSense2.h.
Referenced by getCameraParameters(), getIntrinsics(), getTransformation(), and open().
|
protected |
Definition at line 359 of file vpRealSense2.h.
Referenced by getPointcloud().
|
protected |
Definition at line 360 of file vpRealSense2.h.
Referenced by getPointcloud().