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