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);