48 #include "simple_octree.h"
49 #include "model_library.h"
50 #include <pcl/pcl_exports.h>
108 if ( rigid_transform )
147 inline std::map<const ModelLibrary::Model*,Entry>&
159 return (&res->second);
167 return model_to_entry_[model].addRigidTransform (axis_angle, translation);
201 float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f;
202 float bounds[6] = {min, max, min, max, min, max};
205 octree_.build (bounds, discretization, &cell_creator_);
228 const std::vector<CellOctree::Node*>& full_leaves = octree_.getFullLeaves ();
229 int max_num_transforms = 0;
232 for (
const auto &full_leaf : full_leaves)
240 const std::set<CellOctree::Node*>& neighs = full_leaf->getNeighbors ();
243 for (
const auto &neigh : neighs)
252 if ( num_transforms > max_num_transforms )
254 with_most_votes = *entry;
255 max_num_transforms = num_transforms;
259 if ( !max_num_transforms )
270 CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]);
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]);
282 cell->getData ().addRigidTransform (model, axis_angle, translation);
305 rot_space->setCenter (leaf->getCenter ());
319 const std::list<RotationSpace*>&
322 std::list<RotationSpace*>&
347 build (
const float* pos_bounds,
float translation_cell_size,
float rotation_cell_size)
351 rotation_space_creator_.setDiscretization (rotation_cell_size);
353 pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_);
359 pos_octree_.clear ();
360 rotation_space_creator_.reset ();
363 inline std::list<RotationSpace*>&
366 return (rotation_space_creator_.getRotationSpaces ());
369 inline const std::list<RotationSpace*>&
372 return (rotation_space_creator_.getRotationSpaces ());
378 return (rotation_space_creator_.getNumberOfRotationSpaces ());
385 RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]);
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]);
394 float rot_angle, axis_angle[3];
401 leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9);
Stores some information about the model.
const Entry & addRigidTransform(const float axis_angle[3], const float translation[3])
int getNumberOfTransforms() const
const float * getTranslation() const
const float * getAxisAngle() const
void computeAverageRigidTransform(float *rigid_transform=nullptr)
const Entry & operator=(const Entry &src)
virtual ~RotationSpaceCellCreator()=default
RotationSpaceCellCreator()=default
RotationSpaceCell * create(const SimpleOctree< RotationSpaceCell, RotationSpaceCellCreator, float >::Node *)
const RotationSpaceCell::Entry * getEntry(const ModelLibrary::Model *model) const
std::map< const ModelLibrary::Model *, Entry > model_to_entry_
virtual ~RotationSpaceCell()
RotationSpaceCell()=default
const RotationSpaceCell::Entry & addRigidTransform(const ModelLibrary::Model *model, const float axis_angle[3], const float translation[3])
std::map< const ModelLibrary::Model *, Entry > & getEntries()
std::list< RotationSpace * > & getRotationSpaces()
RotationSpace * create(const SimpleOctree< RotationSpace, RotationSpaceCreator, float >::Node *leaf)
std::list< RotationSpace * > rotation_spaces_
void setDiscretization(float value)
int getNumberOfRotationSpaces() const
virtual ~RotationSpaceCreator()=default
const std::list< RotationSpace * > & getRotationSpaces() const
This is a class for a discrete representation of the rotation space based on the axis-angle represent...
void setCenter(const float *c)
const float * getCenter() const
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'.
void mult3(T *v, T scalar)
v = scalar*v.
void add3(T a[3], const T b[3])
a += b
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
void copy3(const T src[3], T dst[3])
dst = src
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'...