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