Point Cloud Library (PCL) 1.12.0
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
54namespace pcl
55{
56 namespace recognition
57 {
59 {
60 public:
61 class Entry
62 {
63 public:
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)
112 // Save the translation
113 aux::copy3 (translation_, rigid_transform + 9);
114 }
115 }
116
117 inline const float*
119 {
120 return (axis_angle_);
121 }
122
123 inline const float*
125 {
126 return (translation_);
127 }
128
129 inline int
131 {
132 return (num_transforms_);
133 }
134
135 protected:
138 };// class Entry
139
140 public:
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 std::map<const ModelLibrary::Model*, Entry>::const_iterator 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:
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
301
303 {
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
318
319 const std::list<RotationSpace*>&
321
322 std::list<RotationSpace*>&
324
325 void
327 {
328 counter_ = 0;
329 rotation_spaces_.clear ();
330 }
331
332 protected:
335 std::list<RotationSpace*> rotation_spaces_;
336 };
337
339
341 {
342 public:
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
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
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)
std::list< RotationSpace * > & getRotationSpaces()
const std::list< RotationSpace * > & getRotationSpaces() const
const float * getAxisAngle() const
float translation_[3]
const Entry & operator=(const Entry &src)
int getNumberOfTransforms() const
const float * getTranslation() const
const Entry & addRigidTransform(const float axis_angle[3], const float translation[3])
int num_transforms_
Entry()
void computeAverageRigidTransform(float *rigid_transform=nullptr)
float axis_angle_[3]
Entry(const Entry &src)
RotationSpaceCell * create(const SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float >::Node *)
const RotationSpaceCell::Entry & addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
const RotationSpaceCell::Entry * getEntry(const ModelLibrary::Model *model) const
std::map< const ModelLibrary::Model *, Entry > model_to_entry_
std::map< const ModelLibrary::Model *, Entry > & getEntries()
RotationSpace * create(const SimpleOctree< RotationSpace, RotationSpaceCreator, float >::Node *leaf)
std::list< RotationSpace * > rotation_spaces_
const std::list< RotationSpace * > & getRotationSpaces() const
std::list< RotationSpace * > & getRotationSpaces()
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