Loading...
Searching...
No Matches
real_sense_2_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2018-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#pragma once
#include <thread>
#include <mutex>
#include <pcl/io/grabber.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <librealsense2/rs.hpp>
namespace pcl
{
/** \brief Grabber for Intel Realsense 2 SDK devices (D400 series)
* \note Device width/height defaults to 424/240, the lowest resolutions for D400 devices.
* \note Testing on the in_hand_scanner example we found the lower default resolution allowed the app to perform adequately.
* \note Developers should use this resolution as a starting point and gradually increase to get the best results
* \author Patrick Abadi <patrickabadi@gmail.com>, Daniel Packard <pack3754@gmail.com>
* \ingroup io
*/
class PCL_EXPORTS RealSense2Grabber : public pcl::Grabber
{
public:
/** \brief Constructor
* \param[in] file_name_or_serial_number used for either loading bag file or specific device by serial number
* \param[in] repeat_playback whether to repeat playback when reading from file
*/
RealSense2Grabber ( const std::string& file_name_or_serial_number = "", const bool repeat_playback = true );
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
~RealSense2Grabber () override;
/** \brief Set the device options
* \param[in] width resolution
* \param[in] height resolution
* \param[in] fps target frames per second for the device
*/
inline void
setDeviceOptions ( std::uint32_t width, std::uint32_t height, std::uint32_t fps = 30 )
{
device_width_ = width;
device_height_ = height;
target_fps_ = fps;
reInitialize ();
}
/** \brief Start the data acquisition. */
void
start () override;
/** \brief Stop the data acquisition. */
void
stop () override;
/** \brief Check if the data acquisition is still running. */
bool
isRunning () const override;
/** \brief Obtain the number of frames per second (FPS). */
float
getFramesPerSecond () const override;
/** \brief defined grabber name*/
std::string
getName () const override {
return {"RealSense2Grabber"}; }
//define callback signature typedefs
using signal_librealsense_PointXYZ = void( const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& );
using signal_librealsense_PointXYZI = void( const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& );
using signal_librealsense_PointXYZRGB = void( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& );
using signal_librealsense_PointXYZRGBA = void( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& );
protected:
boost::signals2::signal<signal_librealsense_PointXYZ>* signal_PointXYZ;
boost::signals2::signal<signal_librealsense_PointXYZI>* signal_PointXYZI;
boost::signals2::signal<signal_librealsense_PointXYZRGB>* signal_PointXYZRGB;
boost::signals2::signal<signal_librealsense_PointXYZRGBA>* signal_PointXYZRGBA;
/** \brief Handle when a signal callback has been changed
*/
void
signalsChanged () override;
/** \brief the thread function
*/
void
threadFunction ();
/** \brief Dynamic reinitialization.
*/
void
reInitialize ();
/** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
* \param[in] points the depth points
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr
convertDepthToPointXYZ ( const rs2::points& points );
/** \brief Convert an Infrared Depth image to a pcl::PointCloud<pcl::PointXYZI>
* \param[in] points the depth points
* \param[in] ir Infrared video frame
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr
convertIntensityDepthToPointXYZRGBI ( const rs2::points& points, const rs2::video_frame& ir );
/** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGB>
* \param[in] points the depth points
* \param[in] rgb rgb video frame
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
convertRGBDepthToPointXYZRGB ( const rs2::points& points, const rs2::video_frame& rgb );
/** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGBA>
* \param[in] points the depth points
* \param[in] rgb rgb video frame
*/
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
convertRGBADepthToPointXYZRGBA ( const rs2::points& points, const rs2::video_frame& rgb );
/** \brief template function to convert realsense point cloud to PCL point cloud
* \param[in] points - realsense point cloud array
* \param[in] mapColorFunc dynamic function to convert individual point color or intensity values
*/
template <typename PointT, typename Functor>
typename pcl::PointCloud<PointT>::Ptr
convertRealsensePointsToPointCloud ( const rs2::points& points, Functor mapColorFunc );
/** \brief Retrieve pixel index for UV texture coordinate
* \param[in] texture the texture
* \param[in] u 2D coordinate
* \param[in] v 2D coordinate
*/
static size_t
getTextureIdx (const rs2::video_frame & texture, float u, float v);
/** \brief Retrieve RGB color from texture video frame
* \param[in] texture the texture
* \param[in] u 2D coordinate
* \param[in] v 2D coordinate
*/
static pcl::RGB
getTextureColor ( const rs2::video_frame& texture, float u, float v );
/** \brief Retrieve color intensity from texture video frame
* \param[in] texture the texture
* \param[in] u 2D coordinate
* \param[in] v 2D coordinate
*/
static std::uint8_t
getTextureIntensity ( const rs2::video_frame& texture, float u, float v );
/** \brief handle to the thread */
std::thread thread_;
/** \brief Defines either a file path to a bag file or a realsense device serial number. */
std::string file_name_or_serial_number_;
/** \brief Repeat playback when reading from file */
bool repeat_playback_;
/** \brief controlling the state of the thread. */
bool quit_{false};
/** \brief Is the grabber running. */
bool running_{false};
/** \brief Calculated FPS for the grabber. */
float fps_{0.0f};
/** \brief Width for the depth and color sensor. Default 424*/
std::uint32_t device_width_{424};
/** \brief Height for the depth and color sensor. Default 240 */
std::uint32_t device_height_{240};
/** \brief Target FPS for the device. Default 30. */
std::uint32_t target_fps_{30};
/** \brief Declare pointcloud object, for calculating pointclouds and texture mappings */
rs2::pointcloud pc_;
/** \brief Declare RealSense pipeline, encapsulating the actual device and sensors */
rs2::pipeline pipe_;
};
}
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition grabber.h:60
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::RealSense2Grabber
Grabber for Intel Realsense 2 SDK devices (D400 series)
Definition real_sense_2_grabber.h:60
pcl::RealSense2Grabber::convertIntensityDepthToPointXYZRGBI
pcl::PointCloud< pcl::PointXYZI >::Ptr convertIntensityDepthToPointXYZRGBI(const rs2::points &points, const rs2::video_frame &ir)
Convert an Infrared Depth image to a pcl::PointCloud<pcl::PointXYZI>
pcl::RealSense2Grabber::thread_
std::thread thread_
handle to the thread
Definition real_sense_2_grabber.h:196
pcl::RealSense2Grabber::signal_PointXYZ
boost::signals2::signal< signal_librealsense_PointXYZ > * signal_PointXYZ
Definition real_sense_2_grabber.h:115
pcl::RealSense2Grabber::getFramesPerSecond
float getFramesPerSecond() const override
Obtain the number of frames per second (FPS).
pcl::RealSense2Grabber::threadFunction
void threadFunction()
the thread function
pcl::RealSense2Grabber::getTextureIntensity
static std::uint8_t getTextureIntensity(const rs2::video_frame &texture, float u, float v)
Retrieve color intensity from texture video frame.
pcl::RealSense2Grabber::RealSense2Grabber
RealSense2Grabber(const std::string &file_name_or_serial_number="", const bool repeat_playback=true)
Constructor.
pcl::RealSense2Grabber::signal_librealsense_PointXYZI
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) signal_librealsense_PointXYZI
Definition real_sense_2_grabber.h:109
pcl::RealSense2Grabber::~RealSense2Grabber
~RealSense2Grabber() override
virtual Destructor inherited from the Grabber interface.
pcl::RealSense2Grabber::signal_PointXYZRGBA
boost::signals2::signal< signal_librealsense_PointXYZRGBA > * signal_PointXYZRGBA
Definition real_sense_2_grabber.h:118
pcl::RealSense2Grabber::signal_librealsense_PointXYZ
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) signal_librealsense_PointXYZ
Definition real_sense_2_grabber.h:108
pcl::RealSense2Grabber::stop
void stop() override
Stop the data acquisition.
pcl::RealSense2Grabber::convertRGBADepthToPointXYZRGBA
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr convertRGBADepthToPointXYZRGBA(const rs2::points &points, const rs2::video_frame &rgb)
Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGBA>
pcl::RealSense2Grabber::signal_PointXYZI
boost::signals2::signal< signal_librealsense_PointXYZI > * signal_PointXYZI
Definition real_sense_2_grabber.h:116
pcl::RealSense2Grabber::signal_librealsense_PointXYZRGBA
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) signal_librealsense_PointXYZRGBA
Definition real_sense_2_grabber.h:111
pcl::RealSense2Grabber::convertRGBDepthToPointXYZRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr convertRGBDepthToPointXYZRGB(const rs2::points &points, const rs2::video_frame &rgb)
Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGB>
pcl::RealSense2Grabber::convertDepthToPointXYZ
pcl::PointCloud< pcl::PointXYZ >::Ptr convertDepthToPointXYZ(const rs2::points &points)
Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
pcl::RealSense2Grabber::file_name_or_serial_number_
std::string file_name_or_serial_number_
Defines either a file path to a bag file or a realsense device serial number.
Definition real_sense_2_grabber.h:198
pcl::RealSense2Grabber::signal_librealsense_PointXYZRGB
void(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &) signal_librealsense_PointXYZRGB
Definition real_sense_2_grabber.h:110
pcl::RealSense2Grabber::pc_
rs2::pointcloud pc_
Declare pointcloud object, for calculating pointclouds and texture mappings.
Definition real_sense_2_grabber.h:214
pcl::RealSense2Grabber::reInitialize
void reInitialize()
Dynamic reinitialization.
pcl::RealSense2Grabber::setDeviceOptions
void setDeviceOptions(std::uint32_t width, std::uint32_t height, std::uint32_t fps=30)
Set the device options.
Definition real_sense_2_grabber.h:77
pcl::RealSense2Grabber::signalsChanged
void signalsChanged() override
Handle when a signal callback has been changed.
pcl::RealSense2Grabber::convertRealsensePointsToPointCloud
pcl::PointCloud< PointT >::Ptr convertRealsensePointsToPointCloud(const rs2::points &points, Functor mapColorFunc)
template function to convert realsense point cloud to PCL point cloud
pcl::RealSense2Grabber::getTextureIdx
static size_t getTextureIdx(const rs2::video_frame &texture, float u, float v)
Retrieve pixel index for UV texture coordinate.
pcl::RealSense2Grabber::repeat_playback_
bool repeat_playback_
Repeat playback when reading from file.
Definition real_sense_2_grabber.h:200
pcl::RealSense2Grabber::getTextureColor
static pcl::RGB getTextureColor(const rs2::video_frame &texture, float u, float v)
Retrieve RGB color from texture video frame.
pcl::RealSense2Grabber::isRunning
bool isRunning() const override
Check if the data acquisition is still running.
pcl::RealSense2Grabber::pipe_
rs2::pipeline pipe_
Declare RealSense pipeline, encapsulating the actual device and sensors.
Definition real_sense_2_grabber.h:216
pcl::RealSense2Grabber::start
void start() override
Start the data acquisition.
pcl::RealSense2Grabber::signal_PointXYZRGB
boost::signals2::signal< signal_librealsense_PointXYZRGB > * signal_PointXYZRGB
Definition real_sense_2_grabber.h:117
pcl::RealSense2Grabber::getName
std::string getName() const override
defined grabber name
Definition real_sense_2_grabber.h:104
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl
Definition convolution.h:46
pcl::Functor
Base functor all the models that need non linear optimization must define their own one and implement...
Definition sac_model.h:680
pcl::RGB
A structure representing RGB color information.
Definition point_types.hpp:363