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