Point Cloud Library (PCL)  1.7.1
rigid_transform_space.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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 
39 /*
40  * rigid_transform_space.h
41  *
42  * Created on: Feb 15, 2013
43  * Author: papazov
44  */
45 
46 #ifndef PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
47 #define PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
48 
49 #include "simple_octree.h"
50 #include "model_library.h"
51 #include <pcl/pcl_exports.h>
52 #include <list>
53 #include <map>
54 
55 namespace pcl
56 {
57  namespace recognition
58  {
60  {
61  public:
62  class Entry
63  {
64  public:
65  Entry ()
66  : num_transforms_ (0)
67  {
68  aux::set3 (axis_angle_, 0.0f);
69  aux::set3 (translation_, 0.0f);
70  }
71 
72  Entry (const Entry& src)
74  {
75  aux::copy3 (src.axis_angle_, this->axis_angle_);
76  aux::copy3 (src.translation_, this->translation_);
77  }
78 
79  const Entry& operator = (const Entry& src)
80  {
82  aux::copy3 (src.axis_angle_, this->axis_angle_);
83  aux::copy3 (src.translation_, this->translation_);
84 
85  return *this;
86  }
87 
88  inline const Entry&
89  addRigidTransform (const float axis_angle[3], const float translation[3])
90  {
91  aux::add3 (this->axis_angle_, axis_angle);
92  aux::add3 (this->translation_, translation);
94 
95  return *this;
96  }
97 
98  inline void
99  computeAverageRigidTransform (float *rigid_transform = NULL)
100  {
101  if ( num_transforms_ >= 2 )
102  {
103  float factor = 1.0f/static_cast<float> (num_transforms_);
104  aux::mult3 (axis_angle_, factor);
105  aux::mult3 (translation_, factor);
106  num_transforms_ = 1;
107  }
108 
109  if ( rigid_transform )
110  {
111  // Save the rotation (in matrix form)
112  aux::axisAngleToRotationMatrix (axis_angle_, rigid_transform);
113  // Save the translation
114  aux::copy3 (translation_, rigid_transform + 9);
115  }
116  }
117 
118  inline const float*
119  getAxisAngle () const
120  {
121  return (axis_angle_);
122  }
123 
124  inline const float*
125  getTranslation () const
126  {
127  return (translation_);
128  }
129 
130  inline int
132  {
133  return (num_transforms_);
134  }
135 
136  protected:
137  float axis_angle_[3], translation_[3];
139  };// class Entry
140 
141  public:
144  {
145  model_to_entry_.clear ();
146  }
147 
148  inline std::map<const ModelLibrary::Model*,Entry>&
150  {
151  return (model_to_entry_);
152  }
153 
154  inline const RotationSpaceCell::Entry*
155  getEntry (const ModelLibrary::Model* model) const
156  {
157  std::map<const ModelLibrary::Model*, Entry>::const_iterator res = model_to_entry_.find (model);
158 
159  if ( res != model_to_entry_.end () )
160  return (&res->second);
161 
162  return (NULL);
163  }
164 
165  inline const RotationSpaceCell::Entry&
166  addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3])
167  {
168  return model_to_entry_[model].addRigidTransform (axis_angle, translation);
169  }
170 
171  protected:
172  std::map<const ModelLibrary::Model*,Entry> model_to_entry_;
173  }; // class RotationSpaceCell
174 
176  {
177  public:
180 
182  {
183  return (new RotationSpaceCell ());
184  }
185  };
186 
188 
189  /** \brief This is a class for a discrete representation of the rotation space based on the axis-angle representation.
190  * This class is not supposed to be very general. That's why it is dependent on the class ModelLibrary.
191  *
192  * \author Chavdar Papazov
193  * \ingroup recognition
194  */
195  class PCL_EXPORTS RotationSpace
196  {
197  public:
198  /** \brief We use the axis-angle representation for rotations. The axis is encoded in the vector
199  * and the angle is its magnitude. This is represented in an octree with bounds [-pi, pi]^3. */
200  RotationSpace (float discretization)
201  {
202  float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f;
203  float bounds[6] = {min, max, min, max, min, max};
204 
205  // Build the voxel structure
206  octree_.build (bounds, discretization, &cell_creator_);
207  }
208 
209  virtual ~RotationSpace ()
210  {
211  octree_.clear ();
212  }
213 
214  inline void
215  setCenter (const float* c)
216  {
217  center_[0] = c[0];
218  center_[1] = c[1];
219  center_[2] = c[2];
220  }
221 
222  inline const float*
223  getCenter () const { return center_;}
224 
225  inline bool
226  getTransformWithMostVotes (const ModelLibrary::Model* model, float rigid_transform[12]) const
227  {
228  RotationSpaceCell::Entry with_most_votes;
229  const std::vector<CellOctree::Node*>& full_leaves = octree_.getFullLeaves ();
230  int max_num_transforms = 0;
231 
232  // For each full leaf
233  for ( std::vector<CellOctree::Node*>::const_iterator leaf = full_leaves.begin () ; leaf != full_leaves.end () ; ++leaf )
234  {
235  // Is there an entry for 'model' in the current cell
236  const RotationSpaceCell::Entry *entry = (*leaf)->getData ().getEntry (model);
237  if ( !entry )
238  continue;
239 
240  int num_transforms = entry->getNumberOfTransforms ();
241  const std::set<CellOctree::Node*>& neighs = (*leaf)->getNeighbors ();
242 
243  // For each neighbor
244  for ( std::set<CellOctree::Node*>::const_iterator neigh = neighs.begin () ; neigh != neighs.end () ; ++neigh )
245  {
246  const RotationSpaceCell::Entry *neigh_entry = (*neigh)->getData ().getEntry (model);
247  if ( !neigh_entry )
248  continue;
249 
250  num_transforms += neigh_entry->getNumberOfTransforms ();
251  }
252 
253  if ( num_transforms > max_num_transforms )
254  {
255  with_most_votes = *entry;
256  max_num_transforms = num_transforms;
257  }
258  }
259 
260  if ( !max_num_transforms )
261  return false;
262 
263  with_most_votes.computeAverageRigidTransform (rigid_transform);
264 
265  return true;
266  }
267 
268  inline bool
269  addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3])
270  {
271  CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]);
272 
273  if ( !cell )
274  {
275  const float *b = octree_.getBounds ();
276  printf ("WARNING in 'RotationSpace::%s()': the provided axis-angle input (%f, %f, %f) is "
277  "out of the rotation space bounds ([%f, %f], [%f, %f], [%f, %f]).\n",
278  __func__, axis_angle[0], axis_angle[1], axis_angle[2], b[0], b[1], b[2], b[3], b[4], b[5]);
279  return (false);
280  }
281 
282  // Add the rigid transform to the cell
283  cell->getData ().addRigidTransform (model, axis_angle, translation);
284 
285  return (true);
286  }
287 
288  protected:
291  float center_[3];
292  };// class RotationSpace
293 
295  {
296  public:
298  : counter_ (0)
299  {}
300 
302 
304  {
305  RotationSpace *rot_space = new RotationSpace (discretization_);
306  rot_space->setCenter (leaf->getCenter ());
307  rotation_spaces_.push_back (rot_space);
308 
309  ++counter_;
310 
311  return (rot_space);
312  }
313 
314  void
315  setDiscretization (float value){ discretization_ = value;}
316 
317  int
318  getNumberOfRotationSpaces () const { return (counter_);}
319 
320  const std::list<RotationSpace*>&
321  getRotationSpaces () const { return (rotation_spaces_);}
322 
323  std::list<RotationSpace*>&
325 
326  void
327  reset ()
328  {
329  counter_ = 0;
330  rotation_spaces_.clear ();
331  }
332 
333  protected:
335  int counter_;
336  std::list<RotationSpace*> rotation_spaces_;
337  };
338 
340 
341  class PCL_EXPORTS RigidTransformSpace
342  {
343  public:
345  virtual ~RigidTransformSpace (){ this->clear ();}
346 
347  inline void
348  build (const float* pos_bounds, float translation_cell_size, float rotation_cell_size)
349  {
350  this->clear ();
351 
352  rotation_space_creator_.setDiscretization (rotation_cell_size);
353 
354  pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_);
355  }
356 
357  inline void
358  clear ()
359  {
360  pos_octree_.clear ();
361  rotation_space_creator_.reset ();
362  }
363 
364  inline std::list<RotationSpace*>&
366  {
367  return (rotation_space_creator_.getRotationSpaces ());
368  }
369 
370  inline const std::list<RotationSpace*>&
372  {
373  return (rotation_space_creator_.getRotationSpaces ());
374  }
375 
376  inline int
378  {
379  return (rotation_space_creator_.getNumberOfRotationSpaces ());
380  }
381 
382  inline bool
383  addRigidTransform (const ModelLibrary::Model* model, const float position[3], const float rigid_transform[12])
384  {
385  // Get the leaf 'position' ends up in
386  RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]);
387 
388  if ( !leaf )
389  {
390  printf ("WARNING in 'RigidTransformSpace::%s()': the input position (%f, %f, %f) is out of bounds.\n",
391  __func__, position[0], position[1], position[2]);
392  return (false);
393  }
394 
395  float rot_angle, axis_angle[3];
396  // Extract the axis-angle representation from the rotation matrix
397  aux::rotationMatrixToAxisAngle (rigid_transform, axis_angle, rot_angle);
398  // Multiply the axis by the angle to get the final representation
399  aux::mult3 (axis_angle, rot_angle);
400 
401  // Now, add the rigid transform to the rotation space
402  leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9);
403 
404  return (true);
405  }
406 
407  protected:
410  }; // class RigidTransformSpace
411  } // namespace recognition
412 } // namespace pcl
413 
414 #endif /* PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_ */