46 #ifndef PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
47 #define PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
49 #include "simple_octree.h"
50 #include "model_library.h"
51 #include <pcl/pcl_exports.h>
109 if ( rigid_transform )
148 inline std::map<const ModelLibrary::Model*,Entry>&
157 std::map<const ModelLibrary::Model*, Entry>::const_iterator res =
model_to_entry_.find (model);
160 return (&res->second);
168 return model_to_entry_[model].addRigidTransform (axis_angle, translation);
202 float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f;
203 float bounds[6] = {min, max, min, max, min, max};
206 octree_.build (bounds, discretization, &cell_creator_);
229 const std::vector<CellOctree::Node*>& full_leaves = octree_.getFullLeaves ();
230 int max_num_transforms = 0;
233 for ( std::vector<CellOctree::Node*>::const_iterator leaf = full_leaves.begin () ; leaf != full_leaves.end () ; ++leaf )
241 const std::set<CellOctree::Node*>& neighs = (*leaf)->getNeighbors ();
244 for ( std::set<CellOctree::Node*>::const_iterator neigh = neighs.begin () ; neigh != neighs.end () ; ++neigh )
253 if ( num_transforms > max_num_transforms )
255 with_most_votes = *entry;
256 max_num_transforms = num_transforms;
260 if ( !max_num_transforms )
271 CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]);
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]);
283 cell->getData ().addRigidTransform (model, axis_angle, translation);
306 rot_space->
setCenter (leaf->getCenter ());
320 const std::list<RotationSpace*>&
323 std::list<RotationSpace*>&
348 build (
const float* pos_bounds,
float translation_cell_size,
float rotation_cell_size)
352 rotation_space_creator_.setDiscretization (rotation_cell_size);
354 pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_);
360 pos_octree_.clear ();
361 rotation_space_creator_.reset ();
364 inline std::list<RotationSpace*>&
367 return (rotation_space_creator_.getRotationSpaces ());
370 inline const std::list<RotationSpace*>&
373 return (rotation_space_creator_.getRotationSpaces ());
379 return (rotation_space_creator_.getNumberOfRotationSpaces ());
386 RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]);
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]);
395 float rot_angle, axis_angle[3];
402 leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9);
void setDiscretization(float value)
void setCenter(const float *c)
virtual ~RotationSpaceCellCreator()
void mult3(T *v, T scalar)
v = scalar*v.
bool getTransformWithMostVotes(const ModelLibrary::Model *model, float rigid_transform[12]) const
Stores some information about the model.
int getNumberOfRotationSpaces() const
This is a class for a discrete representation of the rotation space based on the axis-angle represent...
void computeAverageRigidTransform(float *rigid_transform=NULL)
const float * getTranslation() const
const Entry & addRigidTransform(const float axis_angle[3], const float translation[3])
RotationSpaceCellCreator cell_creator_
bool addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
const float * getCenter() const
virtual ~RotationSpaceCell()
std::list< RotationSpace * > & getRotationSpaces()
int getNumberOfTransforms() const
const float * getAxisAngle() const
const std::list< RotationSpace * > & getRotationSpaces() const
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'...
std::list< RotationSpace * > rotation_spaces_
void axisAngleToRotationMatrix(const T axis_angle[3], T rotation_matrix[9])
brief Computes a rotation matrix from the provided input vector 'axis_angle'.
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
const RotationSpaceCell::Entry & addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
std::map< const ModelLibrary::Model *, Entry > & getEntries()
void copy3(const T src[3], T dst[3])
dst = src
SimpleOctree< RotationSpace, RotationSpaceCreator, float > RotationSpaceOctree
const Entry & operator=(const Entry &src)
SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float > CellOctree
void add3(T a[3], const T b[3])
a += b
RotationSpaceCellCreator()
const RotationSpaceCell::Entry * getEntry(const ModelLibrary::Model *model) const
std::map< const ModelLibrary::Model *, Entry > model_to_entry_
RotationSpace(float discretization)
We use the axis-angle representation for rotations.
RotationSpace * create(const SimpleOctree< RotationSpace, RotationSpaceCreator, float >::Node *leaf)
virtual ~RotationSpaceCreator()
RotationSpaceCell * create(const SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float >::Node *)