model_library.cpp
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  * $Id$
00037  *
00038  */
00039 
00040 #include <pcl/recognition/ransac_based/model_library.h>
00041 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
00042 #include <pcl/kdtree/kdtree_flann.h>
00043 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00044 #include <pcl/console/print.h>
00045 #include <cmath>
00046 
00047 using namespace std;
00048 using namespace pcl;
00049 using namespace console;
00050 using namespace pcl::recognition;
00051 
00052 //============================================================================================================================================
00053 
00054 ModelLibrary::ModelLibrary (float pair_width, float voxel_size, float max_coplanarity_angle)
00055 : pair_width_ (pair_width),
00056   voxel_size_ (voxel_size),
00057   max_coplanarity_angle_ (max_coplanarity_angle),
00058   ignore_coplanar_opps_ (true)
00059 {
00060   num_of_cells_[0] = 60;
00061   num_of_cells_[1] = 60;
00062   num_of_cells_[2] = 60;
00063 
00064   // Compute the bounds of the hash table
00065   float eps = 0.000001f; // To be sure that an angle of 0 or PI will not be excluded because it lies on the boundary of the voxel structure
00066   float bounds[6] = {-eps, static_cast<float> (M_PI) + eps,
00067                      -eps, static_cast<float> (M_PI) + eps,
00068                      -eps, static_cast<float> (M_PI) + eps};
00069  
00070   hash_table_.build (bounds, num_of_cells_);
00071 }
00072 
00073 //============================================================================================================================================
00074 
00075 void
00076 ModelLibrary::clear ()
00077 {
00078   this->removeAllModels ();
00079 
00080   num_of_cells_[0] = num_of_cells_[1] = num_of_cells_[2] = 0;
00081   hash_table_.clear ();
00082 }
00083 
00084 //============================================================================================================================================
00085 
00086 void
00087 ModelLibrary::removeAllModels ()
00088 {
00089   // Delete the model entries
00090   for ( map<string,Model*>::iterator it = models_.begin() ; it != models_.end() ; ++it )
00091     delete it->second;
00092   models_.clear();
00093 
00094   // Clear the hash table
00095   HashTableCell* cells = hash_table_.getVoxels();
00096   int num_bins = num_of_cells_[0]*num_of_cells_[1]*num_of_cells_[2];
00097 
00098   // Clear each cell
00099   for ( int i = 0 ; i < num_bins ; ++i )
00100     cells[i].clear();
00101 }
00102 
00103 //============================================================================================================================================
00104 
00105 bool
00106 ModelLibrary::addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name,
00107                         float frac_of_points_for_registration, void* user_data)
00108 {
00109 #ifdef OBJ_REC_RANSAC_VERBOSE
00110   printf("ModelLibrary::%s(): begin [%s]\n", __func__, object_name.c_str ());
00111 #endif
00112 
00113   // Try to insert a new model entry
00114   pair<map<string,Model*>::iterator, bool> result = models_.insert (pair<string,Model*> (object_name, static_cast<Model*> (NULL)));
00115 
00116   // Check if 'object_name' is unique
00117   if (!result.second)
00118   {
00119     print_error ("'%s' already exists in the model library.\n", object_name.c_str ());
00120     return (false);
00121   }
00122 
00123   // It is unique -> create a new library model and save it
00124   Model* new_model = new Model (points, normals, voxel_size_, object_name, frac_of_points_for_registration, user_data);
00125   result.first->second = new_model;
00126 
00127   const ORROctree& octree = new_model->getOctree ();
00128   const vector<ORROctree::Node*> &full_leaves = octree.getFullLeaves ();
00129   list<ORROctree::Node*> inter_leaves;
00130   int num_of_pairs = 0;
00131 
00132   // Run through all full leaves
00133   for ( vector<ORROctree::Node*>::const_iterator leaf1 = full_leaves.begin () ; leaf1 != full_leaves.end () ; ++leaf1 )
00134   {
00135     const ORROctree::Node::Data* node_data1 = (*leaf1)->getData ();
00136 
00137     // Get all full leaves at the right distance to the current leaf
00138     inter_leaves.clear ();
00139     octree.getFullLeavesIntersectedBySphere (node_data1->getPoint (), pair_width_, inter_leaves);
00140 
00141     for ( list<ORROctree::Node*>::iterator leaf2 = inter_leaves.begin () ; leaf2 != inter_leaves.end () ; ++leaf2 )
00142     {
00143       // Compute the hash table key
00144       if ( this->addToHashTable(new_model, node_data1, (*leaf2)->getData ()) )
00145         ++num_of_pairs;
00146     }
00147   }
00148 
00149 #ifdef OBJ_REC_RANSAC_VERBOSE
00150   printf("ModelLibrary::%s(): end [%i oriented point pairs]\n", __func__, num_of_pairs);
00151 #endif
00152 
00153   return (true);
00154 }
00155 
00156 //============================================================================================================================================
00157 
00158 bool
00159 ModelLibrary::addToHashTable (Model* model, const ORROctree::Node::Data* data1, const ORROctree::Node::Data* data2)
00160 {
00161   float key[3];
00162 
00163   // Compute the descriptor signature for the oriented point pair (i, j)
00164   ObjRecRANSAC::compute_oriented_point_pair_signature (
00165     data1->getPoint (), data1->getNormal (),
00166     data2->getPoint (), data2->getNormal (), key);
00167 
00168   if ( ignore_coplanar_opps_ )
00169   {
00170     // If the angle between one of the normals and the connecting vector is about 90° and
00171     // the angle between both normals is about 0° then the points are co-planar ignore them!
00172     if ( std::fabs (key[0]-AUX_HALF_PI) < max_coplanarity_angle_ && key[2] < max_coplanarity_angle_ )
00173       return (false);
00174   }
00175 
00176   // Get the hash table cell containing 'key' (there is for sure such a cell since the hash table bounds are large enough)
00177   HashTableCell* cell = hash_table_.getVoxel (key);
00178 
00179   // Insert the pair (data1,data2) belonging to 'model'
00180   (*cell)[model].push_back (std::pair<const ORROctree::Node::Data*, const ORROctree::Node::Data*> (data1, data2));
00181 
00182   return (true);
00183 }
00184 
00185 //============================================================================================================================================


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:40