rigid_transform_space.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *
00037  */
00038 
00039 /*
00040  * rigid_transform_space.h
00041  *
00042  *  Created on: Feb 15, 2013
00043  *      Author: papazov
00044  */
00045 
00046 #ifndef PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
00047 #define PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_
00048 
00049 #include "simple_octree.h"
00050 #include "model_library.h"
00051 #include <pcl/pcl_exports.h>
00052 #include <list>
00053 #include <map>
00054 
00055 namespace pcl
00056 {
00057   namespace recognition
00058   {
00059     class RotationSpaceCell
00060     {
00061       public:
00062         class Entry
00063         {
00064           public:
00065             Entry ()
00066             : num_transforms_ (0)
00067             {
00068               aux::set3 (axis_angle_, 0.0f);
00069               aux::set3 (translation_, 0.0f);
00070             }
00071 
00072             Entry (const Entry& src)
00073             : num_transforms_ (src.num_transforms_)
00074             {
00075               aux::copy3 (src.axis_angle_, this->axis_angle_);
00076               aux::copy3 (src.translation_, this->translation_);
00077             }
00078 
00079             const Entry& operator = (const Entry& src)
00080             {
00081               num_transforms_ = src.num_transforms_;
00082               aux::copy3 (src.axis_angle_, this->axis_angle_);
00083               aux::copy3 (src.translation_, this->translation_);
00084 
00085               return *this;
00086             }
00087 
00088             inline const Entry&
00089             addRigidTransform (const float axis_angle[3], const float translation[3])
00090             {
00091               aux::add3 (this->axis_angle_, axis_angle);
00092               aux::add3 (this->translation_, translation);
00093               ++num_transforms_;
00094 
00095               return *this;
00096             }
00097 
00098             inline void
00099             computeAverageRigidTransform (float *rigid_transform = NULL)
00100             {
00101               if ( num_transforms_ >= 2 )
00102               {
00103                 float factor = 1.0f/static_cast<float> (num_transforms_);
00104                 aux::mult3 (axis_angle_, factor);
00105                 aux::mult3 (translation_, factor);
00106                 num_transforms_ = 1;
00107               }
00108 
00109               if ( rigid_transform )
00110               {
00111                 // Save the rotation (in matrix form)
00112                 aux::axisAngleToRotationMatrix (axis_angle_, rigid_transform);
00113                 // Save the translation
00114                 aux::copy3 (translation_, rigid_transform + 9);
00115               }
00116             }
00117 
00118             inline const float*
00119             getAxisAngle () const
00120             {
00121               return (axis_angle_);
00122             }
00123 
00124             inline const float*
00125             getTranslation () const
00126             {
00127               return (translation_);
00128             }
00129 
00130             inline int
00131             getNumberOfTransforms () const
00132             {
00133               return (num_transforms_);
00134             }
00135 
00136           protected:
00137             float axis_angle_[3], translation_[3];
00138             int num_transforms_;
00139         };// class Entry
00140 
00141       public:
00142         RotationSpaceCell (){}
00143         virtual ~RotationSpaceCell ()
00144         {
00145           model_to_entry_.clear ();
00146         }
00147 
00148         inline std::map<const ModelLibrary::Model*,Entry>&
00149         getEntries ()
00150         {
00151           return (model_to_entry_);
00152         }
00153 
00154         inline const RotationSpaceCell::Entry*
00155         getEntry (const ModelLibrary::Model* model) const
00156         {
00157           std::map<const ModelLibrary::Model*, Entry>::const_iterator res = model_to_entry_.find (model);
00158 
00159           if ( res != model_to_entry_.end () )
00160             return (&res->second);
00161 
00162           return (NULL);
00163         }
00164 
00165         inline const RotationSpaceCell::Entry&
00166         addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3])
00167         {
00168           return model_to_entry_[model].addRigidTransform (axis_angle, translation);
00169         }
00170 
00171       protected:
00172         std::map<const ModelLibrary::Model*,Entry> model_to_entry_;
00173     }; // class RotationSpaceCell
00174 
00175     class RotationSpaceCellCreator
00176     {
00177       public:
00178         RotationSpaceCellCreator (){}
00179         virtual ~RotationSpaceCellCreator (){}
00180 
00181         RotationSpaceCell* create (const SimpleOctree<RotationSpaceCell, RotationSpaceCellCreator, float>::Node* )
00182         {
00183           return (new RotationSpaceCell ());
00184         }
00185     };
00186 
00187     typedef SimpleOctree<RotationSpaceCell, RotationSpaceCellCreator, float> CellOctree;
00188 
00195     class PCL_EXPORTS RotationSpace
00196     {
00197       public:
00200         RotationSpace (float discretization)
00201         {
00202           float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f;
00203           float bounds[6] = {min, max, min, max, min, max};
00204 
00205           // Build the voxel structure
00206           octree_.build (bounds, discretization, &cell_creator_);
00207         }
00208 
00209         virtual ~RotationSpace ()
00210         {
00211           octree_.clear ();
00212         }
00213 
00214         inline void
00215         setCenter (const float* c)
00216         {
00217           center_[0] = c[0];
00218           center_[1] = c[1];
00219           center_[2] = c[2];
00220         }
00221 
00222         inline const float*
00223         getCenter () const { return center_;}
00224 
00225         inline bool
00226         getTransformWithMostVotes (const ModelLibrary::Model* model, float rigid_transform[12]) const
00227         {
00228           RotationSpaceCell::Entry with_most_votes;
00229           const std::vector<CellOctree::Node*>& full_leaves = octree_.getFullLeaves ();
00230           int max_num_transforms = 0;
00231 
00232           // For each full leaf
00233           for ( std::vector<CellOctree::Node*>::const_iterator leaf = full_leaves.begin () ; leaf != full_leaves.end () ; ++leaf )
00234           {
00235             // Is there an entry for 'model' in the current cell
00236             const RotationSpaceCell::Entry *entry = (*leaf)->getData ().getEntry (model);
00237             if ( !entry )
00238               continue;
00239 
00240             int num_transforms = entry->getNumberOfTransforms ();
00241             const std::set<CellOctree::Node*>& neighs = (*leaf)->getNeighbors ();
00242 
00243             // For each neighbor
00244             for ( std::set<CellOctree::Node*>::const_iterator neigh = neighs.begin () ; neigh != neighs.end () ; ++neigh )
00245             {
00246               const RotationSpaceCell::Entry *neigh_entry = (*neigh)->getData ().getEntry (model);
00247               if ( !neigh_entry )
00248                 continue;
00249 
00250               num_transforms += neigh_entry->getNumberOfTransforms ();
00251             }
00252 
00253             if ( num_transforms > max_num_transforms )
00254             {
00255               with_most_votes = *entry;
00256               max_num_transforms = num_transforms;
00257             }
00258           }
00259 
00260           if ( !max_num_transforms )
00261             return false;
00262 
00263           with_most_votes.computeAverageRigidTransform (rigid_transform);
00264 
00265           return true;
00266         }
00267 
00268         inline bool
00269         addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3])
00270         {
00271           CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]);
00272 
00273           if ( !cell )
00274           {
00275             const float *b = octree_.getBounds ();
00276             printf ("WARNING in 'RotationSpace::%s()': the provided axis-angle input (%f, %f, %f) is "
00277                     "out of the rotation space bounds ([%f, %f], [%f, %f], [%f, %f]).\n",
00278                     __func__, axis_angle[0], axis_angle[1], axis_angle[2], b[0], b[1], b[2], b[3], b[4], b[5]);
00279             return (false);
00280           }
00281 
00282           // Add the rigid transform to the cell
00283           cell->getData ().addRigidTransform (model, axis_angle, translation);
00284 
00285           return (true);
00286         }
00287 
00288       protected:
00289         CellOctree octree_;
00290         RotationSpaceCellCreator cell_creator_;
00291         float center_[3];
00292     };// class RotationSpace
00293 
00294     class RotationSpaceCreator
00295     {
00296       public:
00297         RotationSpaceCreator()
00298         : counter_ (0)
00299         {}
00300 
00301         virtual ~RotationSpaceCreator(){}
00302 
00303         RotationSpace* create(const SimpleOctree<RotationSpace, RotationSpaceCreator, float>::Node* leaf)
00304         {
00305           RotationSpace *rot_space = new RotationSpace (discretization_);
00306           rot_space->setCenter (leaf->getCenter ());
00307           rotation_spaces_.push_back (rot_space);
00308 
00309           ++counter_;
00310 
00311           return (rot_space);
00312         }
00313 
00314         void
00315         setDiscretization (float value){ discretization_ = value;}
00316 
00317         int
00318         getNumberOfRotationSpaces () const { return (counter_);}
00319 
00320         const std::list<RotationSpace*>&
00321         getRotationSpaces () const { return (rotation_spaces_);}
00322 
00323         std::list<RotationSpace*>&
00324         getRotationSpaces (){ return (rotation_spaces_);}
00325 
00326         void
00327         reset ()
00328         {
00329           counter_ = 0;
00330           rotation_spaces_.clear ();
00331         }
00332 
00333       protected:
00334         float discretization_;
00335         int counter_;
00336         std::list<RotationSpace*> rotation_spaces_;
00337     };
00338 
00339     typedef SimpleOctree<RotationSpace, RotationSpaceCreator, float> RotationSpaceOctree;
00340 
00341     class PCL_EXPORTS RigidTransformSpace
00342     {
00343       public:
00344         RigidTransformSpace (){}
00345         virtual ~RigidTransformSpace (){ this->clear ();}
00346 
00347         inline void
00348         build (const float* pos_bounds, float translation_cell_size, float rotation_cell_size)
00349         {
00350           this->clear ();
00351 
00352           rotation_space_creator_.setDiscretization (rotation_cell_size);
00353 
00354           pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_);
00355         }
00356 
00357         inline void
00358         clear ()
00359         {
00360           pos_octree_.clear ();
00361           rotation_space_creator_.reset ();
00362         }
00363 
00364         inline std::list<RotationSpace*>&
00365         getRotationSpaces ()
00366         {
00367           return (rotation_space_creator_.getRotationSpaces ());
00368         }
00369 
00370         inline const std::list<RotationSpace*>&
00371         getRotationSpaces () const
00372         {
00373           return (rotation_space_creator_.getRotationSpaces ());
00374         }
00375 
00376         inline int
00377         getNumberOfOccupiedRotationSpaces ()
00378         {
00379           return (rotation_space_creator_.getNumberOfRotationSpaces ());
00380         }
00381 
00382         inline bool
00383         addRigidTransform (const ModelLibrary::Model* model, const float position[3], const float rigid_transform[12])
00384         {
00385           // Get the leaf 'position' ends up in
00386           RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]);
00387 
00388           if ( !leaf )
00389           {
00390             printf ("WARNING in 'RigidTransformSpace::%s()': the input position (%f, %f, %f) is out of bounds.\n",
00391                     __func__, position[0], position[1], position[2]);
00392             return (false);
00393           }
00394 
00395           float rot_angle, axis_angle[3];
00396           // Extract the axis-angle representation from the rotation matrix
00397           aux::rotationMatrixToAxisAngle (rigid_transform, axis_angle, rot_angle);
00398           // Multiply the axis by the angle to get the final representation
00399           aux::mult3 (axis_angle, rot_angle);
00400 
00401           // Now, add the rigid transform to the rotation space
00402           leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9);
00403 
00404           return (true);
00405         }
00406 
00407       protected:
00408         RotationSpaceOctree pos_octree_;
00409         RotationSpaceCreator rotation_space_creator_;
00410     }; // class RigidTransformSpace
00411   } // namespace recognition
00412 } // namespace pcl
00413 
00414 #endif /* PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:10