Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
real_sense_2_grabber.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2018-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
40#include <thread>
41#include <mutex>
42
43#include <pcl/io/grabber.h>
44#include <pcl/point_cloud.h>
45#include <pcl/point_types.h>
46
47#include <librealsense2/rs.hpp>
48
49namespace pcl
50{
51
52 /** \brief Grabber for Intel Realsense 2 SDK devices (D400 series)
53 * \note Device width/height defaults to 424/240, the lowest resolutions for D400 devices.
54 * \note Testing on the in_hand_scanner example we found the lower default resolution allowed the app to perform adequately.
55 * \note Developers should use this resolution as a starting point and gradually increase to get the best results
56 * \author Patrick Abadi <patrickabadi@gmail.com>, Daniel Packard <pack3754@gmail.com>
57 * \ingroup io
58 */
59 class PCL_EXPORTS RealSense2Grabber : public pcl::Grabber
60 {
61 public:
62 /** \brief Constructor
63 * \param[in] file_name_or_serial_number used for either loading bag file or specific device by serial number
64 */
65 RealSense2Grabber ( const std::string& file_name_or_serial_number = "", const bool repeat_playback = true );
66
67 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
69
70 /** \brief Set the device options
71 * \param[in] width resolution
72 * \param[in] height resolution
73 * \param[in] fps target frames per second for the device
74 */
75 inline void
76 setDeviceOptions ( std::uint32_t width, std::uint32_t height, std::uint32_t fps = 30 )
77 {
78 device_width_ = width;
79 device_height_ = height;
80 target_fps_ = fps;
81
82 reInitialize ();
83 }
84
85 /** \brief Start the data acquisition. */
86 void
87 start () override;
88
89 /** \brief Stop the data acquisition. */
90 void
91 stop () override;
92
93 /** \brief Check if the data acquisition is still running. */
94 bool
95 isRunning () const override;
96
97 /** \brief Obtain the number of frames per second (FPS). */
98 float
99 getFramesPerSecond () const override;
100
101 /** \brief defined grabber name*/
102 std::string
103 getName () const override { return std::string ( "RealSense2Grabber" ); }
104
105 //define callback signature typedefs
106 typedef void (signal_librealsense_PointXYZ) ( const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& );
107 typedef void (signal_librealsense_PointXYZI) ( const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& );
108 typedef void (signal_librealsense_PointXYZRGB) ( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& );
109 typedef void (signal_librealsense_PointXYZRGBA) ( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& );
110
111 protected:
112
113 boost::signals2::signal<signal_librealsense_PointXYZ>* signal_PointXYZ;
114 boost::signals2::signal<signal_librealsense_PointXYZI>* signal_PointXYZI;
115 boost::signals2::signal<signal_librealsense_PointXYZRGB>* signal_PointXYZRGB;
116 boost::signals2::signal<signal_librealsense_PointXYZRGBA>* signal_PointXYZRGBA;
117
118 /** \brief Handle when a signal callback has been changed
119 */
120 void
121 signalsChanged () override;
122
123 /** \brief the thread function
124 */
125 void
127
128 /** \brief Dynamic reinitialization.
129 */
130 void
132
133 /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
134 * \param[in] points the depth points
135 */
137 convertDepthToPointXYZ ( const rs2::points& points );
138
139 /** \brief Convert an Infrared Depth image to a pcl::PointCloud<pcl::PointXYZI>
140 * \param[in] points the depth points
141 * \param[in] ir Infrared video frame
142 */
144 convertIntensityDepthToPointXYZRGBI ( const rs2::points& points, const rs2::video_frame& ir );
145
146 /** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGB>
147 * \param[in] points the depth points
148 * \param[in] rgb rgb video frame
149 */
151 convertRGBDepthToPointXYZRGB ( const rs2::points& points, const rs2::video_frame& rgb );
152
153 /** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGBA>
154 * \param[in] points the depth points
155 * \param[in] rgb rgb video frame
156 */
158 convertRGBADepthToPointXYZRGBA ( const rs2::points& points, const rs2::video_frame& rgb );
159
160 /** \brief template function to convert realsense point cloud to PCL point cloud
161 * \param[in] points - realsense point cloud array
162 * \param[in] mapColorFunc dynamic function to convert individual point color or intensity values
163 */
164 template <typename PointT, typename Functor>
167
168 /** \brief Retrieve pixel index for UV texture coordinate
169 * \param[in] texture the texture
170 * \param[in] u 2D coordinate
171 * \param[in] v 2D coordinate
172 */
173 static size_t
174 getTextureIdx (const rs2::video_frame & texture, float u, float v);
175
176 /** \brief Retrieve RGB color from texture video frame
177 * \param[in] texture the texture
178 * \param[in] u 2D coordinate
179 * \param[in] v 2D coordinate
180 */
181 static pcl::RGB
182 getTextureColor ( const rs2::video_frame& texture, float u, float v );
183
184 /** \brief Retrieve color intensity from texture video frame
185 * \param[in] texture the texture
186 * \param[in] u 2D coordinate
187 * \param[in] v 2D coordinate
188 */
189 static std::uint8_t
190 getTextureIntensity ( const rs2::video_frame& texture, float u, float v );
191
192
193 /** \brief handle to the thread */
194 std::thread thread_;
195 /** \brief Defines either a file path to a bag file or a realsense device serial number. */
197 /** \brief Repeat playback when reading from file */
199 /** \brief controlling the state of the thread. */
200 bool quit_;
201 /** \brief Is the grabber running. */
203 /** \brief Calculated FPS for the grabber. */
204 float fps_;
205 /** \brief Width for the depth and color sensor. Default 424*/
206 std::uint32_t device_width_;
207 /** \brief Height for the depth and color sensor. Default 240 */
208 std::uint32_t device_height_;
209 /** \brief Target FPS for the device. Default 30. */
210 std::uint32_t target_fps_;
211 /** \brief Declare pointcloud object, for calculating pointclouds and texture mappings */
212 rs2::pointcloud pc_;
213 /** \brief Declare RealSense pipeline, encapsulating the actual device and sensors */
214 rs2::pipeline pipe_;
215 };
216
217}
Iterator class for point clouds with or without given indices.
Grabber interface for PCL 1.x device drivers.
Definition grabber.h:60
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Grabber for Intel Realsense 2 SDK devices (D400 series)
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>
std::uint32_t device_height_
Height for the depth and color sensor.
std::thread thread_
handle to the thread
std::uint32_t device_width_
Width for the depth and color sensor.
float fps_
Calculated FPS for the grabber.
boost::signals2::signal< signal_librealsense_PointXYZ > * signal_PointXYZ
float getFramesPerSecond() const override
Obtain the number of frames per second (FPS).
void threadFunction()
the thread function
static std::uint8_t getTextureIntensity(const rs2::video_frame &texture, float u, float v)
Retrieve color intensity from texture video frame.
RealSense2Grabber(const std::string &file_name_or_serial_number="", const bool repeat_playback=true)
Constructor.
boost::signals2::signal< signal_librealsense_PointXYZRGBA > * signal_PointXYZRGBA
bool running_
Is the grabber running.
void stop() override
Stop the data acquisition.
~RealSense2Grabber()
virtual Destructor inherited from the Grabber interface.
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>
boost::signals2::signal< signal_librealsense_PointXYZI > * signal_PointXYZI
std::uint32_t target_fps_
Target FPS for the device.
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>
bool quit_
controlling the state of the thread.
pcl::PointCloud< pcl::PointXYZ >::Ptr convertDepthToPointXYZ(const rs2::points &points)
Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
std::string file_name_or_serial_number_
Defines either a file path to a bag file or a realsense device serial number.
rs2::pointcloud pc_
Declare pointcloud object, for calculating pointclouds and texture mappings.
void reInitialize()
Dynamic reinitialization.
void setDeviceOptions(std::uint32_t width, std::uint32_t height, std::uint32_t fps=30)
Set the device options.
void signalsChanged() override
Handle when a signal callback has been changed.
pcl::PointCloud< PointT >::Ptr convertRealsensePointsToPointCloud(const rs2::points &points, Functor mapColorFunc)
template function to convert realsense point cloud to PCL point cloud
static size_t getTextureIdx(const rs2::video_frame &texture, float u, float v)
Retrieve pixel index for UV texture coordinate.
bool repeat_playback_
Repeat playback when reading from file.
static pcl::RGB getTextureColor(const rs2::video_frame &texture, float u, float v)
Retrieve RGB color from texture video frame.
bool isRunning() const override
Check if the data acquisition is still running.
rs2::pipeline pipe_
Declare RealSense pipeline, encapsulating the actual device and sensors.
void start() override
Start the data acquisition.
boost::signals2::signal< signal_librealsense_PointXYZRGB > * signal_PointXYZRGB
std::string getName() const override
defined grabber name
Defines all the PCL implemented PointT point type structures.
Base functor all the models that need non linear optimization must define their own one and implement...
Definition sac_model.h:679
A structure representing RGB color information.