Point Cloud Library (PCL)  1.7.1
linemod.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
39 #define PCL_RECOGNITION_LINEMOD
40 
41 #include <vector>
42 #include <cstddef>
43 #include <string.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/recognition/quantizable_modality.h>
46 #include <pcl/recognition/region_xy.h>
47 #include <pcl/recognition/sparse_quantized_multi_mod_template.h>
48 
49 namespace pcl
50 {
51 
52  /** \brief Stores a set of energy maps.
53  * \author Stefan Holzer
54  */
55  class PCL_EXPORTS EnergyMaps
56  {
57  public:
58  /** \brief Constructor. */
59  EnergyMaps () : width_ (0), height_ (0), nr_bins_ (0), maps_ ()
60  {
61  }
62 
63  /** \brief Destructor. */
64  virtual ~EnergyMaps ()
65  {
66  }
67 
68  /** \brief Returns the width of the energy maps. */
69  inline size_t
70  getWidth () const
71  {
72  return (width_);
73  }
74 
75  /** \brief Returns the height of the energy maps. */
76  inline size_t
77  getHeight () const
78  {
79  return (height_);
80  }
81 
82  /** \brief Returns the number of bins used for quantization (which is equal to the number of energy maps). */
83  inline size_t
84  getNumOfBins () const
85  {
86  return (nr_bins_);
87  }
88 
89  /** \brief Initializes the set of energy maps.
90  * \param[in] width the width of the energy maps.
91  * \param[in] height the height of the energy maps.
92  * \param[in] nr_bins the number of bins used for quantization.
93  */
94  void
95  initialize (const size_t width, const size_t height, const size_t nr_bins)
96  {
97  maps_.resize(nr_bins, NULL);
98  width_ = width;
99  height_ = height;
100  nr_bins_ = nr_bins;
101 
102  const size_t mapsSize = width*height;
103 
104  for (size_t map_index = 0; map_index < maps_.size (); ++map_index)
105  {
106  //maps_[map_index] = new unsigned char[mapsSize];
107  maps_[map_index] = reinterpret_cast<unsigned char*> (aligned_malloc (mapsSize));
108  memset (maps_[map_index], 0, mapsSize);
109  }
110  }
111 
112  /** \brief Releases the internal data. */
113  void
114  releaseAll ()
115  {
116  for (size_t map_index = 0; map_index < maps_.size (); ++map_index)
117  //if (maps_[map_index] != NULL) delete[] maps_[map_index];
118  if (maps_[map_index] != NULL) aligned_free (maps_[map_index]);
119 
120  maps_.clear ();
121  width_ = 0;
122  height_ = 0;
123  nr_bins_ = 0;
124  }
125 
126  /** \brief Operator for accessing a specific element in the set of energy maps.
127  * \param[in] bin_index the quantization bin (states which of the energy maps to access).
128  * \param[in] col_index the column index within the specified energy map.
129  * \param[in] row_index the row index within the specified energy map.
130  */
131  inline unsigned char &
132  operator() (const size_t bin_index, const size_t col_index, const size_t row_index)
133  {
134  return (maps_[bin_index][row_index*width_ + col_index]);
135  }
136 
137  /** \brief Operator for accessing a specific element in the set of energy maps.
138  * \param[in] bin_index the quantization bin (states which of the energy maps to access).
139  * \param[in] index the element index within the specified energy map.
140  */
141  inline unsigned char &
142  operator() (const size_t bin_index, const size_t index)
143  {
144  return (maps_[bin_index][index]);
145  }
146 
147  /** \brief Returns a pointer to the data of the specified energy map.
148  * \param[in] bin_index the index of the energy map to return (== the quantization bin).
149  */
150  inline unsigned char *
151  operator() (const size_t bin_index)
152  {
153  return (maps_[bin_index]);
154  }
155 
156  /** \brief Operator for accessing a specific element in the set of energy maps.
157  * \param[in] bin_index the quantization bin (states which of the energy maps to access).
158  * \param[in] col_index the column index within the specified energy map.
159  * \param[in] row_index the row index within the specified energy map.
160  */
161  inline const unsigned char &
162  operator() (const size_t bin_index, const size_t col_index, const size_t row_index) const
163  {
164  return (maps_[bin_index][row_index*width_ + col_index]);
165  }
166 
167  /** \brief Operator for accessing a specific element in the set of energy maps.
168  * \param[in] bin_index the quantization bin (states which of the energy maps to access).
169  * \param[in] index the element index within the specified energy map.
170  */
171  inline const unsigned char &
172  operator() (const size_t bin_index, const size_t index) const
173  {
174  return (maps_[bin_index][index]);
175  }
176 
177  /** \brief Returns a pointer to the data of the specified energy map.
178  * \param[in] bin_index the index of the energy map to return (== the quantization bin).
179  */
180  inline const unsigned char *
181  operator() (const size_t bin_index) const
182  {
183  return (maps_[bin_index]);
184  }
185 
186  private:
187  /** \brief The width of the energy maps. */
188  size_t width_;
189  /** \brief The height of the energy maps. */
190  size_t height_;
191  /** \brief The number of quantization bins (== the number of internally stored energy maps). */
192  size_t nr_bins_;
193  /** \brief Storage for the energy maps. */
194  std::vector<unsigned char*> maps_;
195  };
196 
197  /** \brief Stores a set of linearized maps.
198  * \author Stefan Holzer
199  */
200  class PCL_EXPORTS LinearizedMaps
201  {
202  public:
203  /** \brief Constructor. */
204  LinearizedMaps () : width_ (0), height_ (0), mem_width_ (0), mem_height_ (0), step_size_ (0), maps_ ()
205  {
206  }
207 
208  /** \brief Destructor. */
209  virtual ~LinearizedMaps ()
210  {
211  }
212 
213  /** \brief Returns the width of the linearized map. */
214  inline size_t
215  getWidth () const { return (width_); }
216 
217  /** \brief Returns the height of the linearized map. */
218  inline size_t
219  getHeight () const { return (height_); }
220 
221  /** \brief Returns the step-size used to construct the linearized map. */
222  inline size_t
223  getStepSize () const { return (step_size_); }
224 
225  /** \brief Returns the size of the memory map. */
226  inline size_t
227  getMapMemorySize () const { return (mem_width_ * mem_height_); }
228 
229  /** \brief Initializes the linearized map.
230  * \param[in] width the width of the source map.
231  * \param[in] height the height of the source map.
232  * \param[in] step_size the step-size used to sample the source map.
233  */
234  void
235  initialize (const size_t width, const size_t height, const size_t step_size)
236  {
237  maps_.resize(step_size*step_size, NULL);
238  width_ = width;
239  height_ = height;
240  mem_width_ = width / step_size;
241  mem_height_ = height / step_size;
242  step_size_ = step_size;
243 
244  const size_t mapsSize = mem_width_ * mem_height_;
245 
246  for (size_t map_index = 0; map_index < maps_.size (); ++map_index)
247  {
248  //maps_[map_index] = new unsigned char[2*mapsSize];
249  maps_[map_index] = reinterpret_cast<unsigned char*> (aligned_malloc (2*mapsSize));
250  memset (maps_[map_index], 0, 2*mapsSize);
251  }
252  }
253 
254  /** \brief Releases the internal memory. */
255  void
256  releaseAll ()
257  {
258  for (size_t map_index = 0; map_index < maps_.size (); ++map_index)
259  //if (maps_[map_index] != NULL) delete[] maps_[map_index];
260  if (maps_[map_index] != NULL) aligned_free (maps_[map_index]);
261 
262  maps_.clear ();
263  width_ = 0;
264  height_ = 0;
265  mem_width_ = 0;
266  mem_height_ = 0;
267  step_size_ = 0;
268  }
269 
270  /** \brief Operator to access elements of the linearized map by column and row index.
271  * \param[in] col_index the column index.
272  * \param[in] row_index the row index.
273  */
274  inline unsigned char *
275  operator() (const size_t col_index, const size_t row_index)
276  {
277  return (maps_[row_index*step_size_ + col_index]);
278  }
279 
280  /** \brief Returns a linearized map starting at the specified position.
281  * \param[in] col_index the column index at which the returned map starts.
282  * \param[in] row_index the row index at which the returned map starts.
283  */
284  inline unsigned char *
285  getOffsetMap (const size_t col_index, const size_t row_index)
286  {
287  const size_t map_col = col_index % step_size_;
288  const size_t map_row = row_index % step_size_;
289 
290  const size_t map_mem_col_index = col_index / step_size_;
291  const size_t map_mem_row_index = row_index / step_size_;
292 
293  return (maps_[map_row*step_size_ + map_col] + map_mem_row_index*mem_width_ + map_mem_col_index);
294  }
295 
296  private:
297  /** \brief the original width of the data represented by the map. */
298  size_t width_;
299  /** \brief the original height of the data represented by the map. */
300  size_t height_;
301  /** \brief the actual width of the linearized map. */
302  size_t mem_width_;
303  /** \brief the actual height of the linearized map. */
304  size_t mem_height_;
305  /** \brief the step-size used for sampling the original data. */
306  size_t step_size_;
307  /** \brief a vector containing all the linearized maps. */
308  std::vector<unsigned char*> maps_;
309  };
310 
311  /** \brief Represents a detection of a template using the LINEMOD approach.
312  * \author Stefan Holzer
313  */
314  struct PCL_EXPORTS LINEMODDetection
315  {
316  /** \brief Constructor. */
317  LINEMODDetection () : x (0), y (0), template_id (0), score (0.0f), scale (1.0f) {}
318 
319  /** \brief x-position of the detection. */
320  int x;
321  /** \brief y-position of the detection. */
322  int y;
323  /** \brief ID of the detected template. */
324  int template_id;
325  /** \brief score of the detection. */
326  float score;
327  /** \brief scale at which the template was detected. */
328  float scale;
329  };
330 
331  /**
332  * \brief Template matching using the LINEMOD approach.
333  * \author Stefan Holzer, Stefan Hinterstoisser
334  */
335  class PCL_EXPORTS LINEMOD
336  {
337  public:
338  /** \brief Constructor */
339  LINEMOD ();
340 
341  /** \brief Destructor */
342  virtual ~LINEMOD ();
343 
344  /** \brief Creates a template from the specified data and adds it to the matching queue.
345  * \param[in] modalities the modalities used to create the template.
346  * \param[in] masks the masks that determine which parts of the modalities are used for creating the template.
347  * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps).
348  */
349  int
350  createAndAddTemplate (const std::vector<QuantizableModality*> & modalities,
351  const std::vector<MaskMap*> & masks,
352  const RegionXY & region);
353 
354  /** \brief Adds the specified template to the matching queue.
355  * \param[in] linemod_template the template to add.
356  */
357  int
358  addTemplate (const SparseQuantizedMultiModTemplate & linemod_template);
359 
360  /** \brief Detects the stored templates in the supplied modality data.
361  * \param[in] modalities the modalities that will be used for detection.
362  * \param[out] detections the destination for the detections.
363  */
364  void
365  detectTemplates (const std::vector<QuantizableModality*> & modalities,
366  std::vector<LINEMODDetection> & detections) const;
367 
368  /** \brief Detects the stored templates in a semi scale invariant manner
369  * by applying the detection to multiple scaled versions of the input data.
370  * \param[in] modalities the modalities that will be used for detection.
371  * \param[out] detections the destination for the detections.
372  * \param[in] min_scale the minimum scale.
373  * \param[in] max_scale the maximum scale.
374  * \param[in] scale_multiplier the multiplier for getting from one scale to the next.
375  */
376  void
377  detectTemplatesSemiScaleInvariant (const std::vector<QuantizableModality*> & modalities,
378  std::vector<LINEMODDetection> & detections,
379  float min_scale = 0.6944444f,
380  float max_scale = 1.44f,
381  float scale_multiplier = 1.2f) const;
382 
383  /** \brief Matches the stored templates to the supplied modality data.
384  * \param[in] modalities the modalities that will be used for matching.
385  * \param[out] matches the found matches.
386  */
387  void
388  matchTemplates (const std::vector<QuantizableModality*> & modalities,
389  std::vector<LINEMODDetection> & matches) const;
390 
391  /** \brief Sets the detection threshold.
392  * \param[in] threshold the detection threshold.
393  */
394  inline void
395  setDetectionThreshold (float threshold)
396  {
397  template_threshold_ = threshold;
398  }
399 
400  /** \brief Enables/disables non-maximum suppression.
401  * \param[in] use_non_max_suppression determines whether to use non-maximum suppression or not.
402  */
403  inline void
404  setNonMaxSuppression (bool use_non_max_suppression)
405  {
406  use_non_max_suppression_ = use_non_max_suppression;
407  }
408 
409  /** \brief Enables/disables averaging of close detections.
410  * \param[in] average_detections determines whether to average close detections or not.
411  */
412  inline void
413  setDetectionAveraging (bool average_detections)
414  {
415  average_detections_ = average_detections;
416  }
417 
418  /** \brief Returns the template with the specified ID.
419  * \param[in] template_id the ID of the template to return.
420  */
421  inline const SparseQuantizedMultiModTemplate &
422  getTemplate (int template_id) const
423  {
424  return (templates_[template_id]);
425  }
426 
427  /** \brief Returns the number of stored/trained templates. */
428  inline size_t
429  getNumOfTemplates () const
430  {
431  return (templates_.size ());
432  }
433 
434  /** \brief Saves the stored templates to the specified file.
435  * \param[in] file_name the name of the file to save the templates to.
436  */
437  void
438  saveTemplates (const char * file_name) const;
439 
440  /** \brief Loads templates from the specified file.
441  * \param[in] file_name the name of the file to load the template from.
442  */
443  void
444  loadTemplates (const char * file_name);
445 
446  /** \brief Serializes the stored templates to the specified stream.
447  * \param[in] stream the stream the templates will be written to.
448  */
449  void
450  serialize (std::ostream & stream) const;
451 
452  /** \brief Deserializes templates from the specified stream.
453  * \param[in] stream the stream the templates will be read from.
454  */
455  void
456  deserialize (std::istream & stream);
457 
458 
459  private:
460  /** template response threshold */
461  float template_threshold_;
462  /** states whether non-max-suppression on detections is enabled or not */
463  bool use_non_max_suppression_;
464  /** states whether to return an averaged detection */
465  bool average_detections_;
466  /** template storage */
467  std::vector<SparseQuantizedMultiModTemplate> templates_;
468  };
469 
470 }
471 
472 #endif