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$ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_IMPL_CVFH_H_ 00042 #define PCL_FEATURES_IMPL_CVFH_H_ 00043 00044 #include <pcl/features/cvfh.h> 00045 #include <pcl/features/normal_3d.h> 00046 #include <pcl/features/pfh_tools.h> 00047 #include <pcl/common/centroid.h> 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////// 00050 template<typename PointInT, typename PointNT, typename PointOutT> void 00051 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output) 00052 { 00053 if (!Feature<PointInT, PointOutT>::initCompute ()) 00054 { 00055 output.width = output.height = 0; 00056 output.points.clear (); 00057 return; 00058 } 00059 // Resize the output dataset 00060 // Important! We should only allocate precisely how many elements we will need, otherwise 00061 // we risk at pre-allocating too much memory which could lead to bad_alloc 00062 // (see http://dev.pointclouds.org/issues/657) 00063 output.width = output.height = 1; 00064 output.points.resize (1); 00065 00066 // Perform the actual feature computation 00067 computeFeature (output); 00068 00069 Feature<PointInT, PointOutT>::deinitCompute (); 00070 } 00071 00072 ////////////////////////////////////////////////////////////////////////////////////////////// 00073 template<typename PointInT, typename PointNT, typename PointOutT> void 00074 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth ( 00075 const pcl::PointCloud<pcl::PointNormal> &cloud, 00076 const pcl::PointCloud<pcl::PointNormal> &normals, 00077 float tolerance, 00078 const pcl::search::Search<pcl::PointNormal>::Ptr &tree, 00079 std::vector<pcl::PointIndices> &clusters, 00080 double eps_angle, 00081 unsigned int min_pts_per_cluster, 00082 unsigned int max_pts_per_cluster) 00083 { 00084 if (tree->getInputCloud ()->points.size () != cloud.points.size ()) 00085 { 00086 PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); 00087 return; 00088 } 00089 if (cloud.points.size () != normals.points.size ()) 00090 { 00091 PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ()); 00092 return; 00093 } 00094 00095 // Create a bool vector of processed point indices, and initialize it to false 00096 std::vector<bool> processed (cloud.points.size (), false); 00097 00098 std::vector<int> nn_indices; 00099 std::vector<float> nn_distances; 00100 // Process all points in the indices vector 00101 for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) 00102 { 00103 if (processed[i]) 00104 continue; 00105 00106 std::vector<unsigned int> seed_queue; 00107 int sq_idx = 0; 00108 seed_queue.push_back (i); 00109 00110 processed[i] = true; 00111 00112 while (sq_idx < static_cast<int> (seed_queue.size ())) 00113 { 00114 // Search for sq_idx 00115 if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) 00116 { 00117 sq_idx++; 00118 continue; 00119 } 00120 00121 for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx 00122 { 00123 if (processed[nn_indices[j]]) // Has this point been processed before ? 00124 continue; 00125 00126 //processed[nn_indices[j]] = true; 00127 // [-1;1] 00128 00129 double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] 00130 + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] 00131 + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2]; 00132 00133 if (fabs (acos (dot_p)) < eps_angle) 00134 { 00135 processed[nn_indices[j]] = true; 00136 seed_queue.push_back (nn_indices[j]); 00137 } 00138 } 00139 00140 sq_idx++; 00141 } 00142 00143 // If this queue is satisfactory, add to the clusters 00144 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) 00145 { 00146 pcl::PointIndices r; 00147 r.indices.resize (seed_queue.size ()); 00148 for (size_t j = 0; j < seed_queue.size (); ++j) 00149 r.indices[j] = seed_queue[j]; 00150 00151 std::sort (r.indices.begin (), r.indices.end ()); 00152 r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); 00153 00154 r.header = cloud.header; 00155 clusters.push_back (r); // We could avoid a copy by working directly in the vector 00156 } 00157 } 00158 } 00159 00160 ////////////////////////////////////////////////////////////////////////////////////////////// 00161 template<typename PointInT, typename PointNT, typename PointOutT> void 00162 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature ( 00163 const pcl::PointCloud<PointNT> & cloud, 00164 std::vector<int> &indices_to_use, 00165 std::vector<int> &indices_out, 00166 std::vector<int> &indices_in, 00167 float threshold) 00168 { 00169 indices_out.resize (cloud.points.size ()); 00170 indices_in.resize (cloud.points.size ()); 00171 00172 size_t in, out; 00173 in = out = 0; 00174 00175 for (int i = 0; i < static_cast<int> (indices_to_use.size ()); i++) 00176 { 00177 if (cloud.points[indices_to_use[i]].curvature > threshold) 00178 { 00179 indices_out[out] = indices_to_use[i]; 00180 out++; 00181 } 00182 else 00183 { 00184 indices_in[in] = indices_to_use[i]; 00185 in++; 00186 } 00187 } 00188 00189 indices_out.resize (out); 00190 indices_in.resize (in); 00191 } 00192 00193 ////////////////////////////////////////////////////////////////////////////////////////////// 00194 template<typename PointInT, typename PointNT, typename PointOutT> void 00195 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00196 { 00197 // Check if input was set 00198 if (!normals_) 00199 { 00200 PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); 00201 output.width = output.height = 0; 00202 output.points.clear (); 00203 return; 00204 } 00205 if (normals_->points.size () != surface_->points.size ()) 00206 { 00207 PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); 00208 output.width = output.height = 0; 00209 output.points.clear (); 00210 return; 00211 } 00212 00213 centroids_dominant_orientations_.clear (); 00214 00215 // ---[ Step 0: remove normals with high curvature 00216 std::vector<int> indices_out; 00217 std::vector<int> indices_in; 00218 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_); 00219 00220 pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ()); 00221 normals_filtered_cloud->width = static_cast<uint32_t> (indices_in.size ()); 00222 normals_filtered_cloud->height = 1; 00223 normals_filtered_cloud->points.resize (normals_filtered_cloud->width); 00224 00225 for (size_t i = 0; i < indices_in.size (); ++i) 00226 { 00227 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x; 00228 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y; 00229 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z; 00230 } 00231 00232 std::vector<pcl::PointIndices> clusters; 00233 00234 if(normals_filtered_cloud->points.size() >= min_points_) 00235 { 00236 //recompute normals and use them for clustering 00237 KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false)); 00238 normals_tree_filtered->setInputCloud (normals_filtered_cloud); 00239 00240 00241 pcl::NormalEstimation<PointNormal, PointNormal> n3d; 00242 n3d.setRadiusSearch (radius_normals_); 00243 n3d.setSearchMethod (normals_tree_filtered); 00244 n3d.setInputCloud (normals_filtered_cloud); 00245 n3d.compute (*normals_filtered_cloud); 00246 00247 KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false)); 00248 normals_tree->setInputCloud (normals_filtered_cloud); 00249 00250 extractEuclideanClustersSmooth (*normals_filtered_cloud, 00251 *normals_filtered_cloud, 00252 cluster_tolerance_, 00253 normals_tree, 00254 clusters, 00255 eps_angle_threshold_, 00256 static_cast<unsigned int> (min_points_)); 00257 00258 } 00259 00260 VFHEstimator vfh; 00261 vfh.setInputCloud (surface_); 00262 vfh.setInputNormals (normals_); 00263 vfh.setIndices(indices_); 00264 vfh.setSearchMethod (this->tree_); 00265 vfh.setUseGivenNormal (true); 00266 vfh.setUseGivenCentroid (true); 00267 vfh.setNormalizeBins (normalize_bins_); 00268 vfh.setNormalizeDistance (true); 00269 vfh.setFillSizeComponent (true); 00270 output.height = 1; 00271 00272 // ---[ Step 1b : check if any dominant cluster was found 00273 if (clusters.size () > 0) 00274 { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information 00275 00276 for (size_t i = 0; i < clusters.size (); ++i) //for each cluster 00277 { 00278 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero (); 00279 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero (); 00280 00281 for (size_t j = 0; j < clusters[i].indices.size (); j++) 00282 { 00283 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap (); 00284 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap (); 00285 } 00286 00287 avg_normal /= static_cast<float> (clusters[i].indices.size ()); 00288 avg_centroid /= static_cast<float> (clusters[i].indices.size ()); 00289 00290 Eigen::Vector4f centroid_test; 00291 pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test); 00292 avg_normal.normalize (); 00293 00294 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]); 00295 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); 00296 00297 //append normal and centroid for the clusters 00298 dominant_normals_.push_back (avg_norm); 00299 centroids_dominant_orientations_.push_back (avg_dominant_centroid); 00300 } 00301 00302 //compute modified VFH for all dominant clusters and add them to the list! 00303 output.points.resize (dominant_normals_.size ()); 00304 output.width = static_cast<uint32_t> (dominant_normals_.size ()); 00305 00306 for (size_t i = 0; i < dominant_normals_.size (); ++i) 00307 { 00308 //configure VFH computation for CVFH 00309 vfh.setNormalToUse (dominant_normals_[i]); 00310 vfh.setCentroidToUse (centroids_dominant_orientations_[i]); 00311 pcl::PointCloud<pcl::VFHSignature308> vfh_signature; 00312 vfh.compute (vfh_signature); 00313 output.points[i] = vfh_signature.points[0]; 00314 } 00315 } 00316 else 00317 { // ---[ Step 1b.1 : If no, compute CVFH using all the object points 00318 Eigen::Vector4f avg_centroid; 00319 pcl::compute3DCentroid (*surface_, avg_centroid); 00320 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); 00321 centroids_dominant_orientations_.push_back (cloud_centroid); 00322 00323 //configure VFH computation for CVFH using all object points 00324 vfh.setCentroidToUse (cloud_centroid); 00325 vfh.setUseGivenNormal (false); 00326 00327 pcl::PointCloud<pcl::VFHSignature308> vfh_signature; 00328 vfh.compute (vfh_signature); 00329 00330 output.points.resize (1); 00331 output.width = 1; 00332 00333 output.points[0] = vfh_signature.points[0]; 00334 } 00335 } 00336 00337 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>; 00338 00339 #endif // PCL_FEATURES_IMPL_VFH_H_