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