Point Cloud Library (PCL)
1.7.0
|
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 00189 /** \brief This is a class for a discrete representation of the rotation space based on the axis-angle representation. 00190 * This class is not supposed to be very general. That's why it is dependent on the class ModelLibrary. 00191 * 00192 * \author Chavdar Papazov 00193 * \ingroup recognition 00194 */ 00195 class PCL_EXPORTS RotationSpace 00196 { 00197 public: 00198 /** \brief We use the axis-angle representation for rotations. The axis is encoded in the vector 00199 * and the angle is its magnitude. This is represented in an octree with bounds [-pi, pi]^3. */ 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_ */