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 #ifndef PCL_RECOGNITION_OBJ_REC_RANSAC_H_ 00040 #define PCL_RECOGNITION_OBJ_REC_RANSAC_H_ 00041 00042 #include <pcl/recognition/ransac_based/hypothesis.h> 00043 #include <pcl/recognition/ransac_based/model_library.h> 00044 #include <pcl/recognition/ransac_based/rigid_transform_space.h> 00045 #include <pcl/recognition/ransac_based/orr_octree.h> 00046 #include <pcl/recognition/ransac_based/orr_octree_zprojection.h> 00047 #include <pcl/recognition/ransac_based/simple_octree.h> 00048 #include <pcl/recognition/ransac_based/trimmed_icp.h> 00049 #include <pcl/recognition/ransac_based/orr_graph.h> 00050 #include <pcl/recognition/ransac_based/auxiliary.h> 00051 #include <pcl/recognition/ransac_based/bvh.h> 00052 #include <pcl/registration/transformation_estimation_svd.h> 00053 #include <pcl/correspondence.h> 00054 #include <pcl/pcl_exports.h> 00055 #include <pcl/point_cloud.h> 00056 #include <cmath> 00057 #include <string> 00058 #include <vector> 00059 #include <list> 00060 00061 #define OBJ_REC_RANSAC_VERBOSE 00062 00063 namespace pcl 00064 { 00065 namespace recognition 00066 { 00067 /** \brief This is a RANSAC-based 3D object recognition method. Do the following to use it: (i) call addModel() k times with k different models 00068 * representing the objects to be recognized and (ii) call recognize() with the 3D scene in which the objects should be recognized. Recognition means both 00069 * object identification and pose (position + orientation) estimation. Check the method descriptions for more details. 00070 * 00071 * \note If you use this code in any academic work, please cite: 00072 * 00073 * - Chavdar Papazov, Sami Haddadin, Sven Parusel, Kai Krieger and Darius Burschka. 00074 * Rigid 3D geometry matching for grasping of known objects in cluttered scenes. 00075 * The International Journal of Robotics Research 2012. DOI: 10.1177/0278364911436019 00076 * 00077 * - Chavdar Papazov and Darius Burschka. 00078 * An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes. 00079 * In Proceedings of the 10th Asian Conference on Computer Vision (ACCV'10), 00080 * November 2010. 00081 * 00082 * 00083 * \author Chavdar Papazov 00084 * \ingroup recognition 00085 */ 00086 class PCL_EXPORTS ObjRecRANSAC 00087 { 00088 public: 00089 typedef ModelLibrary::PointCloudIn PointCloudIn; 00090 typedef ModelLibrary::PointCloudN PointCloudN; 00091 00092 typedef BVH<Hypothesis*> BVHH; 00093 00094 /** \brief This is an output item of the ObjRecRANSAC::recognize() method. It contains the recognized model, its name (the ones passed to 00095 * ObjRecRANSAC::addModel()), the rigid transform which aligns the model with the input scene and the match confidence which is a number 00096 * in the interval (0, 1] which gives the fraction of the model surface area matched to the scene. E.g., a match confidence of 0.3 means 00097 * that 30% of the object surface area was matched to the scene points. If the scene is represented by a single range image, the match 00098 * confidence can not be greater than 0.5 since the range scanner sees only one side of each object. 00099 */ 00100 class Output 00101 { 00102 public: 00103 Output (const std::string& object_name, const float rigid_transform[12], float match_confidence, void* user_data) : 00104 object_name_ (object_name), 00105 match_confidence_ (match_confidence), 00106 user_data_ (user_data) 00107 { 00108 memcpy(this->rigid_transform_, rigid_transform, 12*sizeof (float)); 00109 } 00110 virtual ~Output (){} 00111 00112 public: 00113 std::string object_name_; 00114 float rigid_transform_[12]; 00115 float match_confidence_; 00116 void* user_data_; 00117 }; 00118 00119 class OrientedPointPair 00120 { 00121 public: 00122 OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2) 00123 : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2) 00124 { 00125 } 00126 00127 virtual ~OrientedPointPair (){} 00128 00129 public: 00130 const float *p1_, *n1_, *p2_, *n2_; 00131 }; 00132 00133 class HypothesisCreator 00134 { 00135 public: 00136 HypothesisCreator (){} 00137 virtual ~HypothesisCreator (){} 00138 00139 Hypothesis* create (const SimpleOctree<Hypothesis, HypothesisCreator, float>::Node* ) const { return new Hypothesis ();} 00140 }; 00141 00142 typedef SimpleOctree<Hypothesis, HypothesisCreator, float> HypothesisOctree; 00143 00144 public: 00145 /** \brief Constructor with some important parameters which can not be changed once an instance of that class is created. 00146 * 00147 * \param[in] pair_width should be roughly half the extent of the visible object part. This means, for each object point p there should be (at least) 00148 * one point q (from the same object) such that ||p - q|| <= pair_width. Tradeoff: smaller values allow for detection in more occluded scenes but lead 00149 * to more imprecise alignment. Bigger values lead to better alignment but require large visible object parts (i.e., less occlusion). 00150 * 00151 * \param[in] voxel_size is the size of the leafs of the octree, i.e., the "size" of the discretization. Tradeoff: High values lead to less 00152 * computation time but ignore object details. Small values allow to better distinguish between objects, but will introduce more holes in the resulting 00153 * "voxel-surface" (especially for a sparsely sampled scene). */ 00154 ObjRecRANSAC (float pair_width, float voxel_size); 00155 virtual ~ObjRecRANSAC () 00156 { 00157 this->clear (); 00158 this->clearTestData (); 00159 } 00160 00161 /** \brief Removes all models from the model library and releases some memory dynamically allocated by this instance. */ 00162 void 00163 inline clear() 00164 { 00165 model_library_.removeAllModels (); 00166 scene_octree_.clear (); 00167 scene_octree_proj_.clear (); 00168 sampled_oriented_point_pairs_.clear (); 00169 transform_space_.clear (); 00170 scene_octree_points_.reset (); 00171 } 00172 00173 /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will 00174 * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if 00175 * "ignore co-planar points" is on. Call this method before calling addModel. This method calls the corresponding 00176 * method of the model library. */ 00177 inline void 00178 setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) 00179 { 00180 max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; 00181 model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees); 00182 } 00183 00184 inline void 00185 setSceneBoundsEnlargementFactor (float value) 00186 { 00187 scene_bounds_enlargement_factor_ = value; 00188 } 00189 00190 /** \brief Default is on. This method calls the corresponding method of the model library. */ 00191 inline void 00192 ignoreCoplanarPointPairsOn () 00193 { 00194 ignore_coplanar_opps_ = true; 00195 model_library_.ignoreCoplanarPointPairsOn (); 00196 } 00197 00198 /** \brief Default is on. This method calls the corresponding method of the model library. */ 00199 inline void 00200 ignoreCoplanarPointPairsOff () 00201 { 00202 ignore_coplanar_opps_ = false; 00203 model_library_.ignoreCoplanarPointPairsOff (); 00204 } 00205 00206 inline void 00207 icpHypothesesRefinementOn () 00208 { 00209 do_icp_hypotheses_refinement_ = true; 00210 } 00211 00212 inline void 00213 icpHypothesesRefinementOff () 00214 { 00215 do_icp_hypotheses_refinement_ = false; 00216 } 00217 00218 /** \brief Add an object model to be recognized. 00219 * 00220 * \param[in] points are the object points. 00221 * \param[in] normals at each point. 00222 * \param[in] object_name is an identifier for the object. If that object is detected in the scene 'object_name' 00223 * is returned by the recognition method and you know which object has been detected. Note that 'object_name' has 00224 * to be unique! 00225 * \param[in] user_data is a pointer to some data (can be NULL) 00226 * 00227 * The method returns true if the model was successfully added to the model library and false otherwise (e.g., if 'object_name' is already in use). 00228 */ 00229 inline bool 00230 addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = NULL) 00231 { 00232 return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data)); 00233 } 00234 00235 /** \brief This method performs the recognition of the models loaded to the model library with the method addModel(). 00236 * 00237 * \param[in] scene is the 3d scene in which the object should be recognized. 00238 * \param[in] normals are the scene normals. 00239 * \param[out] recognized_objects is the list of output items each one containing the recognized model instance, its name, the aligning rigid transform 00240 * and the match confidence (see ObjRecRANSAC::Output for further explanations). 00241 * \param[in] success_probability is the user-defined probability of detecting all objects in the scene. 00242 */ 00243 void 00244 recognize (const PointCloudIn& scene, const PointCloudN& normals, std::list<ObjRecRANSAC::Output>& recognized_objects, double success_probability = 0.99); 00245 00246 inline void 00247 enterTestModeSampleOPP () 00248 { 00249 rec_mode_ = ObjRecRANSAC::SAMPLE_OPP; 00250 } 00251 00252 inline void 00253 enterTestModeTestHypotheses () 00254 { 00255 rec_mode_ = ObjRecRANSAC::TEST_HYPOTHESES; 00256 } 00257 00258 inline void 00259 leaveTestMode () 00260 { 00261 rec_mode_ = ObjRecRANSAC::FULL_RECOGNITION; 00262 } 00263 00264 /** \brief This function is useful for testing purposes. It returns the oriented point pairs which were sampled from the 00265 * scene during the recognition process. Makes sense only if some of the testing modes are active. */ 00266 inline const std::list<ObjRecRANSAC::OrientedPointPair>& 00267 getSampledOrientedPointPairs () const 00268 { 00269 return (sampled_oriented_point_pairs_); 00270 } 00271 00272 /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the 00273 * recognition process. Makes sense only if some of the testing modes are active. */ 00274 inline const std::vector<Hypothesis>& 00275 getAcceptedHypotheses () const 00276 { 00277 return (accepted_hypotheses_); 00278 } 00279 00280 /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the 00281 * recognition process. Makes sense only if some of the testing modes are active. */ 00282 inline void 00283 getAcceptedHypotheses (std::vector<Hypothesis>& out) const 00284 { 00285 out = accepted_hypotheses_; 00286 } 00287 00288 /** \brief Returns the hash table in the model library. */ 00289 inline const pcl::recognition::ModelLibrary::HashTable& 00290 getHashTable () const 00291 { 00292 return (model_library_.getHashTable ()); 00293 } 00294 00295 inline const ModelLibrary& 00296 getModelLibrary () const 00297 { 00298 return (model_library_); 00299 } 00300 00301 inline const ModelLibrary::Model* 00302 getModel (const std::string& name) const 00303 { 00304 return (model_library_.getModel (name)); 00305 } 00306 00307 inline const ORROctree& 00308 getSceneOctree () const 00309 { 00310 return (scene_octree_); 00311 } 00312 00313 inline RigidTransformSpace& 00314 getRigidTransformSpace () 00315 { 00316 return (transform_space_); 00317 } 00318 00319 inline float 00320 getPairWidth () const 00321 { 00322 return pair_width_; 00323 } 00324 00325 protected: 00326 enum Recognition_Mode {SAMPLE_OPP, TEST_HYPOTHESES, /*BUILD_CONFLICT_GRAPH,*/ FULL_RECOGNITION}; 00327 00328 friend class ModelLibrary; 00329 00330 inline int 00331 computeNumberOfIterations (double success_probability) const 00332 { 00333 // 'p_obj' is the probability that given that the first sample point belongs to an object, 00334 // the second sample point will belong to the same object 00335 const double p_obj = 0.25f; 00336 // old version: p = p_obj*relative_obj_size_*fraction_of_pairs_in_hash_table_; 00337 const double p = p_obj*relative_obj_size_; 00338 00339 if ( 1.0 - p <= 0.0 ) 00340 return 1; 00341 00342 return static_cast<int> (log (1.0-success_probability)/log (1.0-p) + 1.0); 00343 } 00344 00345 inline void 00346 clearTestData () 00347 { 00348 sampled_oriented_point_pairs_.clear (); 00349 accepted_hypotheses_.clear (); 00350 transform_space_.clear (); 00351 } 00352 00353 void 00354 sampleOrientedPointPairs (int num_iterations, const std::vector<ORROctree::Node*>& full_scene_leaves, std::list<OrientedPointPair>& output) const; 00355 00356 int 00357 generateHypotheses (const std::list<OrientedPointPair>& pairs, std::list<HypothesisBase>& out) const; 00358 00359 /** \brief Groups close hypotheses in 'hypotheses'. Saves a representative for each group in 'out'. Returns the 00360 * number of hypotheses after grouping. */ 00361 int 00362 groupHypotheses(std::list<HypothesisBase>& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space, 00363 HypothesisOctree& grouped_hypotheses) const; 00364 00365 inline void 00366 testHypothesis (Hypothesis* hypothesis, int& match, int& penalty) const; 00367 00368 inline void 00369 testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const; 00370 00371 void 00372 buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph<Hypothesis>& graph) const; 00373 00374 void 00375 filterGraphOfCloseHypotheses (ORRGraph<Hypothesis>& graph, std::vector<Hypothesis>& out) const; 00376 00377 void 00378 buildGraphOfConflictingHypotheses (const BVHH& bvh, ORRGraph<Hypothesis*>& graph) const; 00379 00380 void 00381 filterGraphOfConflictingHypotheses (ORRGraph<Hypothesis*>& graph, std::list<ObjRecRANSAC::Output>& recognized_objects) const; 00382 00383 /** \brief Computes the rigid transform that maps the line (a1, b1) to (a2, b2). 00384 * The computation is based on the corresponding points 'a1' <-> 'a2' and 'b1' <-> 'b2' 00385 * and the normals 'a1_n', 'b1_n', 'a2_n', and 'b2_n'. The result is saved in 00386 * 'rigid_transform' which is an array of length 12. The first 9 elements are the 00387 * rotational part (row major order) and the last 3 are the translation. */ 00388 inline void 00389 computeRigidTransform( 00390 const float *a1, const float *a1_n, const float *b1, const float* b1_n, 00391 const float *a2, const float *a2_n, const float *b2, const float* b2_n, 00392 float* rigid_transform) const 00393 { 00394 // Some local variables 00395 float o1[3], o2[3], x1[3], x2[3], y1[3], y2[3], z1[3], z2[3], tmp1[3], tmp2[3], Ro1[3], invFrame1[3][3]; 00396 00397 // Compute the origins 00398 o1[0] = 0.5f*(a1[0] + b1[0]); 00399 o1[1] = 0.5f*(a1[1] + b1[1]); 00400 o1[2] = 0.5f*(a1[2] + b1[2]); 00401 00402 o2[0] = 0.5f*(a2[0] + b2[0]); 00403 o2[1] = 0.5f*(a2[1] + b2[1]); 00404 o2[2] = 0.5f*(a2[2] + b2[2]); 00405 00406 // Compute the x-axes 00407 aux::diff3 (b1, a1, x1); aux::normalize3 (x1); 00408 aux::diff3 (b2, a2, x2); aux::normalize3 (x2); 00409 // Compute the y-axes. First y-axis 00410 aux::projectOnPlane3 (a1_n, x1, tmp1); aux::normalize3 (tmp1); 00411 aux::projectOnPlane3 (b1_n, x1, tmp2); aux::normalize3 (tmp2); 00412 aux::sum3 (tmp1, tmp2, y1); aux::normalize3 (y1); 00413 // Second y-axis 00414 aux::projectOnPlane3 (a2_n, x2, tmp1); aux::normalize3 (tmp1); 00415 aux::projectOnPlane3 (b2_n, x2, tmp2); aux::normalize3 (tmp2); 00416 aux::sum3 (tmp1, tmp2, y2); aux::normalize3 (y2); 00417 // Compute the z-axes 00418 aux::cross3 (x1, y1, z1); 00419 aux::cross3 (x2, y2, z2); 00420 00421 // 1. Invert the matrix [x1|y1|z1] (note that x1, y1, and z1 are treated as columns!) 00422 invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2]; 00423 invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2]; 00424 invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2]; 00425 // 2. Compute the desired rotation as rigid_transform = [x2|y2|z2]*invFrame1 00426 aux::mult3x3 (x2, y2, z2, invFrame1, rigid_transform); 00427 00428 // Construct the translation which is the difference between the rotated o1 and o2 00429 aux::mult3x3 (rigid_transform, o1, Ro1); 00430 rigid_transform[9] = o2[0] - Ro1[0]; 00431 rigid_transform[10] = o2[1] - Ro1[1]; 00432 rigid_transform[11] = o2[2] - Ro1[2]; 00433 } 00434 00435 /** \brief Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles between 00436 * n1 and (p2-p1), 00437 * n2 and (p1-p2), 00438 * n1 and n2 00439 * 00440 * \param[out] signature is an array of three doubles saving the three angles in the order shown above. */ 00441 static inline void 00442 compute_oriented_point_pair_signature (const float *p1, const float *n1, const float *p2, const float *n2, float signature[3]) 00443 { 00444 // Get the line from p1 to p2 00445 float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; 00446 aux::normalize3 (cl); 00447 00448 signature[0] = std::acos (aux::clamp (aux::dot3 (n1,cl), -1.0f, 1.0f)); cl[0] = -cl[0]; cl[1] = -cl[1]; cl[2] = -cl[2]; 00449 signature[1] = std::acos (aux::clamp (aux::dot3 (n2,cl), -1.0f, 1.0f)); 00450 signature[2] = std::acos (aux::clamp (aux::dot3 (n1,n2), -1.0f, 1.0f)); 00451 } 00452 00453 protected: 00454 // Parameters 00455 float pair_width_; 00456 float voxel_size_; 00457 float position_discretization_; 00458 float rotation_discretization_; 00459 float abs_zdist_thresh_; 00460 float relative_obj_size_; 00461 float visibility_; 00462 float relative_num_of_illegal_pts_; 00463 float intersection_fraction_; 00464 float max_coplanarity_angle_; 00465 float scene_bounds_enlargement_factor_; 00466 bool ignore_coplanar_opps_; 00467 float frac_of_points_for_icp_refinement_; 00468 bool do_icp_hypotheses_refinement_; 00469 00470 ModelLibrary model_library_; 00471 ORROctree scene_octree_; 00472 ORROctreeZProjection scene_octree_proj_; 00473 RigidTransformSpace transform_space_; 00474 TrimmedICP<pcl::PointXYZ, float> trimmed_icp_; 00475 PointCloudIn::Ptr scene_octree_points_; 00476 00477 std::list<OrientedPointPair> sampled_oriented_point_pairs_; 00478 std::vector<Hypothesis> accepted_hypotheses_; 00479 Recognition_Mode rec_mode_; 00480 }; 00481 } // namespace recognition 00482 } // namespace pcl 00483 00484 #endif // PCL_RECOGNITION_OBJ_REC_RANSAC_H_