Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id: cvfh.h 4936 2012-03-07 11:12:45Z aaldoma $ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_OURCVFH_H_ 00042 #define PCL_FEATURES_OURCVFH_H_ 00043 00044 #include <pcl/features/feature.h> 00045 #include <pcl/search/pcl_search.h> 00046 #include <pcl/common/common.h> 00047 00048 namespace pcl 00049 { 00050 /** \brief OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given 00051 * point cloud dataset given XYZ data and normals, as presented in: 00052 * - OUR-CVFH – Oriented, Unique and Repeatable Clustered Viewpoint Feature Histogram for Object Recognition and 6DOF Pose Estimation 00053 * A. Aldoma, F. Tombari, R.B. Rusu and M. Vincze 00054 * DAGM-OAGM 2012 00055 * Graz, Austria 00056 * The suggested PointOutT is pcl::VFHSignature308. 00057 * 00058 * \author Aitor Aldoma 00059 * \ingroup features 00060 */ 00061 template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308> 00062 class OURCVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT> 00063 { 00064 public: 00065 typedef boost::shared_ptr<OURCVFHEstimation<PointInT, PointNT, PointOutT> > Ptr; 00066 typedef boost::shared_ptr<const OURCVFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr; 00067 using Feature<PointInT, PointOutT>::feature_name_; 00068 using Feature<PointInT, PointOutT>::getClassName; 00069 using Feature<PointInT, PointOutT>::indices_; 00070 using Feature<PointInT, PointOutT>::k_; 00071 using Feature<PointInT, PointOutT>::search_radius_; 00072 using Feature<PointInT, PointOutT>::surface_; 00073 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_; 00074 00075 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00076 typedef typename pcl::search::Search<PointNormal>::Ptr KdTreePtr; 00077 typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr; 00078 /** \brief Empty constructor. */ 00079 OURCVFHEstimation () : 00080 vpx_ (0), vpy_ (0), vpz_ (0), leaf_size_ (0.005f), normalize_bins_ (false), curv_threshold_ (0.03f), cluster_tolerance_ (leaf_size_ * 3), 00081 eps_angle_threshold_ (0.125f), min_points_ (50), radius_normals_ (leaf_size_ * 3), centroids_dominant_orientations_ (), 00082 dominant_normals_ () 00083 { 00084 search_radius_ = 0; 00085 k_ = 1; 00086 feature_name_ = "OURCVFHEstimation"; 00087 refine_clusters_ = 1.f; 00088 min_axis_value_ = 0.925f; 00089 axis_ratio_ = 0.8f; 00090 } 00091 ; 00092 00093 /** \brief Creates an affine transformation from the RF axes 00094 * \param[in] evx the x-axis 00095 * \param[in] evy the z-axis 00096 * \param[in] evz the z-axis 00097 * \param[out] transformPC the resulting transformation 00098 * \param[in] center_mat 4x4 matrix concatenated to the resulting transformation 00099 */ 00100 inline Eigen::Matrix4f 00101 createTransFromAxes (Eigen::Vector3f & evx, Eigen::Vector3f & evy, Eigen::Vector3f & evz, Eigen::Affine3f & transformPC, 00102 Eigen::Matrix4f & center_mat) 00103 { 00104 Eigen::Matrix4f trans; 00105 trans.setIdentity (4, 4); 00106 trans (0, 0) = evx (0, 0); 00107 trans (1, 0) = evx (1, 0); 00108 trans (2, 0) = evx (2, 0); 00109 trans (0, 1) = evy (0, 0); 00110 trans (1, 1) = evy (1, 0); 00111 trans (2, 1) = evy (2, 0); 00112 trans (0, 2) = evz (0, 0); 00113 trans (1, 2) = evz (1, 0); 00114 trans (2, 2) = evz (2, 0); 00115 00116 Eigen::Matrix4f homMatrix = Eigen::Matrix4f (); 00117 homMatrix.setIdentity (4, 4); 00118 homMatrix = transformPC.matrix (); 00119 00120 Eigen::Matrix4f trans_copy = trans.inverse (); 00121 trans = trans_copy * center_mat * homMatrix; 00122 return trans; 00123 } 00124 00125 /** \brief Computes SGURF and the shape distribution based on the selected SGURF 00126 * \param[in] processed the input cloud 00127 * \param[out] output the resulting signature 00128 * \param[in] cluster_indices the indices of the stable cluster 00129 */ 00130 void 00131 computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut &output, std::vector<pcl::PointIndices> & cluster_indices); 00132 00133 /** \brief Computes SGURF 00134 * \param[in] centroid the centroid of the cluster 00135 * \param[in] normal_centroid the average of the normals 00136 * \param[in] processed the input cloud 00137 * \param[out] transformations the transformations aligning the cloud to the SGURF axes 00138 * \param[out] grid the cloud transformed internally 00139 * \param[in] indices the indices of the stable cluster 00140 */ 00141 bool 00142 sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations, 00143 PointInTPtr & grid, pcl::PointIndices & indices); 00144 00145 /** \brief Removes normals with high curvature caused by real edges or noisy data 00146 * \param[in] cloud pointcloud to be filtered 00147 * \param[out] indices_out the indices of the points with higher curvature than threshold 00148 * \param[out] indices_in the indices of the remaining points after filtering 00149 * \param[in] threshold threshold value for curvature 00150 */ 00151 void 00152 filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out, 00153 std::vector<int> &indices_in, float threshold); 00154 00155 /** \brief Set the viewpoint. 00156 * \param[in] vpx the X coordinate of the viewpoint 00157 * \param[in] vpy the Y coordinate of the viewpoint 00158 * \param[in] vpz the Z coordinate of the viewpoint 00159 */ 00160 inline void 00161 setViewPoint (float vpx, float vpy, float vpz) 00162 { 00163 vpx_ = vpx; 00164 vpy_ = vpy; 00165 vpz_ = vpz; 00166 } 00167 00168 /** \brief Set the radius used to compute normals 00169 * \param[in] radius_normals the radius 00170 */ 00171 inline void 00172 setRadiusNormals (float radius_normals) 00173 { 00174 radius_normals_ = radius_normals; 00175 } 00176 00177 /** \brief Get the viewpoint. 00178 * \param[out] vpx the X coordinate of the viewpoint 00179 * \param[out] vpy the Y coordinate of the viewpoint 00180 * \param[out] vpz the Z coordinate of the viewpoint 00181 */ 00182 inline void 00183 getViewPoint (float &vpx, float &vpy, float &vpz) 00184 { 00185 vpx = vpx_; 00186 vpy = vpy_; 00187 vpz = vpz_; 00188 } 00189 00190 /** \brief Get the centroids used to compute different CVFH descriptors 00191 * \param[out] centroids vector to hold the centroids 00192 */ 00193 inline void 00194 getCentroidClusters (std::vector<Eigen::Vector3f> & centroids) 00195 { 00196 for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i) 00197 centroids.push_back (centroids_dominant_orientations_[i]); 00198 } 00199 00200 /** \brief Get the normal centroids used to compute different CVFH descriptors 00201 * \param[out] centroids vector to hold the normal centroids 00202 */ 00203 inline void 00204 getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids) 00205 { 00206 for (size_t i = 0; i < dominant_normals_.size (); ++i) 00207 centroids.push_back (dominant_normals_[i]); 00208 } 00209 00210 /** \brief Sets max. Euclidean distance between points to be added to the cluster 00211 * \param[in] d the maximum Euclidean distance 00212 */ 00213 00214 inline void 00215 setClusterTolerance (float d) 00216 { 00217 cluster_tolerance_ = d; 00218 } 00219 00220 /** \brief Sets max. deviation of the normals between two points so they can be clustered together 00221 * \param[in] d the maximum deviation 00222 */ 00223 inline void 00224 setEPSAngleThreshold (float d) 00225 { 00226 eps_angle_threshold_ = d; 00227 } 00228 00229 /** \brief Sets curvature threshold for removing normals 00230 * \param[in] d the curvature threshold 00231 */ 00232 inline void 00233 setCurvatureThreshold (float d) 00234 { 00235 curv_threshold_ = d; 00236 } 00237 00238 /** \brief Set minimum amount of points for a cluster to be considered 00239 * \param[in] min the minimum amount of points to be set 00240 */ 00241 inline void 00242 setMinPoints (size_t min) 00243 { 00244 min_points_ = min; 00245 } 00246 00247 /** \brief Sets wether if the signatures should be normalized or not 00248 * \param[in] normalize true if normalization is required, false otherwise 00249 */ 00250 inline void 00251 setNormalizeBins (bool normalize) 00252 { 00253 normalize_bins_ = normalize; 00254 } 00255 00256 /** \brief Gets the indices of the original point cloud used to compute the signatures 00257 * \param[out] indices vector of point indices 00258 */ 00259 inline void 00260 getClusterIndices (std::vector<pcl::PointIndices> & indices) 00261 { 00262 indices = clusters_; 00263 } 00264 00265 /** \brief Sets the refinement factor for the clusters 00266 * \param[in] rc the factor used to decide if a point is used to estimate a stable cluster 00267 */ 00268 void 00269 setRefineClusters (float rc) 00270 { 00271 refine_clusters_ = rc; 00272 } 00273 00274 /** \brief Returns the transformations aligning the point cloud to the corresponding SGURF 00275 * \param[out] trans vector of transformations 00276 */ 00277 void 00278 getTransforms (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & trans) 00279 { 00280 trans = transforms_; 00281 } 00282 00283 /** \brief Returns a boolean vector indicating of the transformation obtained by getTransforms() represents 00284 * a valid SGURF 00285 * \param[out] valid vector of booleans 00286 */ 00287 void 00288 getValidTransformsVec (std::vector<bool> & valid) 00289 { 00290 valid = valid_transforms_; 00291 } 00292 00293 /** \brief Sets the min axis ratio between the SGURF axes to decide if disambiguition is feasible 00294 * \param[in] f the ratio between axes 00295 */ 00296 void 00297 setAxisRatio (float f) 00298 { 00299 axis_ratio_ = f; 00300 } 00301 00302 /** \brief Sets the min disambiguition axis value to generate several SGURFs for the cluster when disambiguition is difficult 00303 * \param[in] f the min axis value 00304 */ 00305 void 00306 setMinAxisValue (float f) 00307 { 00308 min_axis_value_ = f; 00309 } 00310 00311 /** \brief Overloaded computed method from pcl::Feature. 00312 * \param[out] output the resultant point cloud model dataset containing the estimated features 00313 */ 00314 void 00315 compute (PointCloudOut &output); 00316 00317 private: 00318 /** \brief Values describing the viewpoint ("pinhole" camera model assumed). 00319 * By default, the viewpoint is set to 0,0,0. 00320 */ 00321 float vpx_, vpy_, vpz_; 00322 00323 /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel 00324 * size of the training data or the normalize_bins_ flag must be set to true. 00325 */ 00326 float leaf_size_; 00327 00328 /** \brief Wether to normalize the signatures or not. Default: false. */ 00329 bool normalize_bins_; 00330 00331 /** \brief Curvature threshold for removing normals. */ 00332 float curv_threshold_; 00333 00334 /** \brief allowed Euclidean distance between points to be added to the cluster. */ 00335 float cluster_tolerance_; 00336 00337 /** \brief deviation of the normals between two points so they can be clustered together. */ 00338 float eps_angle_threshold_; 00339 00340 /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH 00341 * computation. 00342 */ 00343 size_t min_points_; 00344 00345 /** \brief Radius for the normals computation. */ 00346 float radius_normals_; 00347 00348 /** \brief Factor for the cluster refinement */ 00349 float refine_clusters_; 00350 00351 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_; 00352 std::vector<bool> valid_transforms_; 00353 00354 float axis_ratio_; 00355 float min_axis_value_; 00356 00357 /** \brief Estimate the OUR-CVFH descriptors at 00358 * a set of points given by <setInputCloud (), setIndices ()> using the surface in 00359 * setSearchSurface () 00360 * 00361 * \param[out] output the resultant point cloud model dataset that contains the OUR-CVFH 00362 * feature estimates 00363 */ 00364 void 00365 computeFeature (PointCloudOut &output); 00366 00367 /** \brief Region growing method using Euclidean distances and neighbors normals to 00368 * add points to a region. 00369 * \param[in] cloud point cloud to split into regions 00370 * \param[in] normals are the normals of cloud 00371 * \param[in] tolerance is the allowed Euclidean distance between points to be added to 00372 * the cluster 00373 * \param[in] tree is the spatial search structure for nearest neighbour search 00374 * \param[out] clusters vector of indices representing the clustered regions 00375 * \param[in] eps_angle deviation of the normals between two points so they can be 00376 * clustered together 00377 * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point) 00378 * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points) 00379 */ 00380 void 00381 extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud, const pcl::PointCloud<pcl::PointNormal> &normals, 00382 float tolerance, const pcl::search::Search<pcl::PointNormal>::Ptr &tree, 00383 std::vector<pcl::PointIndices> &clusters, double eps_angle, unsigned int min_pts_per_cluster = 1, 00384 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ()); 00385 00386 protected: 00387 /** \brief Centroids that were used to compute different OUR-CVFH descriptors */ 00388 std::vector<Eigen::Vector3f> centroids_dominant_orientations_; 00389 /** \brief Normal centroids that were used to compute different OUR-CVFH descriptors */ 00390 std::vector<Eigen::Vector3f> dominant_normals_; 00391 /** \brief Indices to the points representing the stable clusters */ 00392 std::vector<pcl::PointIndices> clusters_; 00393 }; 00394 } 00395 00396 #ifdef PCL_NO_PRECOMPILE 00397 #include <pcl/features/impl/our_cvfh.hpp> 00398 #endif 00399 00400 #endif //#ifndef PCL_FEATURES_VFH_H_