Point Cloud Library (PCL)  1.10.0-dev
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 
45 namespace 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>
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>
Detection()
Constructor.
Definition: line_rgbd.h:80
float z
Z-coordinate of the upper left front point.
Definition: line_rgbd.h:58
A TAR file&#39;s header, as described on http://en.wikipedia.org/wiki/Tar_%28file_format%29.
Definition: tar.h:49
std::vector< pcl::BoundingBoxXYZ > bounding_boxes_
Bounding boxes corresponding to the templates.
Definition: line_rgbd.h:303
pcl::ColorGradientModality< PointRGBT > color_gradient_mod_
Color gradient modality.
Definition: line_rgbd.h:291
Template matching using the LINEMOD approach.
Definition: linemod.h:334
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
std::size_t template_id
The ID of the template.
Definition: line_rgbd.h:83
pcl::PointCloud< pcl::PointXYZRGBA >::CloudVectorType template_point_clouds_
Point clouds corresponding to the templates.
Definition: line_rgbd.h:301
std::size_t object_id
The ID of the object corresponding to the template.
Definition: line_rgbd.h:85
A LineRGBD detection.
Definition: line_rgbd.h:77
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold on the magnitude of color gradients.
Definition: line_rgbd.h:143
void setDetectionThreshold(float threshold)
Sets the threshold for the detection responses.
Definition: line_rgbd.h:133
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
Defines a region in XY-space.
Definition: region_xy.h:81
float y
Y-coordinate of the upper left front point.
Definition: line_rgbd.h:56
A multi-modality template constructed from a set of quantized multi-modality features.
float x
X-coordinate of the upper left front point.
Definition: line_rgbd.h:54
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
pcl::LINEMOD linemod_
LINEMOD instance.
Definition: line_rgbd.h:289
void setInputColors(const typename pcl::PointCloud< PointRGBT >::ConstPtr &cloud)
Sets the input cloud with rgb values.
Definition: line_rgbd.h:177
float width
Width of the bounding box.
Definition: line_rgbd.h:61
std::vector< std::size_t > object_ids_
Object IDs corresponding to the templates.
Definition: line_rgbd.h:305
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
Definition: point_cloud.h:414
BoundingBoxXYZ()
Constructor.
Definition: line_rgbd.h:51
RegionXY region
The 2D template region of the detection.
Definition: line_rgbd.h:93
float depth
Depth of the bounding box.
Definition: line_rgbd.h:65
LineRGBD()
Constructor.
Definition: line_rgbd.h:97
void setInputCloud(const typename pcl::PointCloud< PointXYZT >::ConstPtr &cloud)
Sets the input cloud with xyz point coordinates.
Definition: line_rgbd.h:165
pcl::PointCloud< PointRGBT >::ConstPtr cloud_rgb_
RGB point cloud.
Definition: line_rgbd.h:298
virtual ~LineRGBD()
Destructor.
Definition: line_rgbd.h:108
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
std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > detections_
Detections from last call of method detect (...).
Definition: line_rgbd.h:308
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::SurfaceNormalModality< PointXYZT > surface_normal_mod_
Surface normal modality.
Definition: line_rgbd.h:293
High-level class for template matching using the LINEMOD approach based on RGB and Depth data...
Definition: line_rgbd.h:72
std::size_t detection_id
The ID of this detection.
Definition: line_rgbd.h:87
#define PCL_EXPORTS
Definition: pcl_macros.h:253
pcl::PointCloud< PointXYZT >::ConstPtr cloud_xyz_
XYZ point cloud.
Definition: line_rgbd.h:296
float response
The response of this detection.
Definition: line_rgbd.h:89
float height
Height of the bounding box.
Definition: line_rgbd.h:63
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition: line_rgbd.h:91