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