From 2f49d0a2ee8017ad46d00a7289dea70f976b9f90 Mon Sep 17 00:00:00 2001 From: Chavdar Papazov Date: Tue, 12 Mar 2013 19:32:04 +0000 Subject: [PATCH 1/2] ObjRecRANSAC ready git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@8780 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- recognition/CMakeLists.txt | 9 +- .../pcl/recognition/ransac_based/auxiliary.h | 63 +++- .../recognition/ransac_based/model_library.h | 52 ++- .../recognition/ransac_based/obj_rec_ransac.h | 227 +++-------- .../pcl/recognition/ransac_based/orr_octree.h | 112 ++++-- .../ransac_based/orr_octree_zprojection.h | 25 +- .../ransac_based/rigid_transform_space.h | 353 ++++++++---------- .../src/ransac_based/model_library.cpp | 10 +- .../src/ransac_based/obj_rec_ransac.cpp | 250 +++++++++---- recognition/src/ransac_based/orr_octree.cpp | 2 +- tools/obj_rec_ransac_accepted_hypotheses.cpp | 34 +- tools/obj_rec_ransac_model_opps.cpp | 2 +- tools/obj_rec_ransac_orr_octree.cpp | 2 +- .../obj_rec_ransac_orr_octree_zprojection.cpp | 4 +- tools/obj_rec_ransac_result.cpp | 6 +- tools/obj_rec_ransac_scene_opps.cpp | 2 +- 16 files changed, 633 insertions(+), 520 deletions(-) diff --git a/recognition/CMakeLists.txt b/recognition/CMakeLists.txt index 849bbb7556c..e4516d8af5a 100644 --- a/recognition/CMakeLists.txt +++ b/recognition/CMakeLists.txt @@ -41,10 +41,12 @@ if(build) include/pcl/${SUBSYS_NAME}/ransac_based/obj_rec_ransac.h include/pcl/${SUBSYS_NAME}/ransac_based/orr_graph.h include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree_zprojection.h + include/pcl/${SUBSYS_NAME}/ransac_based/trimmed_icp.h include/pcl/${SUBSYS_NAME}/ransac_based/orr_octree.h + include/pcl/${SUBSYS_NAME}/ransac_based/simple_octree.h include/pcl/${SUBSYS_NAME}/ransac_based/voxel_structure.h include/pcl/${SUBSYS_NAME}/ransac_based/bvh.h - include/pcl/${SUBSYS_NAME}/implicit_shape_model.h + include/pcl/${SUBSYS_NAME}/implicit_shape_model.h ) set(hv_incs @@ -71,8 +73,9 @@ if(build) set(impl_incs include/pcl/${SUBSYS_NAME}/impl/linemod/line_rgbd.hpp + include/pcl/${SUBSYS_NAME}/impl/ransac_based/simple_octree.hpp include/pcl/${SUBSYS_NAME}/impl/ransac_based/voxel_structure.hpp - include/pcl/${SUBSYS_NAME}/impl/implicit_shape_model.hpp + include/pcl/${SUBSYS_NAME}/impl/implicit_shape_model.hpp ) set(hv_impl_incs @@ -105,7 +108,7 @@ if(build) src/ransac_based/orr_octree_zprojection.cpp src/face_detection/face_detector_data_provider.cpp src/face_detection/rf_face_detector_trainer.cpp - src/implicit_shape_model.cpp + src/implicit_shape_model.cpp ) set(metslib_incs diff --git a/recognition/include/pcl/recognition/ransac_based/auxiliary.h b/recognition/include/pcl/recognition/ransac_based/auxiliary.h index 38d22162920..3934482399e 100644 --- a/recognition/include/pcl/recognition/ransac_based/auxiliary.h +++ b/recognition/include/pcl/recognition/ransac_based/auxiliary.h @@ -41,6 +41,7 @@ #include #include #include +#include #define AUX_PI_FLOAT 3.14159265358979323846f #define AUX_HALF_PI 1.57079632679489661923f @@ -61,7 +62,13 @@ namespace pcl return static_cast (a.first < b.first); } - template float + template T + sqr (T a) + { + return (a*a); + } + + template T clamp (T value, T min, T max) { if ( value < min ) @@ -115,6 +122,15 @@ namespace pcl dst[2] = src[2]; } + /** \brief dst = src */ + template void + copy3 (const T src[3], pcl::PointXYZ& dst) + { + dst.x = src[0]; + dst.y = src[1]; + dst.z = src[2]; + } + /** \brief a = -a */ template void flip3 (T a[3]) @@ -174,6 +190,13 @@ namespace pcl return (length3 (l)); } + /** \brief Returns the squared Euclidean distance between a and b. */ + template T + sqrDistance3 (const T a[3], const T b[3]) + { + return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2])); + } + /** \brief Returns the dot product a*b */ template T dot3 (const T a[3], const T b[3]) @@ -290,6 +313,25 @@ namespace pcl out[2] = t[6]*x + t[7]*y + t[8]*z + t[11]; } + /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */ + template void + transform(const Eigen::Matrix& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out) + { + out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3); + out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3); + out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3); + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], const pcl::PointXYZ& p, T out[3]) + { + out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9]; + out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10]; + out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11]; + } + /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1' * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger * the value the larger the deviation between the normals can be which still leads to a positive test result. The @@ -315,6 +357,23 @@ namespace pcl return (true); } + template void + array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix& dst) + { + dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9]; + dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10]; + dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11]; + dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0; + } + + template void + matrix4x4ToArray12 (const Eigen::Matrix& src, Scalar dst[12]) + { + dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3); + dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3); + dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3); + } + /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. * dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); * dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); @@ -356,7 +415,7 @@ namespace pcl } // Normalize the input - float normalized_axis_angle[3]; + T normalized_axis_angle[3]; aux::mult3 (axis_angle, static_cast (1.0)/angle, normalized_axis_angle); // The eigen objects diff --git a/recognition/include/pcl/recognition/ransac_based/model_library.h b/recognition/include/pcl/recognition/ransac_based/model_library.h index 516778949fc..4b8f3426666 100644 --- a/recognition/include/pcl/recognition/ransac_based/model_library.h +++ b/recognition/include/pcl/recognition/ransac_based/model_library.h @@ -42,9 +42,11 @@ #include "auxiliary.h" #include #include +#include #include #include #include +#include #include #include #include @@ -64,7 +66,8 @@ namespace pcl class Model { public: - Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name, void* user_data = NULL) + Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = NULL) : obj_name_(object_name), user_data_ (user_data) { @@ -89,8 +92,34 @@ namespace pcl aux::expandBoundingBoxToContainPoint (bounds_of_octree_points_, (*it)->getData ()->getPoint ()); } + int num_octree_points = static_cast (full_leaves.size ()); // Finalize the center of mass computation - aux::mult3 (octree_center_of_mass_, 1.0f/static_cast (full_leaves.size ())); + aux::mult3 (octree_center_of_mass_, 1.0f/static_cast (num_octree_points)); + + int num_points_for_registration = static_cast (static_cast (num_octree_points)*frac_of_points_for_registration); + points_for_registration_.resize (static_cast (num_points_for_registration)); + + // Prepare for random point sampling + std::vector ids (num_octree_points); + for ( int i = 0 ; i < num_octree_points ; ++i ) + ids[i] = i; + + // The random generator + pcl::common::UniformGenerator randgen (0, num_octree_points - 1, static_cast (time (NULL))); + + // Randomly sample some points from the octree + for ( int i = 0 ; i < num_points_for_registration ; ++i ) + { + // Choose a random position within the array of ids + randgen.setParameters (0, static_cast (ids.size ()) - 1); + int rand_pos = randgen.run (); + + // Copy the randomly selected octree point + aux::copy3 (octree_.getFullLeaves ()[ids[rand_pos]]->getData ()->getPoint (), points_for_registration_[i]); + + // Delete the selected id + ids.erase (ids.begin() + rand_pos); + } } virtual ~Model () @@ -127,15 +156,22 @@ namespace pcl return (bounds_of_octree_points_); } + inline const PointCloudIn& + getPointsForRegistration () const + { + return (points_for_registration_); + } + protected: const std::string obj_name_; ORROctree octree_; float octree_center_of_mass_[3]; float bounds_of_octree_points_[6]; + PointCloudIn points_for_registration_; void* user_data_; }; - typedef std::list > node_data_pair_list; + typedef std::list > node_data_pair_list; typedef std::map HashTableCell; typedef VoxelStructure HashTable; @@ -182,11 +218,13 @@ namespace pcl * \param[in] points represents the model to be added. * \param[in] normals are the normals at the model points. * \param[in] object_name is the unique name of the object to be added. + * \param[in] num_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing * \param[in] user_data is a pointer to some data (can be NULL) * * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */ bool - addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = NULL); + addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = NULL); /** \brief Returns the hash table built by this instance. */ inline const HashTable& @@ -205,6 +243,12 @@ namespace pcl return (NULL); } + inline const std::map& + getModels () const + { + return (models_); + } + protected: /** \brief Removes all models from the library and destroys the hash table. This method should be called upon destroying this object. */ void diff --git a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h index 6714321ef6e..8827fd05063 100644 --- a/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h +++ b/recognition/include/pcl/recognition/ransac_based/obj_rec_ransac.h @@ -39,13 +39,18 @@ #ifndef PCL_RECOGNITION_OBJ_REC_RANSAC_H_ #define PCL_RECOGNITION_OBJ_REC_RANSAC_H_ +#include #include #include #include #include +#include +#include #include #include #include +#include +#include #include #include #include @@ -84,6 +89,8 @@ namespace pcl typedef ModelLibrary::PointCloudIn PointCloudIn; typedef ModelLibrary::PointCloudN PointCloudN; + typedef BVH BVHH; + /** \brief This is an output item of the ObjRecRANSAC::recognize() method. It contains the recognized model, its name (the ones passed to * ObjRecRANSAC::addModel()), the rigid transform which aligns the model with the input scene and the match confidence which is a number * 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 @@ -109,134 +116,30 @@ namespace pcl void* user_data_; }; - class OrientedPointPair - { - public: - OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2) - : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2) - { - } + class OrientedPointPair + { + public: + OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2) + : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2) + { + } + + virtual ~OrientedPointPair (){} - virtual ~OrientedPointPair (){} + public: + const float *p1_, *n1_, *p2_, *n2_; + }; + class HypothesisCreator + { public: - const float *p1_, *n1_, *p2_, *n2_; - }; - - class HypothesisBase - { - public: - HypothesisBase (const ModelLibrary::Model* obj_model) - : obj_model_ (obj_model) - {} - - HypothesisBase (const ModelLibrary::Model* obj_model, const float* rigid_transform) - : obj_model_ (obj_model) - { - memcpy (rigid_transform_, rigid_transform, 12*sizeof (float)); - } - - virtual ~HypothesisBase (){} - - public: - float rigid_transform_[12]; - const ModelLibrary::Model* obj_model_; - }; - - class Hypothesis: public HypothesisBase - { - public: - Hypothesis (const ModelLibrary::Model* obj_model = NULL) - : HypothesisBase (obj_model), - match_confidence_ (-1.0f), - linear_id_ (-1) - { - aux::set3 (pos_id_, -1); - aux::set3 (rot_id_, -1); - } - - Hypothesis (const Hypothesis& src) - : HypothesisBase (src.obj_model_, src.rigid_transform_), - match_confidence_ (src.match_confidence_), - explained_pixels_ (src.explained_pixels_) - { - memcpy (this->pos_id_, src.pos_id_, 3*sizeof (int)); - memcpy (this->rot_id_, src.rot_id_, 3*sizeof (int)); - } - - virtual ~Hypothesis (){} - - void - operator =(const Hypothesis& src) - { - memcpy (this->rigid_transform_, src.rigid_transform_, 12*sizeof (float)); - this->obj_model_ = src.obj_model_; - this->match_confidence_ = src.match_confidence_; - this->explained_pixels_ = src.explained_pixels_; - memcpy (this->pos_id_, src.pos_id_, 3*sizeof (int)); - memcpy (this->rot_id_, src.rot_id_, 3*sizeof (int)); - } - - void - setPositionId (const int id[3]) - { - aux::copy3 (id, pos_id_); - } - - void - setRotationId (const int id[3]) - { - aux::copy3 (id, rot_id_); - } - - void - setLinearId (int id) - { - linear_id_ = id; - } - - int - getLinearId () const - { - return (linear_id_); - } - - void - computeBounds (float bounds[6]) const - { - const float *b = obj_model_->getBoundsOfOctreePoints (); - float p[3]; - - // Initialize 'bounds' - aux::transform (rigid_transform_, b[0], b[2], b[4], p); - bounds[0] = bounds[1] = p[0]; - bounds[2] = bounds[3] = p[1]; - bounds[4] = bounds[5] = p[2]; - - // Expand 'bounds' to contain the other 7 points of the octree bounding box - aux::transform (rigid_transform_, b[0], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[0], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[0], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[1], b[2], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[1], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[1], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); - aux::transform (rigid_transform_, b[1], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); - } - - void - computeCenterOfMass (float center_of_mass[3]) const - { - aux::transform (rigid_transform_, obj_model_->getOctreeCenterOfMass (), center_of_mass); - } - - public: - float match_confidence_; - std::set explained_pixels_; - int pos_id_[3], rot_id_[3]; - int linear_id_; - }; - - typedef BVH BVHH; + HypothesisCreator (){} + virtual ~HypothesisCreator (){} + + Hypothesis* create (const SimpleOctree::Node* ) const { return new Hypothesis ();} + }; + + typedef SimpleOctree HypothesisOctree; public: /** \brief Constructor with some important parameters which can not be changed once an instance of that class is created. @@ -264,6 +167,7 @@ namespace pcl scene_octree_proj_.clear (); sampled_oriented_point_pairs_.clear (); transform_space_.clear (); + scene_octree_points_.reset (); } /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will @@ -299,6 +203,18 @@ namespace pcl model_library_.ignoreCoplanarPointPairsOff (); } + inline void + icpHypothesesRefinementOn () + { + do_icp_hypotheses_refinement_ = true; + } + + inline void + icpHypothesesRefinementOff () + { + do_icp_hypotheses_refinement_ = false; + } + /** \brief Add an object model to be recognized. * * \param[in] points are the object points. @@ -313,7 +229,7 @@ namespace pcl inline bool addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = NULL) { - return (model_library_.addModel (points, normals, object_name, user_data)); + return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data)); } /** \brief This method performs the recognition of the models loaded to the model library with the method addModel(). @@ -355,7 +271,7 @@ namespace pcl /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the * recognition process. Makes sense only if some of the testing modes are active. */ - inline const std::vector& + inline const std::vector& getAcceptedHypotheses () const { return (accepted_hypotheses_); @@ -364,7 +280,7 @@ namespace pcl /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the * recognition process. Makes sense only if some of the testing modes are active. */ inline void - getAcceptedHypotheses (std::vector& out) const + getAcceptedHypotheses (std::vector& out) const { out = accepted_hypotheses_; } @@ -394,8 +310,8 @@ namespace pcl return (scene_octree_); } - inline const RigidTransformSpace& - getRigidTransformSpace () const + inline RigidTransformSpace& + getRigidTransformSpace () { return (transform_space_); } @@ -443,48 +359,17 @@ namespace pcl /** \brief Groups close hypotheses in 'hypotheses'. Saves a representative for each group in 'out'. Returns the * number of hypotheses after grouping. */ int - groupHypotheses(const std::list& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space, - std::vector*>& rot_spaces) const; + groupHypotheses(std::list& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space, + HypothesisOctree& grouped_hypotheses) const; inline void - testHypothesis (Hypothesis* hypothesis, float& match, int& penalty) const - { - match = 0.0; - penalty = 0; - - // For better code readability - const std::vector& full_model_leaves = hypothesis->obj_model_->getOctree ().getFullLeaves (); - const float* rigid_transform = hypothesis->rigid_transform_; - const ORROctreeZProjection::Pixel* pixel; - float transformed_point[3]; - - // The match/penalty loop - for ( std::vector::const_iterator leaf_it = full_model_leaves.begin () ; leaf_it != full_model_leaves.end () ; ++leaf_it ) - { - // Transform the model point with the current rigid transform - aux::transform (rigid_transform, (*leaf_it)->getData ()->getPoint (), transformed_point); - - // Get the pixel 'transformed_point' lies in - pixel = scene_octree_proj_.getPixel (transformed_point); - // Check if we have a valid pixel - if ( pixel == NULL ) - continue; - - if ( transformed_point[2] < pixel->z1_ ) // The transformed model point overshadows a pixel -> penalize the hypothesis - ++penalty; - else if ( transformed_point[2] <= pixel->z2_ ) // The point is OK - { - ++match; - // 'hypo_it' explains 'pixel' => insert the pixel's id in the id set of 'hypo_it' - hypothesis->explained_pixels_.insert (pixel->id_); - } - } - } + testHypothesis (Hypothesis* hypothesis, int& match, int& penalty) const; + + inline void + testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const; void - buildGraphOfCloseHypotheses (const RigidTransformSpace& transform_space, - const std::vector*> rotation_spaces, - ORRGraph& graph) const; + buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph& graph) const; void filterGraphOfCloseHypotheses (ORRGraph& graph, std::vector& out) const; @@ -579,11 +464,15 @@ namespace pcl float max_coplanarity_angle_; float scene_bounds_enlargement_factor_; bool ignore_coplanar_opps_; + float frac_of_points_for_icp_refinement_; + bool do_icp_hypotheses_refinement_; ModelLibrary model_library_; ORROctree scene_octree_; ORROctreeZProjection scene_octree_proj_; - RigidTransformSpace transform_space_; + RigidTransformSpace transform_space_; + TrimmedICP trimmed_icp_; + PointCloudIn::Ptr scene_octree_points_; std::list sampled_oriented_point_pairs_; std::vector accepted_hypotheses_; diff --git a/recognition/include/pcl/recognition/ransac_based/orr_octree.h b/recognition/include/pcl/recognition/ransac_based/orr_octree.h index b287b323b86..8eaad540dec 100644 --- a/recognition/include/pcl/recognition/ransac_based/orr_octree.h +++ b/recognition/include/pcl/recognition/ransac_based/orr_octree.h @@ -54,6 +54,7 @@ #include #include #include +#include //#define PCL_REC_ORR_OCTREE_VERBOSE @@ -80,11 +81,12 @@ namespace pcl class Data { public: - Data (void* user_data = NULL) - : id_x_ (-1), - id_y_ (-1), - id_z_ (-1), - num_points_(0), + Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = NULL) + : id_x_ (id_x), + id_y_ (id_y), + id_z_ (id_z), + lin_id_ (lin_id), + num_points_ (0), user_data_ (user_data) { n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f; @@ -124,9 +126,6 @@ namespace pcl inline float* getNormal (){ return n_;} - inline void - set3dId (int x, int y, int z){ id_x_ = x; id_y_ = y; id_z_ = z;} - inline void get3dId (int id[3]) const { @@ -144,21 +143,25 @@ namespace pcl inline int get3dIdZ () const {return id_z_;} + inline int + getLinearId () const { return lin_id_;} + inline void - setUserData (void* user_data) - { - user_data_ = user_data; - } + setUserData (void* user_data){ user_data_ = user_data;} inline void* - getUserData () const - { - return user_data_; - } + getUserData () const { return user_data_;} + + inline void + insertNeighbor (Node* node){ neighbors_.insert (node);} + + inline const std::set& + getNeighbors () const { return (neighbors_);} protected: float n_[3], p_[3]; - int id_x_, id_y_, id_z_, num_points_; + int id_x_, id_y_, id_z_, lin_id_, num_points_; + std::set neighbors_; void *user_data_; }; @@ -257,6 +260,18 @@ namespace pcl } } + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node) + { + if ( !this->getData () || !node->getData () ) + return; + + this->getData ()->insertNeighbor (node); + node->getData ()->insertNeighbor (this); + } + protected: Node::Data *data_; float center_[3], bounds_[6], radius_; @@ -316,15 +331,14 @@ namespace pcl if ( !node->getData () ) { - Node::Data* data = new Node::Data (); - // Compute the 3d integer id of the leaf - // Compute the 3d integer id of the leaf - data->set3dId( - static_cast ((node->getCenter ()[0] - bounds_[0])/voxel_size_), - static_cast ((node->getCenter ()[1] - bounds_[2])/voxel_size_), - static_cast ((node->getCenter ()[2] - bounds_[4])/voxel_size_)); - // Save the data + Node::Data* data = new Node::Data ( + static_cast ((node->getCenter ()[0] - bounds_[0])/voxel_size_), + static_cast ((node->getCenter ()[1] - bounds_[2])/voxel_size_), + static_cast ((node->getCenter ()[2] - bounds_[4])/voxel_size_), + static_cast (full_leaves_.size ())); + node->setData (data); + this->insertNeighbors (node); full_leaves_.push_back (node); } @@ -347,8 +361,8 @@ namespace pcl /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf * with id [i, j, k] or NULL is no such leaf exists. */ - const ORROctree::Node* - getLeaf (int i, int j, int k) const + ORROctree::Node* + getLeaf (int i, int j, int k) { float offset = 0.5f*voxel_size_; float p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, @@ -359,8 +373,8 @@ namespace pcl } /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */ - inline const ORROctree::Node* - getLeaf (float x, float y, float z) const + inline ORROctree::Node* + getLeaf (float x, float y, float z) { // Make sure that the input point is within the octree bounds if ( x < bounds_[0] || x > bounds_[1] || @@ -405,7 +419,7 @@ namespace pcl getFullLeaves () const { return full_leaves_;} void - getFullLeafPoints (PointCloudOut& out) const; + getFullLeavesPoints (PointCloudOut& out) const; void getNormalsOfFullLeaves (PointCloudN& out) const; @@ -428,6 +442,44 @@ namespace pcl inline float getVoxelSize () const { return voxel_size_;} + inline void + insertNeighbors (Node* node) + { + const float* c = node->getCenter (); + float s = 0.5f*voxel_size_; + Node *neigh; + + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + } + protected: float voxel_size_, bounds_[6]; int tree_levels_; diff --git a/recognition/include/pcl/recognition/ransac_based/orr_octree_zprojection.h b/recognition/include/pcl/recognition/ransac_based/orr_octree_zprojection.h index c1ebf08f548..524a0441011 100644 --- a/recognition/include/pcl/recognition/ransac_based/orr_octree_zprojection.h +++ b/recognition/include/pcl/recognition/ransac_based/orr_octree_zprojection.h @@ -71,7 +71,16 @@ namespace pcl inline void set_z2 (float z2) { z2_ = z2;} - public: + float + z1 () const { return z1_;} + + float + z2 () const { return z2_;} + + int + getId () const { return id_;} + + protected: float z1_, z2_; int id_; }; @@ -149,6 +158,20 @@ namespace pcl return (pixels_[x][y]); } + inline const std::set* + getOctreeNodes (const float* p) const + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (NULL); + if ( y < 0 || y >= num_pixels_y_ ) return (NULL); + + if ( !sets_[x][y] ) + return NULL; + + return (&sets_[x][y]->get_nodes ()); + } + inline std::list& getFullPixels (){ return full_pixels_;} diff --git a/recognition/include/pcl/recognition/ransac_based/rigid_transform_space.h b/recognition/include/pcl/recognition/ransac_based/rigid_transform_space.h index dd549f15c22..86574b0185d 100644 --- a/recognition/include/pcl/recognition/ransac_based/rigid_transform_space.h +++ b/recognition/include/pcl/recognition/ransac_based/rigid_transform_space.h @@ -46,19 +46,17 @@ #ifndef PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_ #define PCL_RECOGNITION_RIGID_TRANSFORM_SPACE_H_ -#include "orr_octree.h" +#include "simple_octree.h" #include "model_library.h" #include #include #include -#define ROT_SPACE_TEST - namespace pcl { namespace recognition { - class PCL_EXPORTS Cell + class RotationSpaceCell { public: class Entry @@ -78,12 +76,23 @@ namespace pcl aux::copy3 (src.translation_, this->translation_); } - inline void + const Entry& operator = (const Entry& src) + { + num_transforms_ = src.num_transforms_; + aux::copy3 (src.axis_angle_, this->axis_angle_); + aux::copy3 (src.translation_, this->translation_); + + return *this; + } + + inline const Entry& addRigidTransform (const float axis_angle[3], const float translation[3]) { aux::add3 (this->axis_angle_, axis_angle); aux::add3 (this->translation_, translation); ++num_transforms_; + + return *this; } inline void @@ -118,46 +127,64 @@ namespace pcl return (translation_); } - public: + inline int + getNumberOfTransforms () const + { + return (num_transforms_); + } + + protected: float axis_angle_[3], translation_[3]; int num_transforms_; };// class Entry public: - Cell (){} - virtual ~Cell () + RotationSpaceCell (){} + virtual ~RotationSpaceCell () { model_to_entry_.clear (); } - inline int* - getPositionId () + inline std::map& + getEntries () { - return (pos_id_); + return (model_to_entry_); } - inline int* - getRotationId () + inline const RotationSpaceCell::Entry* + getEntry (const ModelLibrary::Model* model) const { - return (rot_id_); - } + std::map::const_iterator res = model_to_entry_.find (model); - inline std::map& - getEntries () - { - return (model_to_entry_); + if ( res != model_to_entry_.end () ) + return (&res->second); + + return (NULL); } - inline void + inline const RotationSpaceCell::Entry& addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) { - model_to_entry_[model].addRigidTransform (axis_angle, translation); + return model_to_entry_[model].addRigidTransform (axis_angle, translation); } protected: std::map model_to_entry_; - int pos_id_[3], rot_id_[3]; - }; // class Cell + }; // class RotationSpaceCell + + class RotationSpaceCellCreator + { + public: + RotationSpaceCellCreator (){} + virtual ~RotationSpaceCellCreator (){} + + RotationSpaceCell* create (const SimpleOctree::Node* ) + { + return (new RotationSpaceCell ()); + } + }; + + typedef SimpleOctree CellOctree; /** \brief This is a class for a discrete representation of the rotation space based on the axis-angle representation. * This class is not supposed to be very general. That's why it is dependent on the class ModelLibrary. @@ -165,7 +192,6 @@ namespace pcl * \author Chavdar Papazov * \ingroup recognition */ - template class PCL_EXPORTS RotationSpace { public: @@ -177,71 +203,74 @@ namespace pcl float bounds[6] = {min, max, min, max, min, max}; // Build the voxel structure - octree_.build (bounds, discretization); - aux::set3 (pos_id_, -1); + octree_.build (bounds, discretization, &cell_creator_); } virtual ~RotationSpace () { - for ( std::list::iterator it = full_cells_.begin () ; it != full_cells_.end () ; ++it ) - delete *it; - full_cells_.clear (); + octree_.clear (); } - inline int* - getPositionId () + inline void + setCenter (const float* c) { - return (pos_id_); + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; } - inline std::list& - getFullCells () - { - return (full_cells_); - } + inline const float* + getCenter () const { return center_;} - inline void - setOctreeLeaf (ORROctree::Node* leaf) + inline bool + getTransformWithMostVotes (const ModelLibrary::Model* model, float rigid_transform[12]) const { - positional_leaf_ = leaf; - } + RotationSpaceCell::Entry with_most_votes; + const std::vector& full_leaves = octree_.getFullLeaves (); + int max_num_transforms = 0; - inline ORROctree::Node* - getOctreeLeaf () const - { - return (positional_leaf_); - } + // For each full leaf + for ( std::vector::const_iterator leaf = full_leaves.begin () ; leaf != full_leaves.end () ; ++leaf ) + { + // Is there an entry for 'model' in the current cell + const RotationSpaceCell::Entry *entry = (*leaf)->getData ().getEntry (model); + if ( !entry ) + continue; - inline void - setData (const Data& data) - { - data_ = data; - } + int num_transforms = entry->getNumberOfTransforms (); + const std::set& neighs = (*leaf)->getNeighbors (); - inline const Data& - getData () const - { - return (data_); - } + // For each neighbor + for ( std::set::const_iterator neigh = neighs.begin () ; neigh != neighs.end () ; ++neigh ) + { + const RotationSpaceCell::Entry *neigh_entry = (*neigh)->getData ().getEntry (model); + if ( !neigh_entry ) + continue; - inline void - setLinearId (int id) - { - linear_id_ = id; - } + num_transforms += neigh_entry->getNumberOfTransforms (); + } - inline int - getLinearId () const - { - return (linear_id_); + if ( num_transforms > max_num_transforms ) + { + with_most_votes = *entry; + max_num_transforms = num_transforms; + } + } + + if ( !max_num_transforms ) + return false; + + with_most_votes.computeAverageRigidTransform (rigid_transform); + + return true; } inline bool addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) { - ORROctree::Node* rot_leaf = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]); + CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]); - if ( !rot_leaf ) + if ( !cell ) { const float *b = octree_.getBounds (); printf ("WARNING in 'RotationSpace::%s()': the provided axis-angle input (%f, %f, %f) is " @@ -250,157 +279,111 @@ namespace pcl return (false); } - Cell* cell; - - if ( !rot_leaf->getData ()->getUserData () ) - { - cell = new Cell (); - rot_leaf->setUserData (cell); - // Save the ids - aux::copy3(pos_id_, cell->getPositionId ()); - rot_leaf->getData ()->get3dId (cell->getRotationId ()); - // Save the cell - full_cells_.push_back (cell); - } - else - cell = static_cast (rot_leaf->getData ()->getUserData ()); - // Add the rigid transform to the cell - cell->addRigidTransform (model, axis_angle, translation); + cell->getData ().addRigidTransform (model, axis_angle, translation); return (true); } protected: - ORROctree octree_; - std::list full_cells_; - int pos_id_[3]; - int linear_id_; - ORROctree::Node* positional_leaf_; - Data data_; + CellOctree octree_; + RotationSpaceCellCreator cell_creator_; + float center_[3]; };// class RotationSpace - template - class PCL_EXPORTS RigidTransformSpace + class RotationSpaceCreator { public: - RigidTransformSpace () - : num_occupied_rotation_spaces_ (0) + RotationSpaceCreator() + : counter_ (0) {} - virtual ~RigidTransformSpace () + virtual ~RotationSpaceCreator(){} + + RotationSpace* create(const SimpleOctree::Node* leaf) { - this->clear (); + RotationSpace *rot_space = new RotationSpace (discretization_); + rot_space->setCenter (leaf->getCenter ()); + rotation_spaces_.push_back (rot_space); + + ++counter_; + + return (rot_space); } + void + setDiscretization (float value){ discretization_ = value;} + + int + getNumberOfRotationSpaces () const { return (counter_);} + + const std::list& + getRotationSpaces () const { return (rotation_spaces_);} + + std::list& + getRotationSpaces (){ return (rotation_spaces_);} + + void + reset () + { + counter_ = 0; + rotation_spaces_.clear (); + } + + protected: + float discretization_; + int counter_; + std::list rotation_spaces_; + }; + + typedef SimpleOctree RotationSpaceOctree; + + class PCL_EXPORTS RigidTransformSpace + { + public: + RigidTransformSpace (){} + virtual ~RigidTransformSpace (){ this->clear ();} + inline void build (const float* pos_bounds, float translation_cell_size, float rotation_cell_size) { this->clear (); - translation_cell_size_ = translation_cell_size; - rotation_cell_size_ = rotation_cell_size; + rotation_space_creator_.setDiscretization (rotation_cell_size); - pos_octree_.build (pos_bounds, translation_cell_size); + pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_); } inline void clear () { - for ( typename std::list*>::iterator it = rotation_space_list_.begin () ; it != rotation_space_list_.end () ; ++it ) - delete *it; - rotation_space_list_.clear (); pos_octree_.clear (); - num_occupied_rotation_spaces_ = 0; + rotation_space_creator_.reset (); } - inline std::list*>& + inline std::list& getRotationSpaces () { - return (rotation_space_list_); + return (rotation_space_creator_.getRotationSpaces ()); } - inline const std::list*>& + inline const std::list& getRotationSpaces () const { - return (rotation_space_list_); + return (rotation_space_creator_.getRotationSpaces ()); } inline int getNumberOfOccupiedRotationSpaces () { - return (num_occupied_rotation_spaces_); - } - - inline bool - getPositionCellBounds (const int id[3], float bounds[6]) const - { - const ORROctree::Node* leaf = pos_octree_.getLeaf (id[0], id[1], id[2]); - - if ( !leaf ) - return (false); - - leaf->getBounds (bounds); - - return (true); - } - - inline RotationSpace* - getRotationSpace (int i, int j, int k) const - { - const ORROctree::Node* leaf = pos_octree_.getLeaf (i, j, k); - - if ( !leaf ) - return NULL; - - if ( !leaf->getData () ) - return NULL; - - return static_cast*> (leaf->getData ()->getUserData ()); - } - - inline void - getNeighbors(const int id[3], std::list*>& neighs) const - { - RotationSpace* neigh; - - // Note that 'node' is not a neighbor of itself - neigh = this->getRotationSpace (id[0]+1, id[1]+1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]+1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]+1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1] , id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1] , id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1] , id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1]-1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]-1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]-1, id[2]+1); if ( neigh ) neighs.push_back (neigh); - - neigh = this->getRotationSpace (id[0]+1, id[1]+1, id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]+1, id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]+1, id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1] , id[2] ); if ( neigh ) neighs.push_back (neigh); - //neigh = this->getRotationSpace (id[0] , id[1] , id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1] , id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1]-1, id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]-1, id[2] ); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]-1, id[2] ); if ( neigh ) neighs.push_back (neigh); - - neigh = this->getRotationSpace (id[0]+1, id[1]+1, id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]+1, id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]+1, id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1] , id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1] , id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1] , id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]+1, id[1]-1, id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0] , id[1]-1, id[2]-1); if ( neigh ) neighs.push_back (neigh); - neigh = this->getRotationSpace (id[0]-1, id[1]-1, id[2]-1); if ( neigh ) neighs.push_back (neigh); + return (rotation_space_creator_.getNumberOfRotationSpaces ()); } inline bool addRigidTransform (const ModelLibrary::Model* model, const float position[3], const float rigid_transform[12]) { // Get the leaf 'position' ends up in - ORROctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]); + RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]); if ( !leaf ) { @@ -409,39 +392,21 @@ namespace pcl return (false); } - RotationSpace* rot_space; - - // Shall we create a new rotation space instance - if ( !leaf->getData ()->getUserData () ) - { - rot_space = new RotationSpace (rotation_cell_size_); - leaf->setUserData (rot_space); - leaf->getData ()->get3dId (rot_space->getPositionId ()); - rot_space->setOctreeLeaf (leaf); - rotation_space_list_.push_back (rot_space); - ++num_occupied_rotation_spaces_; - } - else // get the existing rotation space - rot_space = static_cast*> (leaf->getData ()->getUserData ()); - float rot_angle, axis_angle[3]; // Extract the axis-angle representation from the rotation matrix aux::rotationMatrixToAxisAngle (rigid_transform, axis_angle, rot_angle); // Multiply the axis by the angle to get the final representation aux::mult3 (axis_angle, rot_angle); - // Now, add the rigid transform to the space representation - rot_space->addRigidTransform (model, axis_angle, rigid_transform + 9); + // Now, add the rigid transform to the rotation space + leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9); return (true); } protected: - ORROctree pos_octree_; - float translation_cell_size_; - float rotation_cell_size_; - int num_occupied_rotation_spaces_; - std::list*> rotation_space_list_; + RotationSpaceOctree pos_octree_; + RotationSpaceCreator rotation_space_creator_; }; // class RigidTransformSpace } // namespace recognition } // namespace pcl diff --git a/recognition/src/ransac_based/model_library.cpp b/recognition/src/ransac_based/model_library.cpp index ad08e596d26..df5c11b5a9b 100644 --- a/recognition/src/ransac_based/model_library.cpp +++ b/recognition/src/ransac_based/model_library.cpp @@ -103,7 +103,8 @@ ModelLibrary::removeAllModels () //============================================================================================================================================ bool -ModelLibrary::addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data) +ModelLibrary::addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, + float frac_of_points_for_registration, void* user_data) { #ifdef OBJ_REC_RANSAC_VERBOSE printf("ModelLibrary::%s(): begin [%s]\n", __func__, object_name.c_str ()); @@ -120,19 +121,18 @@ ModelLibrary::addModel (const PointCloudIn& points, const PointCloudN& normals, } // It is unique -> create a new library model and save it - Model* new_model = new Model (points, normals, voxel_size_, object_name, user_data); + Model* new_model = new Model (points, normals, voxel_size_, object_name, frac_of_points_for_registration, user_data); result.first->second = new_model; const ORROctree& octree = new_model->getOctree (); const vector &full_leaves = octree.getFullLeaves (); list inter_leaves; - const ORROctree::Node::Data *node_data1; int num_of_pairs = 0; // Run through all full leaves for ( vector::const_iterator leaf1 = full_leaves.begin () ; leaf1 != full_leaves.end () ; ++leaf1 ) { - node_data1 = (*leaf1)->getData (); + const ORROctree::Node::Data* node_data1 = (*leaf1)->getData (); // Get all full leaves at the right distance to the current leaf inter_leaves.clear (); @@ -177,7 +177,7 @@ ModelLibrary::addToHashTable (Model* model, const ORROctree::Node::Data* data1, HashTableCell* cell = hash_table_.getVoxel (key); // Insert the pair (data1,data2) belonging to 'model' - (*cell)[model].push_back (std::pair (data1, data2)); + (*cell)[model].push_back (std::pair (data1, data2)); return (true); } diff --git a/recognition/src/ransac_based/obj_rec_ransac.cpp b/recognition/src/ransac_based/obj_rec_ransac.cpp index 5e96ffca26b..576af74a09f 100644 --- a/recognition/src/ransac_based/obj_rec_ransac.cpp +++ b/recognition/src/ransac_based/obj_rec_ransac.cpp @@ -56,6 +56,8 @@ pcl::recognition::ObjRecRANSAC::ObjRecRANSAC (float pair_width, float voxel_size max_coplanarity_angle_ (3.0f*AUX_DEG_TO_RADIANS), scene_bounds_enlargement_factor_ (0.25f), // 25% enlargement ignore_coplanar_opps_ (true), + frac_of_points_for_icp_refinement_ (0.3f), + do_icp_hypotheses_refinement_ (true), model_library_ (pair_width, voxel_size, max_coplanarity_angle_), rec_mode_ (ObjRecRANSAC::FULL_RECOGNITION) { @@ -74,6 +76,18 @@ pcl::recognition::ObjRecRANSAC::recognize (const PointCloudIn& scene, const Poin // Project it on the xy-plane (which roughly corresponds to the projection plane of the scanning device) scene_octree_proj_.build(scene_octree_, abs_zdist_thresh_, abs_zdist_thresh_); + // Needed only if icp hypotheses refinement is to be performed + scene_octree_points_ = PointCloudIn::Ptr (new PointCloudIn ()); + // First, get the scene octree points + scene_octree_.getFullLeavesPoints (*scene_octree_points_); + + if ( do_icp_hypotheses_refinement_ ) + { + // Build the ICP instance with the scene points as the target + trimmed_icp_.init (scene_octree_points_); + trimmed_icp_.setNewToOldEnergyRatio (0.99f); + } + if ( success_probability >= 1.0 ) success_probability = 0.99; @@ -101,13 +115,13 @@ pcl::recognition::ObjRecRANSAC::recognize (const PointCloudIn& scene, const Poin int num_hypotheses = this->generateHypotheses (sampled_oriented_point_pairs_, pre_hypotheses); // Cluster the hypotheses - vector*> rot_spaces; - num_hypotheses = this->groupHypotheses (pre_hypotheses, num_hypotheses, transform_space_, rot_spaces); + HypothesisOctree grouped_hypotheses; + num_hypotheses = this->groupHypotheses (pre_hypotheses, num_hypotheses, transform_space_, grouped_hypotheses); pre_hypotheses.clear (); // The last graph-based steps in the algorithm ORRGraph graph_of_close_hypotheses; - this->buildGraphOfCloseHypotheses (transform_space_, rot_spaces, graph_of_close_hypotheses); + this->buildGraphOfCloseHypotheses (grouped_hypotheses, graph_of_close_hypotheses); this->filterGraphOfCloseHypotheses (graph_of_close_hypotheses, accepted_hypotheses_); graph_of_close_hypotheses.clear (); @@ -191,7 +205,7 @@ pcl::recognition::ObjRecRANSAC::sampleOrientedPointPairs (int num_iterations, co const float *p1 = leaf1->getData ()->getPoint (); const float *n1 = leaf1->getData ()->getNormal (); - // Randomly select a leaf at the right distance from 'leaves[i]' + // Randomly select a leaf at the right distance from 'leaf1' ORROctree::Node *leaf2 = scene_octree_.getRandomFullLeafOnSphere (p1, pair_width_); if ( !leaf2 ) continue; @@ -284,8 +298,8 @@ pcl::recognition::ObjRecRANSAC::generateHypotheses (const list& hypotheses, int num_hypotheses, - RigidTransformSpace& transform_space, vector*>& rot_spaces) const +pcl::recognition::ObjRecRANSAC::groupHypotheses(list& hypotheses, int num_hypotheses, + RigidTransformSpace& transform_space, HypothesisOctree& grouped_hypotheses) const { #ifdef OBJ_REC_RANSAC_VERBOSE printf("ObjRecRANSAC::%s():\n grouping %i hypotheses ... ", __func__, num_hypotheses); fflush (stdout); @@ -298,23 +312,26 @@ pcl::recognition::ObjRecRANSAC::groupHypotheses(const list& hypo b[2] -= enlr; b[3] += enlr; b[4] -= enlr; b[5] += enlr; + // Build the output octree which will contain the grouped hypotheses + HypothesisCreator hypothesis_creator; + grouped_hypotheses.build (b, position_discretization_, &hypothesis_creator); + // Build the rigid transform space transform_space.build (b, position_discretization_, rotation_discretization_); float transformed_point[3]; - // First, add all rigid transforms to the discrete rigid transform space - for ( list::const_iterator hypo_it = hypotheses.begin () ; hypo_it != hypotheses.end () ; ++hypo_it ) + // Add all rigid transforms to the discrete rigid transform space + for ( list::iterator hypo_it = hypotheses.begin () ; hypo_it != hypotheses.end () ; ++hypo_it ) { - // First transform the center of mass of the model + // Transform the center of mass of the model aux::transform (hypo_it->rigid_transform_, hypo_it->obj_model_->getOctreeCenterOfMass (), transformed_point); + // Now add the rigid transform at the right place transform_space.addRigidTransform (hypo_it->obj_model_, transformed_point, hypo_it->rigid_transform_); } - list*>& rotation_spaces = transform_space.getRotationSpaces (); - ObjRecRANSAC::Hypothesis hypothesis; - int penalty, penalty_thresh, num_accepted = 0; - float match, match_thresh, num_full_leaves; + list& rotation_spaces = transform_space.getRotationSpaces (); + int num_accepted = 0; #ifdef OBJ_REC_RANSAC_VERBOSE printf("done\n testing the cluster representatives ...\n", __func__); fflush (stdout); @@ -324,62 +341,60 @@ pcl::recognition::ObjRecRANSAC::groupHypotheses(const list& hypo #endif // Now take the best hypothesis from each rotation space - for ( list*>::iterator rs_it = rotation_spaces.begin () ; rs_it != rotation_spaces.end () ; ++rs_it ) + for ( list::iterator rs_it = rotation_spaces.begin () ; rs_it != rotation_spaces.end () ; ++rs_it ) { - list& cells = (*rs_it)->getFullCells (); - ObjRecRANSAC::Hypothesis *best_hypothesis = NULL; + const map& models = model_library_.getModels (); + Hypothesis best_hypothesis; + best_hypothesis.match_confidence_ = 0.0f; - // Run through the cells and take the best hypothesis - for ( list::iterator cell_it = cells.begin () ; cell_it != cells.end () ; ++cell_it ) + // For each model in the library + for ( map::const_iterator model = models.begin () ; model != models.end () ; ++model ) { - // For better code readability - map& entries = (*cell_it)->getEntries (); - map::iterator entry_it; - - // Setup the ids - hypothesis.setPositionId ((*cell_it)->getPositionId ()); - hypothesis.setRotationId ((*cell_it)->getRotationId ()); + // Build a hypothesis based on the entry with most votes + Hypothesis hypothesis (model->second); - // Run through all entries - for ( entry_it = entries.begin () ; entry_it != entries.end () ; ++entry_it ) - { - // Construct a hypothesis - entry_it->second.computeAverageRigidTransform (hypothesis.rigid_transform_); - hypothesis.obj_model_ = entry_it->first; - hypothesis.explained_pixels_.clear (); + if ( !(*rs_it)->getTransformWithMostVotes (model->second, hypothesis.rigid_transform_) ) + continue; - // Now that we constructed a hypothesis -> test it - this->testHypothesis (&hypothesis, match, penalty); + int int_match; + int penalty; + this->testHypothesis (&hypothesis, int_match, penalty); - // For better code readability - num_full_leaves = static_cast (hypothesis.obj_model_->getOctree ().getFullLeaves ().size ()); - match_thresh = num_full_leaves*visibility_; - penalty_thresh = static_cast (num_full_leaves*relative_num_of_illegal_pts_ + 0.5f); + // For better code readability + float num_full_leaves = static_cast (hypothesis.obj_model_->getOctree ().getFullLeaves ().size ()); + float match_thresh = num_full_leaves*visibility_; + int penalty_thresh = static_cast (num_full_leaves*relative_num_of_illegal_pts_ + 0.5f); - // Check if this hypothesis is OK - if ( match >= match_thresh && penalty <= penalty_thresh ) + // Check if this hypothesis is OK + if ( int_match >= match_thresh && penalty <= penalty_thresh ) + { + if ( do_icp_hypotheses_refinement_ && int_match > 3 ) { - if ( best_hypothesis == NULL ) - { - best_hypothesis = new ObjRecRANSAC::Hypothesis (hypothesis); - best_hypothesis->match_confidence_ = static_cast (match)/num_full_leaves; - } - else if ( hypothesis.explained_pixels_.size () > best_hypothesis->explained_pixels_.size () ) - { - *best_hypothesis = hypothesis; - best_hypothesis->match_confidence_ = static_cast (match)/num_full_leaves; - } + // Convert from array to 4x4 matrix + Eigen::Matrix mat; + aux::array12ToMatrix4x4 (hypothesis.rigid_transform_, mat); + // Perform registration + trimmed_icp_.align ( + hypothesis.obj_model_->getPointsForRegistration (), + static_cast (static_cast (int_match)*frac_of_points_for_icp_refinement_), + mat); + aux::matrix4x4ToArray12 (mat, hypothesis.rigid_transform_); + + this->testHypothesis (&hypothesis, int_match, penalty); } + + if ( hypothesis.match_confidence_ > best_hypothesis.match_confidence_ ) + best_hypothesis = hypothesis; } } - // Save the best hypothesis in the rotation space and in the output list - if ( best_hypothesis ) + if ( best_hypothesis.match_confidence_ > 0.0f ) { - (*rs_it)->setData (*best_hypothesis); - rot_spaces.push_back (*rs_it); + const float *c = (*rs_it)->getCenter (); + HypothesisOctree::Node* node = grouped_hypotheses.createLeaf (c[0], c[1], c[2]); + + node->setData (best_hypothesis); ++num_accepted; - delete best_hypothesis; } #ifdef OBJ_REC_RANSAC_VERBOSE @@ -398,41 +413,34 @@ pcl::recognition::ObjRecRANSAC::groupHypotheses(const list& hypo //=============================================================================================================================================== void -pcl::recognition::ObjRecRANSAC::buildGraphOfCloseHypotheses (const RigidTransformSpace& transform_space, - const vector*> rotation_spaces, ORRGraph& graph) const +pcl::recognition::ObjRecRANSAC::buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph& graph) const { #ifdef OBJ_REC_RANSAC_VERBOSE printf ("ObjRecRANSAC::%s(): building the graph ... ", __func__); fflush (stdout); #endif + vector hypo_leaves = hypotheses.getFullLeaves (); int i = 0; - graph.resize (static_cast (rotation_spaces.size ())); + graph.resize (static_cast (hypo_leaves.size ())); - for ( vector*>::const_iterator rs = rotation_spaces.begin () ; rs != rotation_spaces.end () ; ++rs, ++i ) - (*rs)->setLinearId (i); + for ( vector::iterator hypo = hypo_leaves.begin () ; hypo != hypo_leaves.end () ; ++hypo, ++i ) + (*hypo)->getData ().setLinearId (i); i = 0; // Now create the graph connectivity such that each two neighboring rotation spaces are neighbors in the graph - for ( vector*>::const_iterator rs = rotation_spaces.begin () ; rs != rotation_spaces.end () ; ++rs, ++i ) + for ( vector::const_iterator hypo = hypo_leaves.begin () ; hypo != hypo_leaves.end () ; ++hypo, ++i ) { // Compute the fitness of the graph node - graph.getNodes ()[i]->setFitness (static_cast ((*rs)->getData ().explained_pixels_.size ())); - graph.getNodes ()[i]->setData ((*rs)->getData ()); + graph.getNodes ()[i]->setFitness (static_cast ((*hypo)->getData ().explained_pixels_.size ())); + graph.getNodes ()[i]->setData ((*hypo)->getData ()); // Get the neighbors of the current rotation space - list*> neighbors; - transform_space.getNeighbors ((*rs)->getPositionId (), neighbors); + const set& neighbors = (*hypo)->getNeighbors (); - for ( list*>::iterator n = neighbors.begin() ; n != neighbors.end() ; ++n ) - { - // Check if that neighbor has a valid hypothesis - if ( (*n)->getData ().match_confidence_ < 0.0 ) - continue; - - graph.insertDirectedEdge ((*rs)->getLinearId (), (*n)->getLinearId ()); - } + for ( set::const_iterator n = neighbors.begin() ; n != neighbors.end() ; ++n ) + graph.insertDirectedEdge ((*hypo)->getData ().getLinearId (), (*n)->getData ().getLinearId ()); } #ifdef OBJ_REC_RANSAC_VERBOSE @@ -604,3 +612,99 @@ pcl::recognition::ObjRecRANSAC::filterGraphOfConflictingHypotheses (ORRGraph& full_model_leaves = hypothesis->obj_model_->getOctree ().getFullLeaves (); + const float* rigid_transform = hypothesis->rigid_transform_; + const ORROctreeZProjection::Pixel* pixel; + float transformed_point[3]; + + // The match/penalty loop + for ( std::vector::const_iterator leaf_it = full_model_leaves.begin () ; leaf_it != full_model_leaves.end () ; ++leaf_it ) + { + // Transform the model point with the current rigid transform + aux::transform (rigid_transform, (*leaf_it)->getData ()->getPoint (), transformed_point); + + // Get the pixel 'transformed_point' lies in + pixel = scene_octree_proj_.getPixel (transformed_point); + // Check if we have a valid pixel + if ( pixel == NULL ) + continue; + + if ( transformed_point[2] < pixel->z1 () ) // The transformed model point overshadows a pixel -> penalize the hypothesis + ++penalty; + else if ( transformed_point[2] <= pixel->z2 () ) // The point is OK + { + ++match; + // 'hypo_it' explains 'pixel' => insert the pixel's id in the id set of 'hypo_it' + hypothesis->explained_pixels_.insert (pixel->getId ()); + } + } + + hypothesis->match_confidence_ = static_cast (match)/static_cast (hypothesis->obj_model_->getOctree ().getFullLeaves ().size ()); +} + +//=============================================================================================================================================== + +inline void +pcl::recognition::ObjRecRANSAC::testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const +{ + match = 0.0f; + + // For better code readability + const std::vector& full_model_leaves = hypothesis->obj_model_->getOctree ().getFullLeaves (); + const float* rigid_transform = hypothesis->rigid_transform_; + float transformed_point[3]; + + // The match/penalty loop + for ( std::vector::const_iterator leaf_it = full_model_leaves.begin () ; leaf_it != full_model_leaves.end () ; ++leaf_it ) + { + // Transform the model point with the current rigid transform + aux::transform (rigid_transform, (*leaf_it)->getData ()->getPoint (), transformed_point); + + // Get the pixel 'transformed_point' lies in + const ORROctreeZProjection::Pixel* pixel = scene_octree_proj_.getPixel (transformed_point); + // Check if we have a valid pixel + if ( pixel == NULL ) + continue; + + // Check if the point is OK + if ( pixel->z1 () <= transformed_point[2] && transformed_point[2] <= pixel->z2 () ) + { + // 'hypo_it' explains 'pixel' => insert the pixel's id in the id set of 'hypo_it' + hypothesis->explained_pixels_.insert (pixel->getId ()); + + // Compute the match based on the normal agreement + const set* nodes = scene_octree_proj_.getOctreeNodes (transformed_point); + + set::const_iterator n = nodes->begin (); + ORROctree::Node *closest_node = *n; + float sqr_dist, min_sqr_dist = aux::sqrDistance3 (closest_node->getData ()->getPoint (), transformed_point); + + for ( ++n ; n != nodes->end () ; ++n ) + { + sqr_dist = aux::sqrDistance3 ((*n)->getData ()->getPoint (), transformed_point); + if ( sqr_dist < min_sqr_dist ) + { + closest_node = *n; + min_sqr_dist = sqr_dist; + } + } + + float rotated_normal[3]; + aux::mult3x3 (rigid_transform, closest_node->getData ()->getNormal (), rotated_normal); + + match += aux::dot3 (rotated_normal, (*leaf_it)->getData ()->getNormal ()); + } + } + + hypothesis->match_confidence_ = match/static_cast (hypothesis->obj_model_->getOctree ().getFullLeaves ().size ()); +} + +//=============================================================================================================================================== diff --git a/recognition/src/ransac_based/orr_octree.cpp b/recognition/src/ransac_based/orr_octree.cpp index 36054f792c4..a9f0db480c6 100644 --- a/recognition/src/ransac_based/orr_octree.cpp +++ b/recognition/src/ransac_based/orr_octree.cpp @@ -413,7 +413,7 @@ pcl::recognition::ORROctree::deleteBranch (Node* node) //================================================================================================================================================================ void -pcl::recognition::ORROctree::getFullLeafPoints (PointCloudOut& out) const +pcl::recognition::ORROctree::getFullLeavesPoints (PointCloudOut& out) const { out.resize(full_leaves_.size ()); size_t i = 0; diff --git a/tools/obj_rec_ransac_accepted_hypotheses.cpp b/tools/obj_rec_ransac_accepted_hypotheses.cpp index 09c59a2208c..d12b05979a8 100644 --- a/tools/obj_rec_ransac_accepted_hypotheses.cpp +++ b/tools/obj_rec_ransac_accepted_hypotheses.cpp @@ -72,7 +72,6 @@ vtk_to_pointcloud (const char* file_name, PointCloud& pcl_points, Poin //#define _SHOW_SCENE_POINTS_ #define _SHOW_OCTREE_POINTS_ -#define _SHOW_TRANSFORM_SPACE_ //#define _SHOW_SCENE_OPPS_ //#define _SHOW_OCTREE_NORMALS_ @@ -156,7 +155,7 @@ vtk_to_pointcloud (const char* file_name, PointCloud& pcl_points, Poin //=============================================================================================================================== void -showHypothesisAsCoordinateFrame (ObjRecRANSAC::Hypothesis& hypo, CallbackParameters* parameters, string frame_name) +showHypothesisAsCoordinateFrame (Hypothesis& hypo, CallbackParameters* parameters, string frame_name) { float rot_col[3], x_dir[3], y_dir[3], z_dir[3], origin[3], scale = 2.0f*parameters->objrec_.getPairWidth (); pcl::ModelCoefficients coeffs; coeffs.values.resize (6); @@ -205,7 +204,7 @@ showHypothesisAsCoordinateFrame (ObjRecRANSAC::Hypothesis& hypo, CallbackParamet //=============================================================================================================================== bool -compareHypotheses (const ObjRecRANSAC::Hypothesis& a, const ObjRecRANSAC::Hypothesis& b) +compareHypotheses (const Hypothesis& a, const Hypothesis& b) { return (static_cast (a.match_confidence_ > b.match_confidence_)); } @@ -292,14 +291,8 @@ update (CallbackParameters* params) params->viz_.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 0.0, normals_str_id); #endif -#ifdef _SHOW_TRANSFORM_SPACE_ - const RigidTransformSpace &transform_space = params->objrec_.getRigidTransformSpace (); - char pos_cell_name[256]; - float cb[6]; -#endif - // Now show some of the accepted hypotheses - vector accepted_hypotheses; + vector accepted_hypotheses; params->objrec_.getAcceptedHypotheses (accepted_hypotheses); int i = 0; @@ -307,7 +300,7 @@ update (CallbackParameters* params) std::sort(accepted_hypotheses.begin (), accepted_hypotheses.end (), compareHypotheses); // Show the hypotheses - for ( vector::iterator acc_hypo = accepted_hypotheses.begin () ; i < params->num_hypotheses_to_show_ && acc_hypo != accepted_hypotheses.end () ; ++i, ++acc_hypo ) + for ( vector::iterator acc_hypo = accepted_hypotheses.begin () ; i < params->num_hypotheses_to_show_ && acc_hypo != accepted_hypotheses.end () ; ++i, ++acc_hypo ) { // Visualize the orientation as a tripod char frame_name[128]; @@ -343,23 +336,6 @@ update (CallbackParameters* params) // Compose the model's id cout << acc_hypo->obj_model_->getObjectName () << "_" << i+1 << " has a confidence value of " << acc_hypo->match_confidence_ << "; "; - printf ("t_id = [%i, %i, %i], rot_id = [%i, %i, %i]\n", - acc_hypo->pos_id_[0], - acc_hypo->pos_id_[1], - acc_hypo->pos_id_[2], - acc_hypo->rot_id_[0], - acc_hypo->rot_id_[1], - acc_hypo->rot_id_[2]); - -#ifdef _SHOW_TRANSFORM_SPACE_ - if ( transform_space.getPositionCellBounds (acc_hypo->pos_id_, cb) ) - { - sprintf (pos_cell_name, "cell [%i, %i, %i]\n", acc_hypo->pos_id_[0], acc_hypo->pos_id_[1], acc_hypo->pos_id_[2]); - params->viz_.addCube (cb[0], cb[1], cb[2], cb[3], cb[4], cb[5], 1.0, 1.0, 1.0, pos_cell_name); - } - else - printf ("WARNING: no cell at position [%i, %i, %i]\n", acc_hypo->pos_id_[0], acc_hypo->pos_id_[1], acc_hypo->pos_id_[2]); -#endif } // Show the bounds of the scene octree @@ -461,7 +437,7 @@ run (float pair_width, float voxel_size, float max_coplanarity_angle, int num_hy #ifdef _SHOW_OCTREE_POINTS_ PointCloud::Ptr octree_points (new PointCloud ()); - objrec.getSceneOctree ().getFullLeafPoints (*octree_points); + objrec.getSceneOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); diff --git a/tools/obj_rec_ransac_model_opps.cpp b/tools/obj_rec_ransac_model_opps.cpp index f6c9796b4b6..d056b18a0c1 100644 --- a/tools/obj_rec_ransac_model_opps.cpp +++ b/tools/obj_rec_ransac_model_opps.cpp @@ -141,7 +141,7 @@ void run (float pair_width, float voxel_size, float max_coplanarity_angle) #ifdef _SHOW_MODEL_OCTREE_POINTS_ PointCloud::Ptr octree_points (new PointCloud ()); - model->getOctree ().getFullLeafPoints (*octree_points); + model->getOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); diff --git a/tools/obj_rec_ransac_orr_octree.cpp b/tools/obj_rec_ransac_orr_octree.cpp index 9e9dd38fd5f..77d43f95b08 100644 --- a/tools/obj_rec_ransac_orr_octree.cpp +++ b/tools/obj_rec_ransac_orr_octree.cpp @@ -205,7 +205,7 @@ void run (const char* file_name, float voxel_size) std::vector::iterator leaf = octree.getFullLeaves ().begin (); // Get the average points in every full octree leaf - octree.getFullLeafPoints (*points_out); + octree.getFullLeavesPoints (*points_out); // Get the average normal at the points in each leaf if ( normals_in->size () ) octree.getNormalsOfFullLeaves (*normals_out); diff --git a/tools/obj_rec_ransac_orr_octree_zprojection.cpp b/tools/obj_rec_ransac_orr_octree_zprojection.cpp index 64f7fb930e6..bd6d5e8f5ae 100644 --- a/tools/obj_rec_ransac_orr_octree_zprojection.cpp +++ b/tools/obj_rec_ransac_orr_octree_zprojection.cpp @@ -236,8 +236,8 @@ void show_octree_zproj (ORROctreeZProjection* zproj, PCLVisualizer& viz) if ( !pixel ) continue; - rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z1_, lower_bound); - rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z2_, upper_bound); + rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z1 (), lower_bound); + rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z2 (), upper_bound); } } diff --git a/tools/obj_rec_ransac_result.cpp b/tools/obj_rec_ransac_result.cpp index 9e44939b52a..c3e49c7888d 100644 --- a/tools/obj_rec_ransac_result.cpp +++ b/tools/obj_rec_ransac_result.cpp @@ -39,7 +39,7 @@ * Created on: Jan 23, 2013 * Author: papazov * - * Visualizes the result of the ObjRecRANSAC class. STILL WORK IN PROGRESS!! + * Visualizes the result of the ObjRecRANSAC class. */ #include @@ -101,8 +101,6 @@ class CallbackParameters int main (int argc, char** argv) { - // THIS IS STILL WORK IN PROGRESS!! - printf ("\nUsage: ./pcl_obj_rec_ransac_scene_opps \n\n"); const int num_params = 3; @@ -198,7 +196,7 @@ run (float pair_width, float voxel_size, float max_coplanarity_angle) #ifdef _SHOW_OCTREE_POINTS_ PointCloud::Ptr octree_points (new PointCloud ()); - objrec.getSceneOctree ().getFullLeafPoints (*octree_points); + objrec.getSceneOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); diff --git a/tools/obj_rec_ransac_scene_opps.cpp b/tools/obj_rec_ransac_scene_opps.cpp index b20b289482b..7330104340c 100644 --- a/tools/obj_rec_ransac_scene_opps.cpp +++ b/tools/obj_rec_ransac_scene_opps.cpp @@ -222,7 +222,7 @@ void run (float pair_width, float voxel_size, float max_coplanarity_angle) #ifdef _SHOW_OCTREE_POINTS_ PointCloud::Ptr octree_points (new PointCloud ()); - objrec.getSceneOctree ().getFullLeafPoints (*octree_points); + objrec.getSceneOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); From 2daf68d88390648a82ff6149988dff10d5848f06 Mon Sep 17 00:00:00 2001 From: Chavdar Papazov Date: Tue, 12 Mar 2013 19:41:13 +0000 Subject: [PATCH 2/2] Forgot some files git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@8781 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- .../impl/ransac_based/simple_octree.hpp | 403 ++++++++++++++++++ .../pcl/recognition/ransac_based/hypothesis.h | 160 +++++++ .../recognition/ransac_based/simple_octree.h | 211 +++++++++ .../recognition/ransac_based/trimmed_icp.h | 187 ++++++++ 4 files changed, 961 insertions(+) create mode 100644 recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp create mode 100644 recognition/include/pcl/recognition/ransac_based/hypothesis.h create mode 100644 recognition/include/pcl/recognition/ransac_based/simple_octree.h create mode 100644 recognition/include/pcl/recognition/ransac_based/trimmed_icp.h diff --git a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp new file mode 100644 index 00000000000..2068a6fc24b --- /dev/null +++ b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp @@ -0,0 +1,403 @@ +/* + * simple_octree.hpp + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#ifndef SIMPLE_OCTREE_HPP_ +#define SIMPLE_OCTREE_HPP_ + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::Node () +: data_ (0), + parent_ (0), + children_(0) +{} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::~Node () +{ + this->deleteChildren (); + this->deleteData (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setCenter (const Scalar *c) +{ + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setBounds (const Scalar *b) +{ + bounds_[0] = b[0]; + bounds_[1] = b[1]; + bounds_[2] = b[2]; + bounds_[3] = b[3]; + bounds_[4] = b[4]; + bounds_[5] = b[5]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::computeRadius () +{ + Scalar v[3] = {static_cast (0.5)*(bounds_[1]-bounds_[0]), + static_cast (0.5)*(bounds_[3]-bounds_[2]), + static_cast (0.5)*(bounds_[5]-bounds_[4])}; + + radius_ = static_cast (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); +} + +//=============================================================================================================================== + +template inline bool +pcl::recognition::SimpleOctree::Node::createChildren () +{ + if ( children_ ) + return (false); + + Scalar bounds[6], center[3], childside = static_cast (0.5)*(bounds_[1]-bounds_[0]); + children_ = new Node[8]; + + // Compute bounds and center for child 0, i.e., for (0,0,0) + bounds[0] = bounds_[0]; bounds[1] = center_[0]; + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Compute the center of the new child + center[0] = 0.5f*(bounds[0] + bounds[1]); + center[1] = 0.5f*(bounds[2] + bounds[3]); + center[2] = 0.5f*(bounds[4] + bounds[5]); + // Save the results + children_[0].setBounds(bounds); + children_[0].setCenter(center); + + // Compute bounds and center for child 1, i.e., for (0,0,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[1].setBounds(bounds); + children_[1].setCenter(center); + + // Compute bounds and center for child 3, i.e., for (0,1,1) + bounds[2] = center_[1]; bounds[3] = bounds_[3]; + // Update the center + center[1] += childside; + // Save the results + children_[3].setBounds(bounds); + children_[3].setCenter(center); + + // Compute bounds and center for child 2, i.e., for (0,1,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[2].setBounds(bounds); + children_[2].setCenter(center); + + // Compute bounds and center for child 6, i.e., for (1,1,0) + bounds[0] = center_[0]; bounds[1] = bounds_[1]; + // Update the center + center[0] += childside; + // Save the results + children_[6].setBounds(bounds); + children_[6].setCenter(center); + + // Compute bounds and center for child 7, i.e., for (1,1,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[7].setBounds(bounds); + children_[7].setCenter(center); + + // Compute bounds and center for child 5, i.e., for (1,0,1) + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + // Update the center + center[1] -= childside; + // Save the results + children_[5].setBounds(bounds); + children_[5].setCenter(center); + + // Compute bounds and center for child 4, i.e., for (1,0,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[4].setBounds(bounds); + children_[4].setCenter(center); + + for ( int i = 0 ; i < 8 ; ++i ) + { + children_[i].computeRadius(); + children_[i].setParent(this); + } + + return (true); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteChildren () +{ + if ( children_ ) + { + delete[] children_; + children_ = 0; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteData () +{ + if ( data_ ) + { + delete data_; + data_ = 0; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::makeNeighbors (Node* node) +{ + if ( !this->hasData () || !node->hasData () ) + return; + + this->full_leaf_neighbors_.insert (node); + node->full_leaf_neighbors_.insert (this); +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::SimpleOctree () +: tree_levels_ (0), + root_ (0) +{ +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::~SimpleOctree () +{ + this->clear (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::clear () +{ + if ( root_ ) + { + delete root_; + root_ = 0; + } + + full_leaves_.clear(); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::build (const Scalar* bounds, Scalar voxel_size, + NodeDataCreator* node_data_creator) +{ + if ( voxel_size <= 0 ) + return; + + this->clear(); + + voxel_size_ = voxel_size; + node_data_creator_ = node_data_creator; + + Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]); + Scalar center[3] = {static_cast (0.5)*(bounds[0]+bounds[1]), + static_cast (0.5)*(bounds[2]+bounds[3]), + static_cast (0.5)*(bounds[4]+bounds[5])}; + + Scalar arg = extent/voxel_size; + + // Compute the number of tree levels + if ( arg > 1 ) + tree_levels_ = static_cast (ceil (log (arg)/log (2.0)) + 0.5); + else + tree_levels_ = 0; + + // Compute the number of octree levels and the bounds of the root + Scalar half_root_side = static_cast (0.5f*pow (2.0, tree_levels_)*voxel_size); + + // Determine the bounding box of the octree + bounds_[0] = center[0] - half_root_side; + bounds_[1] = center[0] + half_root_side; + bounds_[2] = center[1] - half_root_side; + bounds_[3] = center[1] + half_root_side; + bounds_[4] = center[2] - half_root_side; + bounds_[5] = center[2] + half_root_side; + + // Create and initialize the root + root_ = new SimpleOctree::Node(); + root_->setCenter(center); + root_->setBounds(bounds_); + root_->setParent(NULL); + root_->computeRadius(); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::createLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (NULL); + } + + Node* node = root_; + const Scalar *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + node->createChildren (); + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + { + node->setData (node_data_creator_->create (node)); + this->insertNeighbors (node); + full_leaves_.push_back (node); + } + + return (node); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (int i, int j, int k) +{ + Scalar offset = 0.5f*voxel_size_; + Scalar p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, + bounds_[2] + offset + static_cast (j)*voxel_size_, + bounds_[4] + offset + static_cast (k)*voxel_size_}; + + return (this->getFullLeaf (p[0], p[1], p[2])); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (NULL); + } + + Node* node = root_; + const Scalar *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + if ( !node->hasChildren () ) + return (NULL); + + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + return (NULL); + + return (node); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::insertNeighbors (Node* node) +{ + const Scalar* c = node->getCenter (); + Scalar s = static_cast (0.5)*voxel_size_; + Node *neigh; + + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); +//neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); +} + +//=============================================================================================================================== + +#endif /* SIMPLE_OCTREE_HPP_ */ diff --git a/recognition/include/pcl/recognition/ransac_based/hypothesis.h b/recognition/include/pcl/recognition/ransac_based/hypothesis.h new file mode 100644 index 00000000000..4ac961add06 --- /dev/null +++ b/recognition/include/pcl/recognition/ransac_based/hypothesis.h @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * hypothesis.h + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#ifndef PCL_RECOGNITION_HYPOTHESIS_H_ +#define PCL_RECOGNITION_HYPOTHESIS_H_ + +#include +#include + +namespace pcl +{ + namespace recognition + { + class HypothesisBase + { + public: + HypothesisBase (const ModelLibrary::Model* obj_model) + : obj_model_ (obj_model) + {} + + HypothesisBase (const ModelLibrary::Model* obj_model, const float* rigid_transform) + : obj_model_ (obj_model) + { + memcpy (rigid_transform_, rigid_transform, 12*sizeof (float)); + } + + virtual ~HypothesisBase (){} + + void + setModel (const ModelLibrary::Model* model) + { + obj_model_ = model; + } + + public: + float rigid_transform_[12]; + const ModelLibrary::Model* obj_model_; + }; + + class Hypothesis: public HypothesisBase + { + public: + Hypothesis (const ModelLibrary::Model* obj_model = NULL) + : HypothesisBase (obj_model), + match_confidence_ (-1.0f), + linear_id_ (-1) + { + } + + Hypothesis (const Hypothesis& src) + : HypothesisBase (src.obj_model_, src.rigid_transform_), + match_confidence_ (src.match_confidence_), + explained_pixels_ (src.explained_pixels_) + { + } + + virtual ~Hypothesis (){} + + const Hypothesis& + operator =(const Hypothesis& src) + { + memcpy (this->rigid_transform_, src.rigid_transform_, 12*sizeof (float)); + this->obj_model_ = src.obj_model_; + this->match_confidence_ = src.match_confidence_; + this->explained_pixels_ = src.explained_pixels_; + + return *this; + } + + void + setLinearId (int id) + { + linear_id_ = id; + } + + int + getLinearId () const + { + return (linear_id_); + } + + void + computeBounds (float bounds[6]) const + { + const float *b = obj_model_->getBoundsOfOctreePoints (); + float p[3]; + + // Initialize 'bounds' + aux::transform (rigid_transform_, b[0], b[2], b[4], p); + bounds[0] = bounds[1] = p[0]; + bounds[2] = bounds[3] = p[1]; + bounds[4] = bounds[5] = p[2]; + + // Expand 'bounds' to contain the other 7 points of the octree bounding box + aux::transform (rigid_transform_, b[0], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + } + + void + computeCenterOfMass (float center_of_mass[3]) const + { + aux::transform (rigid_transform_, obj_model_->getOctreeCenterOfMass (), center_of_mass); + } + + public: + float match_confidence_; + std::set explained_pixels_; + int linear_id_; + }; + } // namespace recognition +} // namespace pcl + +#endif /* PCL_RECOGNITION_HYPOTHESIS_H_ */ diff --git a/recognition/include/pcl/recognition/ransac_based/simple_octree.h b/recognition/include/pcl/recognition/ransac_based/simple_octree.h new file mode 100644 index 00000000000..db42ced091e --- /dev/null +++ b/recognition/include/pcl/recognition/ransac_based/simple_octree.h @@ -0,0 +1,211 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * simple_octree.h + * + * Created on: Mar 11, 2013 + * Author: papazov + */ + +#ifndef SIMPLE_OCTREE_H_ +#define SIMPLE_OCTREE_H_ + +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS SimpleOctree + { + public: + class Node + { + public: + Node (); + + virtual~ Node (); + + inline void + setCenter (const Scalar *c); + + inline void + setBounds (const Scalar *b); + + inline const Scalar* + getCenter () const { return center_;} + + inline const Scalar* + getBounds () const { return bounds_;} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Node* + getChild (int id) { return &children_[id];} + + inline Node* + getChildren () { return children_;} + + inline void + setData (const NodeData& src){ *data_ = src;} + + inline NodeData& + getData (){ return *data_;} + + inline const NodeData& + getData () const { return *data_;} + + inline Node* + getParent (){ return parent_;} + + inline float + getRadius () const { return radius_;} + + inline bool + hasData (){ return static_cast (data_);} + + inline bool + hasChildren (){ return static_cast (children_);} + + inline const std::set& + getNeighbors () const { return (full_leaf_neighbors_);} + + inline void + deleteChildren (); + + inline void + deleteData (); + + friend class SimpleOctree; + + protected: + void + setData (NodeData* data){ if ( data_ ) delete data_; data_ = data;} + + inline bool + createChildren (); + + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node); + + inline void + setParent (Node* parent){ parent_ = parent;} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline void + computeRadius (); + + protected: + NodeData *data_; + Scalar center_[3], bounds_[6]; + Node *parent_, *children_; + Scalar radius_; + std::set full_leaf_neighbors_; + }; + + public: + SimpleOctree (); + + virtual ~SimpleOctree (); + + void + clear (); + + /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf + * size equal to 'voxel_size'. */ + void + build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator); + + /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within + * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method + * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and + * method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data + * object. */ + inline Node* + createLeaf (Scalar x, Scalar y, Scalar z); + + /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full + * leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */ + inline Node* + getFullLeaf (int i, int j, int k); + + /** \brief Returns a pointer to the full leaf, i.e., one having a data pbject, containing p = (x, y, z) or NULL if no such leaf exists. */ + inline Node* + getFullLeaf (Scalar x, Scalar y, Scalar z); + + inline std::vector& + getFullLeaves () { return full_leaves_;} + + inline const std::vector& + getFullLeaves () const { return full_leaves_;} + + inline Node* + getRoot (){ return root_;} + + inline const Scalar* + getBounds () const { return (bounds_);} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Scalar + getVoxelSize () const { return voxel_size_;} + + protected: + inline void + insertNeighbors (Node* node); + + protected: + Scalar voxel_size_, bounds_[6]; + int tree_levels_; + Node* root_; + std::vector full_leaves_; + NodeDataCreator* node_data_creator_; + }; + } // namespace recognition +} // namespace pcl + +#include + +#endif /* SIMPLE_OCTREE_H_ */ diff --git a/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h b/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h new file mode 100644 index 00000000000..e07bab0e5b0 --- /dev/null +++ b/recognition/include/pcl/recognition/ransac_based/trimmed_icp.h @@ -0,0 +1,187 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +/* + * trimmed_icp.h + * + * Created on: Mar 10, 2013 + * Author: papazov + */ + +#ifndef TRIMMED_ICP_H_ +#define TRIMMED_ICP_H_ + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD + { + public: + typedef pcl::PointCloud PointCloud; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename Eigen::Matrix Matrix4; + + public: + TrimmedICP () + : new_to_old_energy_ratio_ (0.99f) + {} + + virtual ~TrimmedICP () + {} + + /** \brief Call this method before calling align(). + * + * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search. + * The source point cloud will be registered to 'target' (see align() method). + * */ + inline void + init (const PointCloudConstPtr& target) + { + target_points_ = target; + kdtree_.setInputCloud (target); + } + + /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method). + * + * \param[in] source_points is the point cloud to be registered to the target. + * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest + * source points we mean the source points closest to the target. These points are computed anew at each iteration. + * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess + * for the alignment. If there is no guess, set the matrix to identity! + * */ + inline void + align (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const + { + int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast (source_points.size ()); + + if ( num_trimmed_source_points >= num_source_points ) + { + printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to " + "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set " + "the number of source points to use to a lower value or use standard ICP.\n", __func__); + num_trimmed_source_points = num_source_points; + } + + // These are vectors containing source to target correspondences + pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points); + + // Some variables for the closest point search + pcl::PointXYZ transformed_source_point; + std::vector target_index (1); + std::vector sqr_dist_to_target (1); + float old_energy, energy = std::numeric_limits::max (); + +// printf ("\nalign\n"); + + do + { + // Update the correspondences + for ( int i = 0 ; i < num_source_points ; ++i ) + { + // Transform the i-th source point based on the current transform matrix + aux::transform (guess_and_result, source_points.points[i], transformed_source_point); + + // Perform the closest point search + kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target); + + // Update the i-th correspondence + full_src_to_tgt[i].index_query = i; + full_src_to_tgt[i].index_match = target_index[0]; + full_src_to_tgt[i].distance = sqr_dist_to_target[0]; + } + + // Sort in ascending order according to the squared distance + std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences); + + old_energy = energy; + energy = 0.0f; + + // Now, setup the trimmed correspondences used for the transform estimation + for ( int i = 0 ; i < num_trimmed_source_points ; ++i ) + { + trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query; + trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match; + energy += full_src_to_tgt[i].distance; + } + + this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result); + +// printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy); + } + while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress + +// printf ("\n"); + } + + inline void + setNewToOldEnergyRatio (float ratio) + { + if ( ratio >= 1 ) + new_to_old_energy_ratio_ = 0.99f; + else + new_to_old_energy_ratio_ = ratio; + } + + protected: + static inline bool + compareCorrespondences (const pcl::Correspondence& a, const pcl::Correspondence& b) + { + return static_cast (a.distance < b.distance); + } + + protected: + PointCloudConstPtr target_points_; + pcl::KdTreeFLANN kdtree_; + float new_to_old_energy_ratio_; + }; + } // namespace recognition +} // namespace pcl + + +#endif /* TRIMMED_ICP_H_ */