Point Cloud Library (PCL) 1.15.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 * \param[in] repeat_playback whether to repeat playback when reading from file
65 */
66 RealSense2Grabber ( const std::string& file_name_or_serial_number = "", const bool repeat_playback = true );
67
68 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
69 ~RealSense2Grabber () override;
70
71 /** \brief Set the device options
72 * \param[in] width resolution
73 * \param[in] height resolution
74 * \param[in] fps target frames per second for the device
75 */
76 inline void
77 setDeviceOptions ( std::uint32_t width, std::uint32_t height, std::uint32_t fps = 30 )
78 {
79 device_width_ = width;
80 device_height_ = height;
81 target_fps_ = fps;
82
83 reInitialize ();
84 }
85
86 /** \brief Start the data acquisition. */
87 void
88 start () override;
89
90 /** \brief Stop the data acquisition. */
91 void
92 stop () override;
93
94 /** \brief Check if the data acquisition is still running. */
95 bool
96 isRunning () const override;
97
98 /** \brief Obtain the number of frames per second (FPS). */
99 float
100 getFramesPerSecond () const override;
101
102 /** \brief defined grabber name*/
103 std::string
104 getName () const override {
105 return {"RealSense2Grabber"}; }
106
107 //define callback signature typedefs
112
113 protected:
114
115 boost::signals2::signal<signal_librealsense_PointXYZ>* signal_PointXYZ;
116 boost::signals2::signal<signal_librealsense_PointXYZI>* signal_PointXYZI;
117 boost::signals2::signal<signal_librealsense_PointXYZRGB>* signal_PointXYZRGB;
118 boost::signals2::signal<signal_librealsense_PointXYZRGBA>* signal_PointXYZRGBA;
119
120 /** \brief Handle when a signal callback has been changed
121 */
122 void
123 signalsChanged () override;
124
125 /** \brief the thread function
126 */
127 void
129
130 /** \brief Dynamic reinitialization.
131 */
132 void
134
135 /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
136 * \param[in] points the depth points
137 */
139 convertDepthToPointXYZ ( const rs2::points& points );
140
141 /** \brief Convert an Infrared Depth image to a pcl::PointCloud<pcl::PointXYZI>
142 * \param[in] points the depth points
143 * \param[in] ir Infrared video frame
144 */
146 convertIntensityDepthToPointXYZRGBI ( const rs2::points& points, const rs2::video_frame& ir );
147
148 /** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGB>
149 * \param[in] points the depth points
150 * \param[in] rgb rgb video frame
151 */
153 convertRGBDepthToPointXYZRGB ( const rs2::points& points, const rs2::video_frame& rgb );
154
155 /** \brief Convert an rgb Depth image to a pcl::PointCloud<pcl::PointXYZRGBA>
156 * \param[in] points the depth points
157 * \param[in] rgb rgb video frame
158 */
160 convertRGBADepthToPointXYZRGBA ( const rs2::points& points, const rs2::video_frame& rgb );
161
162 /** \brief template function to convert realsense point cloud to PCL point cloud
163 * \param[in] points - realsense point cloud array
164 * \param[in] mapColorFunc dynamic function to convert individual point color or intensity values
165 */
166 template <typename PointT, typename Functor>
168 convertRealsensePointsToPointCloud ( const rs2::points& points, Functor mapColorFunc );
169
170 /** \brief Retrieve pixel index for UV texture coordinate
171 * \param[in] texture the texture
172 * \param[in] u 2D coordinate
173 * \param[in] v 2D coordinate
174 */
175 static size_t
176 getTextureIdx (const rs2::video_frame & texture, float u, float v);
177
178 /** \brief Retrieve RGB color from texture video frame
179 * \param[in] texture the texture
180 * \param[in] u 2D coordinate
181 * \param[in] v 2D coordinate
182 */
183 static pcl::RGB
184 getTextureColor ( const rs2::video_frame& texture, float u, float v );
185
186 /** \brief Retrieve color intensity from texture video frame
187 * \param[in] texture the texture
188 * \param[in] u 2D coordinate
189 * \param[in] v 2D coordinate
190 */
191 static std::uint8_t
192 getTextureIntensity ( const rs2::video_frame& texture, float u, float v );
193
194
195 /** \brief handle to the thread */
196 std::thread thread_;
197 /** \brief Defines either a file path to a bag file or a realsense device serial number. */
199 /** \brief Repeat playback when reading from file */
201 /** \brief controlling the state of the thread. */
202 bool quit_{false};
203 /** \brief Is the grabber running. */
204 bool running_{false};
205 /** \brief Calculated FPS for the grabber. */
206 float fps_{0.0f};
207 /** \brief Width for the depth and color sensor. Default 424*/
208 std::uint32_t device_width_{424};
209 /** \brief Height for the depth and color sensor. Default 240 */
210 std::uint32_t device_height_{240};
211 /** \brief Target FPS for the device. Default 30. */
212 std::uint32_t target_fps_{30};
213 /** \brief Declare pointcloud object, for calculating pointclouds and texture mappings */
214 rs2::pointcloud pc_;
215 /** \brief Declare RealSense pipeline, encapsulating the actual device and sensors */
216 rs2::pipeline pipe_;
217 };
218
219}
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::thread thread_
handle to the thread
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.
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) signal_librealsense_PointXYZI
~RealSense2Grabber() override
virtual Destructor inherited from the Grabber interface.
boost::signals2::signal< signal_librealsense_PointXYZRGBA > * signal_PointXYZRGBA
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) signal_librealsense_PointXYZ
void stop() override
Stop the data acquisition.
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
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) signal_librealsense_PointXYZRGBA
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::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.
void(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &) signal_librealsense_PointXYZRGB
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:680
A structure representing RGB color information.