Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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 {
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)
111 // Save the translation
112 aux::copy3 (translation_, rigid_transform + 9);
113 }
114 }
115
116 inline const float*
118 {
119 return (axis_angle_);
120 }
121
122 inline const float*
124 {
125 return (translation_);
126 }
127
128 inline int
130 {
131 return (num_transforms_);
132 }
133
134 protected:
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
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 */
193 class PCL_EXPORTS RotationSpace
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:
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
315
316 const std::list<RotationSpace*>&
318
319 std::list<RotationSpace*>&
321
322 void
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
337 class PCL_EXPORTS RigidTransformSpace
338 {
339 public:
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
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.
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