Visual Servoing Platform  version 3.2.0
vpRealSense2 Class Reference

#include <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, 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::PointXYZRGB >::Ptr &pointcloud, unsigned char *const data_infrared=NULL, rs2::align *const align_to=NULL)
 
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 (const 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

rs2_intrinsics m_colorIntrinsics
 
rs2_extrinsics m_depth2ColorExtrinsics
 
rs2_intrinsics m_depthIntrinsics
 
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)
 

Detailed Description

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.

Note
Supported devices for Intel® RealSense™ SDK 2.0 (build 2.8.3):
  • Intel® RealSense™ Camera D400-Series (not tested)
  • Intel® RealSense™ Developer Kit SR300 (vpRealSense2 is ok)

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.

Warning
Notice that the usage of this class requires compiler and library support for the ISO C++ 2011 standard. This support must be enabled with the -std=c++11 compiler option. Hereafter we give an example of a CMakeLists.txt file that allows to build sample-realsense.cpp that uses vpRealSense2 class.
project(sample)
cmake_minimum_required(VERSION 2.6)
find_package(VISP REQUIRED)
include_directories(${VISP_INCLUDE_DIRS})
include(CheckCXXCompilerFlag)
check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
endif()
add_executable(sample-realsense sample-realsense.cpp)
target_link_libraries(sample-realsense ${VISP_LIBRARIES})

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:

#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpRealSense2.h>
int main()
{
rs.open();
vpImage<unsigned char> I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
#ifdef VISP_HAVE_X11
vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
#endif
while (true) {
rs.acquire(I);
if (vpDisplay::getClick(I, false))
break;
}
return 0;
}

If you want to acquire color images, in the previous sample replace:

vpImage<unsigned char> I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);

by

vpImage<vpRGBa> I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);

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:

#include <visp3/sensor/vpRealSense2.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
int main()
{
rs.open();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
rs.acquire(NULL, NULL, NULL, pointcloud);
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
viewer->setBackgroundColor(0, 0, 0);
viewer->initCameraParameters();
viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
while (true) {
rs.acquire(NULL, NULL, NULL, pointcloud);
static bool update = false;
if (!update) {
viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
update = true;
} else {
viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
}
viewer->spinOnce(30);
}
return 0;
}

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:

#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpRealSense2.h>
int main() {
rs2::config config;
config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_RGBA8, 30);
config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
rs.open(config);
vpImage<vpRGBa> Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
vpImage<unsigned char> Ii(rs.getIntrinsics(RS2_STREAM_INFRARED).height,
rs.getIntrinsics(RS2_STREAM_INFRARED).width);
#ifdef VISP_HAVE_X11
vpDisplayX dc(Ic, 0, 0, "Color");
vpDisplayX di(Ii, 100, 100, "Infrared");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI dc(Ic, 0, 0, "Color");
vpDisplayGDI di(Ii, 100, 100, "Infrared");
#endif
while (true) {
rs.acquire((unsigned char *) Ic.bitmap, NULL, NULL, Ii.bitmap);
if (vpDisplay::getClick(Ic, false) || vpDisplay::getClick(Ii, false))
break;
}
return 0;
}

This example shows how to get depth stream aligned on color stream:

#include <visp3/core/vpImageConvert.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpRealSense2.h>
int main() {
rs2::config config;
config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
rs.open(config);
vpImage<vpRGBa> Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
vpImage<uint16_t> Id_raw(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width);
vpImage<vpRGBa> Id(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width);
#ifdef VISP_HAVE_X11
vpDisplayX dc(Ic, 0, 0, "Color");
vpDisplayX dd(Id, 100, 100, "Depth aligned to color");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI dc(Ic, 0, 0, "Color");
vpDisplayGDI dd(Id, 100, 100, "Depth aligned to color");
#endif
rs2::align align_to(RS2_STREAM_COLOR);
while (true) {
rs.acquire((unsigned char *) Ic.bitmap, (unsigned char *) Id_raw.bitmap, NULL, NULL, &align_to);
if (vpDisplay::getClick(Ic, false) || vpDisplay::getClick(Id, false))
break;
}
return 0;
}

References to rs2::pipeline_profile and rs2::pipeline can be retrieved with (rs.open() must be called before):

rs2::pipeline_profile& profile = rs.getPipelineProfile(); rs2::pipeline& pipeline = rs.getPipeline();

Information about the sensor can be printed with:

#include <visp3/sensor/vpRealSense2.h>
int main() {
rs.open();
std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
return 0;
}
Note
This class has been tested with the Intel RealSense SR300 (Firmware: 3.21.0.0) using librealsense (API version: 2.8.3). Refer to the librealsense2 documentation or API how to for additional information.
Examples
getRealSense2Info.cpp, grabRealSense2.cpp, servoFrankaPBVS.cpp, testRealSense2_D435.cpp, testRealSense2_SR300.cpp, tutorial-apriltag-detector-live.cpp, tutorial-franka-acquire-calib-data.cpp, tutorial-grabber-realsense.cpp, tutorial-mb-generic-tracker-apriltag-live-realsense2.cpp, tutorial-mb-generic-tracker-live.cpp, tutorial-mb-generic-tracker-rgbd-realsense.cpp, and tutorial-pose-from-points-live.cpp.

Definition at line 280 of file vpRealSense2.h.

Constructor & Destructor Documentation

◆ vpRealSense2()

vpRealSense2::vpRealSense2 ( )

Default constructor.

Definition at line 50 of file vpRealSense2.cpp.

◆ ~vpRealSense2()

vpRealSense2::~vpRealSense2 ( )
virtual

Default destructor that stops the streaming.

See also
stop()

Definition at line 60 of file vpRealSense2.cpp.

Member Function Documentation

◆ acquire() [1/5]

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.

Parameters
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.

Definition at line 135 of file vpRealSense2.cpp.

◆ acquire() [2/5]

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.

Parameters
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.

Definition at line 181 of file vpRealSense2.cpp.

◆ acquire() [3/5]

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.

Parameters
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.

Definition at line 92 of file vpRealSense2.cpp.

◆ acquire() [4/5]

◆ acquire() [5/5]

void vpRealSense2::acquire ( vpImage< vpRGBa > &  color)

Acquire color image from RealSense device.

Parameters
color: Color image.

Definition at line 77 of file vpRealSense2.cpp.

◆ close()

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.

Examples
testRealSense2_D435.cpp, and testRealSense2_SR300.cpp.

Definition at line 229 of file vpRealSense2.cpp.

◆ getCameraParameters()

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().

Parameters
stream: stream for which camera intrinsic parameters are returned.
type: Indicate if the model should include distorsion parameters or not.
See also
getIntrinsics()
Examples
grabRealSense2.cpp, servoFrankaPBVS.cpp, testRealSense2_D435.cpp, testRealSense2_SR300.cpp, tutorial-apriltag-detector-live.cpp, tutorial-franka-acquire-calib-data.cpp, tutorial-mb-generic-tracker-apriltag-live-realsense2.cpp, tutorial-mb-generic-tracker-live.cpp, tutorial-mb-generic-tracker-rgbd-realsense.cpp, and tutorial-pose-from-points-live.cpp.

Definition at line 239 of file vpRealSense2.cpp.

◆ getColorFrame()

void vpRealSense2::getColorFrame ( const rs2::frame &  frame,
vpImage< vpRGBa > &  color 
)
protected

◆ getDepthScale()

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.

Examples
getRealSense2Info.cpp.

Definition at line 296 of file vpRealSense2.cpp.

◆ getGreyFrame()

void vpRealSense2::getGreyFrame ( const rs2::frame &  frame,
vpImage< unsigned char > &  grey 
)
protected

◆ getIntrinsics()

rs2_intrinsics vpRealSense2::getIntrinsics ( const rs2_stream &  stream) const

Get intrinsic parameters corresponding to the stream. This function has to be called after open().

Parameters
stream: stream for which the camera intrinsic parameters are returned.
See also
getCameraParameters()
Examples
grabRealSense2.cpp.

Definition at line 268 of file vpRealSense2.cpp.

◆ getInvalidDepthValue()

float vpRealSense2::getInvalidDepthValue ( ) const
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 314 of file vpRealSense2.h.

◆ getMaxZ()

float vpRealSense2::getMaxZ ( ) const
inline

Get the maximum Z value (used to discard bad reconstructed depth for pointcloud).

Definition at line 318 of file vpRealSense2.h.

◆ getNativeFrameData()

void vpRealSense2::getNativeFrameData ( const rs2::frame &  frame,
unsigned char *const  data 
)
protected

Definition at line 319 of file vpRealSense2.cpp.

◆ getPipeline()

rs2::pipeline& vpRealSense2::getPipeline ( )
inline

Get a reference to rs2::pipeline.

Examples
testRealSense2_D435.cpp, and testRealSense2_SR300.cpp.

Definition at line 321 of file vpRealSense2.h.

◆ getPipelineProfile()

rs2::pipeline_profile& vpRealSense2::getPipelineProfile ( )
inline

Get a reference to rs2::pipeline_profile.

Examples
testRealSense2_D435.cpp, and testRealSense2_SR300.cpp.

Definition at line 324 of file vpRealSense2.h.

◆ getPointcloud() [1/3]

void vpRealSense2::getPointcloud ( const rs2::depth_frame &  depth_frame,
const rs2::frame &  color_frame,
pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  pointcloud 
)
protected

◆ getPointcloud() [2/3]

void vpRealSense2::getPointcloud ( const rs2::depth_frame &  depth_frame,
pcl::PointCloud< pcl::PointXYZ >::Ptr &  pointcloud 
)
protected

◆ getPointcloud() [3/3]

void vpRealSense2::getPointcloud ( const rs2::depth_frame &  depth_frame,
std::vector< vpColVector > &  pointcloud 
)
protected

◆ getTransformation()

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().

Parameters
from,to: streams for which the camera extrinsic parameters are returned.
Examples
grabRealSense2.cpp, testRealSense2_D435.cpp, testRealSense2_SR300.cpp, tutorial-mb-generic-tracker-apriltag-live-realsense2.cpp, and tutorial-mb-generic-tracker-rgbd-realsense.cpp.

Definition at line 597 of file vpRealSense2.cpp.

◆ open()

◆ setInvalidDepthValue()

void vpRealSense2::setInvalidDepthValue ( const float  value)
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 335 of file vpRealSense2.h.

◆ setMaxZ()

void vpRealSense2::setMaxZ ( const float  maxZ)
inline

Set the maximum Z value (used to discard bad reconstructed depth for pointcloud).

Definition at line 339 of file vpRealSense2.h.

Friends And Related Function Documentation

◆ operator<<

std::ostream & operator<< ( std::ostream &  os,
const vpRealSense2 rs 
)
friend

Return information from the sensor.

Parameters
os: Input stream.
rs: RealSense object.

The following example shows how to use this method.

#include <visp3/sensor/vpRealSense2.h>
int main()
{
rs.open();
std::cout << "RealSense sensor information:\n" << rs << std::endl;
return 0;
}

Definition at line 769 of file vpRealSense2.cpp.

Member Data Documentation

◆ m_colorIntrinsics

rs2_intrinsics vpRealSense2::m_colorIntrinsics
protected

Definition at line 342 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_depth2ColorExtrinsics

rs2_extrinsics vpRealSense2::m_depth2ColorExtrinsics
protected

Definition at line 343 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_depthIntrinsics

rs2_intrinsics vpRealSense2::m_depthIntrinsics
protected

Definition at line 344 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_depthScale

float vpRealSense2::m_depthScale
protected

Definition at line 345 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_invalidDepthValue

float vpRealSense2::m_invalidDepthValue
protected

Definition at line 346 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_max_Z

float vpRealSense2::m_max_Z
protected

Definition at line 347 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_pipe

rs2::pipeline vpRealSense2::m_pipe
protected

Definition at line 348 of file vpRealSense2.h.

◆ m_pipelineProfile

rs2::pipeline_profile vpRealSense2::m_pipelineProfile
protected

Definition at line 349 of file vpRealSense2.h.

◆ m_pointcloud

rs2::pointcloud vpRealSense2::m_pointcloud
protected

Definition at line 350 of file vpRealSense2.h.

Referenced by getPointcloud().

◆ m_points

rs2::points vpRealSense2::m_points
protected

Definition at line 351 of file vpRealSense2.h.

Referenced by getPointcloud().

vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpRealSense2::getPipeline
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.
Definition: vpRealSense2.h:321
vpRealSense2::getPipelineProfile
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.
Definition: vpRealSense2.h:324
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpRealSense2::acquire
void acquire(vpImage< unsigned char > &grey)
Definition: vpRealSense2.cpp:66
vpRealSense2
Definition: vpRealSense2.h:280
vpRealSense2::getIntrinsics
rs2_intrinsics getIntrinsics(const rs2_stream &stream) const
Definition: vpRealSense2.cpp:268
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:676
vpImageConvert::createDepthHistogram
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
Definition: vpImageConvert.cpp:200
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:652
vpImage< unsigned char >
vpRealSense2::open
void open(const rs2::config &cfg=rs2::config())
Definition: vpRealSense2.cpp:618
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:701