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 copy3(const T src[3], T dst[3])
dst = src
RotationSpaceCellCreator()
const RotationSpaceCell::Entry & addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
const float * getAxisAngle() const
void setCenter(const float *c)
virtual ~RotationSpaceCreator()
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
RotationSpace * create(const SimpleOctree< RotationSpace, RotationSpaceCreator, float >::Node *leaf)
This is a class for a discrete representation of the rotation space based on the axis-angle represent...
virtual ~RigidTransformSpace()
int getNumberOfRotationSpaces() const
std::map< const ModelLibrary::Model *, Entry > & getEntries()
int getNumberOfTransforms() const
SimpleOctree< RotationSpace, RotationSpaceCreator, float > RotationSpaceOctree
const Entry & operator=(const Entry &src)
RotationSpaceCellCreator cell_creator_
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'...
void axisAngleToRotationMatrix(const T axis_angle[3], T rotation_matrix[9])
brief Computes a rotation matrix from the provided input vector 'axis_angle'.
Stores some information about the model.
RotationSpaceCreator rotation_space_creator_
std::list< RotationSpace * > & getRotationSpaces()
RotationSpaceOctree pos_octree_
std::list< RotationSpace * > rotation_spaces_
bool getTransformWithMostVotes(const ModelLibrary::Model *model, float rigid_transform[12]) const
std::map< const ModelLibrary::Model *, Entry > model_to_entry_
bool addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
const RotationSpaceCell::Entry * getEntry(const ModelLibrary::Model *model) const
void mult3(T *v, T scalar)
v = scalar*v.
void setDiscretization(float value)
void add3(T a[3], const T b[3])
a += b
RotationSpaceCell * create(const SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float >::Node *)
const std::list< RotationSpace * > & getRotationSpaces() const
RotationSpace(float discretization)
We use the axis-angle representation for rotations.
virtual ~RotationSpaceCell()
virtual ~RotationSpaceCellCreator()
const Entry & addRigidTransform(const float axis_angle[3], const float translation[3])
bool addRigidTransform(const ModelLibrary::Model *model, const float position[3], const float rigid_transform[12])
const std::list< RotationSpace * > & getRotationSpaces() const
SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float > CellOctree
void computeAverageRigidTransform(float *rigid_transform=NULL)
std::list< RotationSpace * > & getRotationSpaces()
const float * getCenter() const
void build(const float *pos_bounds, float translation_cell_size, float rotation_cell_size)
const float * getTranslation() const
int getNumberOfOccupiedRotationSpaces()