Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: cvfh.hpp 3022 2011-11-01 03:42:22Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_CVFH_H_ 00039 #define PCL_FEATURES_IMPL_CVFH_H_ 00040 00041 #include "pcl/features/cvfh.h" 00042 #include "pcl/features/pfh.h" 00043 00045 template<typename PointInT, typename PointNT, typename PointOutT> 00046 void 00047 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth ( 00048 const pcl::PointCloud< 00049 pcl::PointNormal> &cloud, 00050 const pcl::PointCloud< 00051 pcl::PointNormal> &normals, 00052 float tolerance, 00053 const pcl::search::Search<pcl::PointNormal>::Ptr &tree, 00054 std::vector<pcl::PointIndices> &clusters, 00055 double eps_angle, 00056 unsigned int min_pts_per_cluster, 00057 unsigned int max_pts_per_cluster) 00058 { 00059 if (tree->getInputCloud ()->points.size () != cloud.points.size ()) 00060 { 00061 PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", (unsigned long)tree->getInputCloud ()->points.size (), (unsigned long)cloud.points.size ()); 00062 return; 00063 } 00064 if (cloud.points.size () != normals.points.size ()) 00065 { 00066 PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", (unsigned long)cloud.points.size (), (unsigned long)normals.points.size ()); 00067 return; 00068 } 00069 00070 // Create a bool vector of processed point indices, and initialize it to false 00071 std::vector<bool> processed (cloud.points.size (), false); 00072 00073 std::vector<int> nn_indices; 00074 std::vector<float> nn_distances; 00075 // Process all points in the indices vector 00076 for (size_t i = 0; i < cloud.points.size (); ++i) 00077 { 00078 if (processed[i]) 00079 continue; 00080 00081 std::vector<unsigned int> seed_queue; 00082 int sq_idx = 0; 00083 seed_queue.push_back (i); 00084 00085 processed[i] = true; 00086 00087 while (sq_idx < (int)seed_queue.size ()) 00088 { 00089 // Search for sq_idx 00090 if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) 00091 { 00092 sq_idx++; 00093 continue; 00094 } 00095 00096 for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx 00097 { 00098 if (processed[nn_indices[j]]) // Has this point been processed before ? 00099 continue; 00100 00101 //processed[nn_indices[j]] = true; 00102 // [-1;1] 00103 00104 double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] 00105 + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] 00106 + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2]; 00107 00108 if (fabs (acos (dot_p)) < eps_angle) 00109 { 00110 processed[nn_indices[j]] = true; 00111 seed_queue.push_back (nn_indices[j]); 00112 } 00113 } 00114 00115 sq_idx++; 00116 } 00117 00118 // If this queue is satisfactory, add to the clusters 00119 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) 00120 { 00121 pcl::PointIndices r; 00122 r.indices.resize (seed_queue.size ()); 00123 for (size_t j = 0; j < seed_queue.size (); ++j) 00124 r.indices[j] = seed_queue[j]; 00125 00126 std::sort (r.indices.begin (), r.indices.end ()); 00127 r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); 00128 00129 r.header = cloud.header; 00130 clusters.push_back (r); // We could avoid a copy by working directly in the vector 00131 } 00132 } 00133 } 00134 00136 template<typename PointInT, typename PointNT, typename PointOutT> 00137 void 00138 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature ( 00139 const pcl::PointCloud<PointNT> & cloud, 00140 std::vector<int> & indices_out, 00141 std::vector<int> & indices_in, 00142 float threshold) 00143 { 00144 indices_out.resize (cloud.points.size ()); 00145 indices_in.resize (cloud.points.size ()); 00146 00147 size_t in, out; 00148 in = out = 0; 00149 00150 for (size_t i = 0; i < cloud.points.size (); i++) 00151 { 00152 if (cloud.points[i].curvature > threshold) 00153 { 00154 indices_out[out] = i; 00155 out++; 00156 } 00157 else 00158 { 00159 indices_in[in] = i; 00160 in++; 00161 } 00162 } 00163 00164 indices_out.resize (out); 00165 indices_in.resize (in); 00166 } 00167 00169 template<typename PointInT, typename PointNT, typename PointOutT> 00170 void 00171 pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00172 { 00173 // Check if input was set 00174 if (!normals_) 00175 { 00176 PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); 00177 output.width = output.height = 0; 00178 output.points.clear (); 00179 return; 00180 } 00181 if (normals_->points.size () != surface_->points.size ()) 00182 { 00183 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 ()); 00184 output.width = output.height = 0; 00185 output.points.clear (); 00186 return; 00187 } 00188 00189 centroids_dominant_orientations_.clear (); 00190 00191 // ---[ Step 0: remove normals with high curvature 00192 float curv_threshold = curv_threshold_; 00193 std::vector<int> indices_out; 00194 std::vector<int> indices_in; 00195 filterNormalsWithHighCurvature (*normals_, indices_out, indices_in, curv_threshold); 00196 00197 pcl::PointCloud<pcl::PointNormal>::Ptr filtered (new pcl::PointCloud<pcl::PointNormal> ()); 00198 00199 filtered->width = indices_in.size (); 00200 filtered->height = 1; 00201 filtered->points.resize (filtered->width); 00202 00203 for (size_t i = 0; i < indices_in.size (); ++i) 00204 { 00205 filtered->points[i].x = surface_->points[indices_in[i]].x; 00206 filtered->points[i].y = surface_->points[indices_in[i]].y; 00207 filtered->points[i].z = surface_->points[indices_in[i]].z; 00208 00209 filtered->points[i].normal[0] = normals_->points[indices_in[i]].normal[0]; 00210 filtered->points[i].normal[1] = normals_->points[indices_in[i]].normal[1]; 00211 filtered->points[i].normal[2] = normals_->points[indices_in[i]].normal[2]; 00212 } 00213 00214 // ---[ Step 1a : compute clustering 00215 pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ()); 00216 if (indices_in.size () >= 100) //TODO: parameter 00217 { 00218 normals_filtered_cloud->width = indices_in.size (); 00219 normals_filtered_cloud->height = 1; 00220 normals_filtered_cloud->points.resize (normals_filtered_cloud->width); 00221 00222 for (size_t i = 0; i < indices_in.size (); ++i) 00223 { 00224 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x; 00225 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y; 00226 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z; 00227 00228 normals_filtered_cloud->points[i].normal[0] = normals_->points[indices_in[i]].normal[0]; 00229 normals_filtered_cloud->points[i].normal[1] = normals_->points[indices_in[i]].normal[1]; 00230 normals_filtered_cloud->points[i].normal[2] = normals_->points[indices_in[i]].normal[2]; 00231 } 00232 } 00233 else 00234 { 00235 normals_filtered_cloud->width = surface_->size (); 00236 normals_filtered_cloud->height = 1; 00237 normals_filtered_cloud->points.resize (normals_filtered_cloud->width); 00238 00239 for (size_t i = 0; i < surface_->size (); ++i) 00240 { 00241 normals_filtered_cloud->points[i].x = surface_->points[i].x; 00242 normals_filtered_cloud->points[i].y = surface_->points[i].y; 00243 normals_filtered_cloud->points[i].z = surface_->points[i].z; 00244 00245 normals_filtered_cloud->points[i].normal[0] = normals_->points[i].normal[0]; 00246 normals_filtered_cloud->points[i].normal[1] = normals_->points[i].normal[1]; 00247 normals_filtered_cloud->points[i].normal[2] = normals_->points[i].normal[2]; 00248 } 00249 } 00250 00251 //recompute normals normals and use them for clustering! 00252 KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false)); 00253 normals_tree_filtered->setInputCloud (normals_filtered_cloud); 00254 00255 NormalEstimator n3d; 00256 n3d.setRadiusSearch (radius_normals_); 00257 n3d.setSearchMethod (normals_tree_filtered); 00258 n3d.setInputCloud (normals_filtered_cloud); 00259 n3d.compute (*normals_filtered_cloud); 00260 00261 KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false)); 00262 normals_tree->setInputCloud (normals_filtered_cloud); 00263 std::vector<pcl::PointIndices> clusters; 00264 00265 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, 00266 clusters, eps_angle_threshold_, min_points_); 00267 00268 /*clusters_colored_.reset (new pcl::PointCloud<pcl::PointXYZINormal> ()); 00269 clusters_colored_->points.resize(normals_filtered_cloud->points.size()); 00270 clusters_colored_->width = clusters_colored_->points.size(); 00271 clusters_colored_->height = 1; 00272 00273 float intensity = 0.1; 00274 int n_points=0; 00275 for (size_t i = 0; i < clusters.size (); ++i) //for each cluster 00276 { 00277 for (size_t j = 0; j < clusters[i].indices.size (); j++) 00278 { 00279 clusters_colored_->points[n_points].intensity = intensity; 00280 clusters_colored_->points[n_points].getVector4fMap() = normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap(); 00281 clusters_colored_->points[n_points].getNormalVector4fMap() = normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap(); 00282 clusters_colored_->points[n_points].curvature = normals_filtered_cloud->points[clusters[i].indices[j]].curvature; 00283 00284 n_points++; 00285 } 00286 00287 intensity += 0.1; 00288 } 00289 00290 clusters_colored_->points.resize(n_points); 00291 clusters_colored_->width = n_points;*/ 00292 00293 std::vector<Eigen::Vector3f> dominant_normals; 00294 00295 VFHEstimator vfh; 00296 vfh.setInputCloud (surface_); 00297 vfh.setInputNormals (normals_); 00298 vfh.setSearchMethod (this->tree_); 00299 vfh.setUseGivenNormal (true); 00300 vfh.setUseGivenCentroid (true); 00301 vfh.setNormalizeBins (normalize_bins_); 00302 vfh.setNormalizeDistance (true); 00303 vfh.setFillSizeComponent (true); 00304 output.height = 1; 00305 00306 // ---[ Step 1b : check if any dominant cluster was found 00307 if (clusters.size () > 0) 00308 { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information 00309 00310 for (size_t i = 0; i < clusters.size (); ++i) //for each cluster 00311 { 00312 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero (); 00313 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero (); 00314 00315 for (size_t j = 0; j < clusters[i].indices.size (); j++) 00316 { 00317 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap (); 00318 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap (); 00319 } 00320 00321 avg_normal /= clusters[i].indices.size (); 00322 avg_centroid /= clusters[i].indices.size (); 00323 00324 Eigen::Vector4f centroid_test; 00325 pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test); 00326 avg_normal.normalize (); 00327 00328 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]); 00329 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); 00330 00331 //append normal and centroid for the clusters 00332 dominant_normals.push_back (avg_norm); 00333 centroids_dominant_orientations_.push_back (avg_dominant_centroid); 00334 } 00335 00336 //compute modified VFH for all dominant clusters and add them to the list! 00337 output.points.resize (dominant_normals.size ()); 00338 output.width = dominant_normals.size (); 00339 00340 for (size_t i = 0; i < dominant_normals.size (); ++i) 00341 { 00342 //configure VFH computation for CVFH 00343 vfh.setNormalToUse (dominant_normals[i]); 00344 vfh.setCentroidToUse (centroids_dominant_orientations_[i]); 00345 pcl::PointCloud<pcl::VFHSignature308> vfh_signature; 00346 vfh.compute (vfh_signature); 00347 output.points[i] = vfh_signature.points[0]; 00348 } 00349 } 00350 else 00351 { // ---[ Step 1b.1 : If no, compute CVFH using all the object points 00352 Eigen::Vector4f avg_centroid; 00353 pcl::compute3DCentroid (*surface_, avg_centroid); 00354 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); 00355 centroids_dominant_orientations_.push_back (cloud_centroid); 00356 00357 //configure VFH computation for CVFH using all object points 00358 vfh.setCentroidToUse (cloud_centroid); 00359 vfh.setUseGivenNormal (false); 00360 00361 pcl::PointCloud<pcl::VFHSignature308> vfh_signature; 00362 vfh.compute (vfh_signature); 00363 00364 output.points.resize (1); 00365 output.width = 1; 00366 00367 output.points[0] = vfh_signature.points[0]; 00368 } 00369 } 00370 00371 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>; 00372 00373 #endif // PCL_FEATURES_IMPL_VFH_H_