Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
line_rgbd.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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 <pcl/recognition/linemod.h>
41#include <pcl/recognition/color_gradient_modality.h>
42#include <pcl/recognition/surface_normal_modality.h>
43#include <pcl/io/tar.h>
44
45namespace pcl
46{
47
49 {
50 /** \brief Constructor. */
51 BoundingBoxXYZ () : x (0.0f), y (0.0f), z (0.0f), width (0.0f), height (0.0f), depth (0.0f) {}
52
53 /** \brief X-coordinate of the upper left front point */
54 float x;
55 /** \brief Y-coordinate of the upper left front point */
56 float y;
57 /** \brief Z-coordinate of the upper left front point */
58 float z;
59
60 /** \brief Width of the bounding box */
61 float width;
62 /** \brief Height of the bounding box */
63 float height;
64 /** \brief Depth of the bounding box */
65 float depth;
66 };
67
68 /** \brief High-level class for template matching using the LINEMOD approach based on RGB and Depth data.
69 * \author Stefan Holzer
70 */
71 template <typename PointXYZT, typename PointRGBT=PointXYZT>
72 class PCL_EXPORTS LineRGBD
73 {
74 public:
75
76 /** \brief A LineRGBD detection. */
77 struct Detection
78 {
79 /** \brief Constructor. */
80 Detection () : template_id (0), object_id (0), detection_id (0), response (0.0f) {}
81
82 /** \brief The ID of the template. */
83 std::size_t template_id;
84 /** \brief The ID of the object corresponding to the template. */
85 std::size_t object_id;
86 /** \brief The ID of this detection. This is only valid for the last call of the method detect (...). */
87 std::size_t detection_id;
88 /** \brief The response of this detection. Responses are between 0 and 1, where 1 is best. */
89 float response;
90 /** \brief The 3D bounding box of the detection. */
92 /** \brief The 2D template region of the detection. */
94 };
95
96 /** \brief Constructor */
98 : intersection_volume_threshold_ (1.0f)
99 , color_gradient_mod_ ()
100 , surface_normal_mod_ ()
101 , cloud_xyz_ ()
102 , cloud_rgb_ ()
103 , detections_ ()
104 {
105 }
106
107 /** \brief Destructor */
108 virtual ~LineRGBD ()
109 {
110 }
111
112 /** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates.
113 *
114 * LineMod Template files are TAR files that store pairs of PCD datasets
115 * together with their LINEMOD signatures in \ref
116 * SparseQuantizedMultiModTemplate format.
117 *
118 * \param[in] file_name The name of the file that stores the templates.
119 * \param object_id
120 *
121 * \return true, if the operation was successful, false otherwise.
122 */
123 bool
124 loadTemplates (const std::string &file_name, std::size_t object_id = 0);
125
126 bool
127 addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, std::size_t object_id = 0);
128
129 /** \brief Sets the threshold for the detection responses. Responses are between 0 and 1, where 1 is a best.
130 * \param[in] threshold The threshold used to decide where a template is detected.
131 */
132 inline void
133 setDetectionThreshold (float threshold)
134 {
135 linemod_.setDetectionThreshold (threshold);
136 }
137
138 /** \brief Sets the threshold on the magnitude of color gradients. Color gradients with a magnitude below
139 * this threshold are not considered in the detection process.
140 * \param[in] threshold The threshold on the magnitude of color gradients.
141 */
142 inline void
143 setGradientMagnitudeThreshold (const float threshold)
144 {
145 color_gradient_mod_.setGradientMagnitudeThreshold (threshold);
146 }
147
148 /** \brief Sets the threshold for the decision whether two detections of the same template are merged or not.
149 * If ratio between the intersection of the bounding boxes of two detections and the original bounding
150 * boxes is larger than the specified threshold then they are merged. If detection A overlaps with
151 * detection B and B with C than A, B, and C are merged. Threshold has to be between 0 and 1.
152 * \param[in] threshold The threshold on the ratio between the intersection bounding box and the original
153 * bounding box.
154 */
155 inline void
156 setIntersectionVolumeThreshold (const float threshold = 1.0f)
157 {
158 intersection_volume_threshold_ = threshold;
159 }
160
161 /** \brief Sets the input cloud with xyz point coordinates. The cloud has to be organized.
162 * \param[in] cloud The input cloud with xyz point coordinates.
163 */
164 inline void
166 {
167 cloud_xyz_ = cloud;
168
169 surface_normal_mod_.setInputCloud (cloud);
170 surface_normal_mod_.processInputData ();
171 }
172
173 /** \brief Sets the input cloud with rgb values. The cloud has to be organized.
174 * \param[in] cloud The input cloud with rgb values.
175 */
176 inline void
178 {
179 cloud_rgb_ = cloud;
180
181 color_gradient_mod_.setInputCloud (cloud);
182 color_gradient_mod_.processInputData ();
183 }
184
185 /** \brief Creates a template from the specified data and adds it to the matching queue.
186 * \param cloud
187 * \param object_id
188 * \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template.
189 * \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template.
190 * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps).
191 */
192 int
193 createAndAddTemplate (
195 const std::size_t object_id,
196 const MaskMap & mask_xyz,
197 const MaskMap & mask_rgb,
198 const RegionXY & region);
199
200
201 /** \brief Applies the detection process and fills the supplied vector with the detection instances.
202 * \param[out] detections The storage for the detection instances.
203 */
204 void
205 detect (std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections);
206
207 /** \brief Applies the detection process in a semi-scale-invariant manner. This is done by actually
208 * scaling the template to different sizes.
209 */
210 void
211 detectSemiScaleInvariant (std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections,
212 float min_scale = 0.6944444f,
213 float max_scale = 1.44f,
214 float scale_multiplier = 1.2f);
215
216 /** \brief Computes and returns the point cloud of the specified detection. This is the template point
217 * cloud transformed to the detection coordinates. The detection ID refers to the last call of
218 * the method detect (...).
219 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
220 * \param[out] cloud The storage for the transformed points.
221 */
222 void
223 computeTransformedTemplatePoints (const std::size_t detection_id,
225
226 /** \brief Finds the indices of the points in the input cloud which correspond to the specified detection.
227 * The detection ID refers to the last call of the method detect (...).
228 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
229 */
230 inline std::vector<std::size_t>
231 findObjectPointIndices (const std::size_t detection_id)
232 {
233 if (detection_id >= detections_.size ())
234 PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
235
236 // TODO: compute transform from detection
237 // TODO: transform template points
238 std::vector<std::size_t> vec;
239 return (vec);
240 }
241
242
243 protected:
244
245 /** \brief Aligns the template points with the points found at the detection position of the specified detection.
246 * The detection ID refers to the last call of the method detect (...).
247 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
248 */
249 inline std::vector<std::size_t>
250 alignTemplatePoints (const std::size_t detection_id)
251 {
252 if (detection_id >= detections_.size ())
253 PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
254
255 // TODO: compute transform from detection
256 // TODO: transform template points
257 std::vector<std::size_t> vec;
258 return (vec);
259 }
260
261 /** \brief Refines the found detections along the depth. */
262 void
263 refineDetectionsAlongDepth ();
264
265 /** \brief Applies projective ICP on detections to find their correct position in depth. */
266 void
267 applyProjectiveDepthICPOnDetections ();
268
269 /** \brief Checks for overlapping detections, removes them and keeps only the strongest one. */
270 void
271 removeOverlappingDetections ();
272
273 /** \brief Computes the volume of the intersection between two bounding boxes.
274 * \param[in] box1 First bounding box.
275 * \param[in] box2 Second bounding box.
276 */
277 static float
278 computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2);
279
280 private:
281 /** \brief Read another LTM header chunk. */
282 bool
283 readLTMHeader (int fd, pcl::io::TARHeader &header);
284
285 /** \brief Intersection volume threshold. */
286 float intersection_volume_threshold_;
287
288 /** \brief LINEMOD instance. */
290 /** \brief Color gradient modality. */
292 /** \brief Surface normal modality. */
294
295 /** \brief XYZ point cloud. */
297 /** \brief RGB point cloud. */
299
300 /** \brief Point clouds corresponding to the templates. */
302 /** \brief Bounding boxes corresponding to the templates. */
303 std::vector<pcl::BoundingBoxXYZ> bounding_boxes_;
304 /** \brief Object IDs corresponding to the templates. */
305 std::vector<std::size_t> object_ids_;
306
307 /** \brief Detections from last call of method detect (...). */
308 std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> detections_;
309 };
310
311}
312
313#include <pcl/recognition/impl/linemod/line_rgbd.hpp>
Modality based on max-RGB gradients.
Template matching using the LINEMOD approach.
Definition linemod.h:335
High-level class for template matching using the LINEMOD approach based on RGB and Depth data.
Definition line_rgbd.h:73
void setDetectionThreshold(float threshold)
Sets the threshold for the detection responses.
Definition line_rgbd.h:133
pcl::PointCloud< PointXYZT >::ConstPtr cloud_xyz_
XYZ point cloud.
Definition line_rgbd.h:296
pcl::LINEMOD linemod_
LINEMOD instance.
Definition line_rgbd.h:289
virtual ~LineRGBD()
Destructor.
Definition line_rgbd.h:108
pcl::PointCloud< PointRGBT >::ConstPtr cloud_rgb_
RGB point cloud.
Definition line_rgbd.h:298
std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > detections_
Detections from last call of method detect (...).
Definition line_rgbd.h:308
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold on the magnitude of color gradients.
Definition line_rgbd.h:143
std::vector< pcl::BoundingBoxXYZ > bounding_boxes_
Bounding boxes corresponding to the templates.
Definition line_rgbd.h:303
std::vector< std::size_t > object_ids_
Object IDs corresponding to the templates.
Definition line_rgbd.h:305
pcl::ColorGradientModality< PointRGBT > color_gradient_mod_
Color gradient modality.
Definition line_rgbd.h:291
pcl::SurfaceNormalModality< PointXYZT > surface_normal_mod_
Surface normal modality.
Definition line_rgbd.h:293
pcl::PointCloud< pcl::PointXYZRGBA >::CloudVectorType template_point_clouds_
Point clouds corresponding to the templates.
Definition line_rgbd.h:301
void setIntersectionVolumeThreshold(const float threshold=1.0f)
Sets the threshold for the decision whether two detections of the same template are merged or not.
Definition line_rgbd.h:156
std::vector< std::size_t > alignTemplatePoints(const std::size_t detection_id)
Aligns the template points with the points found at the detection position of the specified detection...
Definition line_rgbd.h:250
void setInputColors(const typename pcl::PointCloud< PointRGBT >::ConstPtr &cloud)
Sets the input cloud with rgb values.
Definition line_rgbd.h:177
void setInputCloud(const typename pcl::PointCloud< PointXYZT >::ConstPtr &cloud)
Sets the input cloud with xyz point coordinates.
Definition line_rgbd.h:165
std::vector< std::size_t > findObjectPointIndices(const std::size_t detection_id)
Finds the indices of the points in the input cloud which correspond to the specified detection.
Definition line_rgbd.h:231
LineRGBD()
Constructor.
Definition line_rgbd.h:97
PointCloud represents the base class in PCL for storing collections of 3D points.
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
shared_ptr< const PointCloud< PointT > > ConstPtr
Modality based on surface normals.
float width
Width of the bounding box.
Definition line_rgbd.h:61
float x
X-coordinate of the upper left front point.
Definition line_rgbd.h:54
float y
Y-coordinate of the upper left front point.
Definition line_rgbd.h:56
float depth
Depth of the bounding box.
Definition line_rgbd.h:65
BoundingBoxXYZ()
Constructor.
Definition line_rgbd.h:51
float z
Z-coordinate of the upper left front point.
Definition line_rgbd.h:58
float height
Height of the bounding box.
Definition line_rgbd.h:63
A LineRGBD detection.
Definition line_rgbd.h:78
std::size_t template_id
The ID of the template.
Definition line_rgbd.h:83
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition line_rgbd.h:91
std::size_t object_id
The ID of the object corresponding to the template.
Definition line_rgbd.h:85
float response
The response of this detection.
Definition line_rgbd.h:89
std::size_t detection_id
The ID of this detection.
Definition line_rgbd.h:87
RegionXY region
The 2D template region of the detection.
Definition line_rgbd.h:93
Detection()
Constructor.
Definition line_rgbd.h:80
Defines a region in XY-space.
Definition region_xy.h:82
A multi-modality template constructed from a set of quantized multi-modality features.
A TAR file's header, as described on http://en.wikipedia.org/wiki/Tar_%28file_format%29.
Definition tar.h:50