Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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 */ 00035 00036 #include <boost/thread/thread.hpp> 00037 #include <boost/date_time/posix_time/posix_time.hpp> 00038 00039 #include <pcl/apps/dominant_plane_segmentation.h> 00040 #include <pcl/visualization/pcl_visualizer.h> 00041 #include <pcl/features/integral_image_normal.h> 00042 #include <pcl/common/time.h> 00043 00044 template<typename PointType> 00045 void 00046 pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters) 00047 { 00048 // Has the input dataset been set already? 00049 if (!input_) 00050 { 00051 PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n"); 00052 return; 00053 } 00054 00055 // Is the input dataset organized? 00056 if (input_->is_dense) 00057 { 00058 PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n"); 00059 return; 00060 } 00061 00062 CloudConstPtr cloud_; 00063 CloudPtr cloud_filtered_ (new Cloud ()); 00064 CloudPtr cloud_downsampled_ (new Cloud ()); 00065 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ()); 00066 pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ()); 00067 pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ()); 00068 CloudPtr table_projected_ (new Cloud ()); 00069 CloudPtr table_hull_ (new Cloud ()); 00070 CloudPtr cloud_objects_ (new Cloud ()); 00071 CloudPtr cluster_object_ (new Cloud ()); 00072 00073 typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>); 00074 typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>); 00075 clusters_tree_->setEpsilon (1); 00076 00077 // Normal estimation parameters 00078 n3d_.setKSearch (k_); 00079 n3d_.setSearchMethod (normals_tree_); 00080 00081 // Table model fitting parameters 00082 seg_.setDistanceThreshold (sac_distance_threshold_); 00083 seg_.setMaxIterations (2000); 00084 seg_.setNormalDistanceWeight (0.1); 00085 seg_.setOptimizeCoefficients (true); 00086 seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00087 seg_.setMethodType (pcl::SAC_RANSAC); 00088 seg_.setProbability (0.99); 00089 00090 proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00091 bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00092 00093 prism_.setHeightLimits (object_min_height_, object_max_height_); 00094 00095 // Clustering parameters 00096 cluster_.setClusterTolerance (object_cluster_tolerance_); 00097 cluster_.setMinClusterSize (object_cluster_min_size_); 00098 cluster_.setSearchMethod (clusters_tree_); 00099 00100 // ---[ PassThroughFilter 00101 pass_.setFilterLimits (min_z_bounds_, max_z_bounds_); 00102 pass_.setFilterFieldName ("z"); 00103 pass_.setInputCloud (input_); 00104 pass_.filter (*cloud_filtered_); 00105 00106 if ((int)cloud_filtered_->points.size () < k_) 00107 { 00108 PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.", 00109 (int)cloud_filtered_->points.size ()); 00110 return; 00111 } 00112 00113 // Downsample the point cloud 00114 grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_); 00115 grid_.setDownsampleAllData (false); 00116 grid_.setInputCloud (cloud_filtered_); 00117 grid_.filter (*cloud_downsampled_); 00118 00119 // ---[ Estimate the point normals 00120 n3d_.setInputCloud (cloud_downsampled_); 00121 n3d_.compute (*cloud_normals_); 00122 00123 // ---[ Perform segmentation 00124 seg_.setInputCloud (cloud_downsampled_); 00125 seg_.setInputNormals (cloud_normals_); 00126 seg_.segment (*table_inliers_, *table_coefficients_); 00127 00128 if (table_inliers_->indices.size () == 0) 00129 { 00130 PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting."); 00131 return; 00132 } 00133 00134 // ---[ Extract the plane 00135 proj_.setInputCloud (cloud_downsampled_); 00136 proj_.setIndices (table_inliers_); 00137 proj_.setModelCoefficients (table_coefficients_); 00138 proj_.filter (*table_projected_); 00139 00140 // ---[ Estimate the convex hull 00141 std::vector<pcl::Vertices> polygons; 00142 CloudPtr table_hull (new Cloud ()); 00143 hull_.setInputCloud (table_projected_); 00144 hull_.reconstruct (*table_hull, polygons); 00145 00146 // Compute the plane coefficients 00147 Eigen::Vector4f model_coefficients; 00148 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00149 00150 model_coefficients[0] = table_coefficients_->values[0]; 00151 model_coefficients[1] = table_coefficients_->values[1]; 00152 model_coefficients[2] = table_coefficients_->values[2]; 00153 model_coefficients[3] = table_coefficients_->values[3]; 00154 00155 // Need to flip the plane normal towards the viewpoint 00156 Eigen::Vector4f vp (0, 0, 0, 0); 00157 // See if we need to flip any plane normals 00158 vp -= table_hull->points[0].getVector4fMap (); 00159 vp[3] = 0; 00160 // Dot product between the (viewpoint - point) and the plane normal 00161 float cos_theta = vp.dot (model_coefficients); 00162 // Flip the plane normal 00163 if (cos_theta < 0) 00164 { 00165 model_coefficients *= -1; 00166 model_coefficients[3] = 0; 00167 // Hessian form (D = nc . p_plane (centroid here) + p) 00168 model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ())); 00169 } 00170 00171 //Set table_coeffs 00172 table_coeffs_ = model_coefficients; 00173 00174 // ---[ Get the objects on top of the table 00175 pcl::PointIndices cloud_object_indices; 00176 prism_.setInputCloud (input_); 00177 prism_.setInputPlanarHull (table_hull); 00178 prism_.segment (cloud_object_indices); 00179 00180 pcl::ExtractIndices<PointType> extract_object_indices; 00181 extract_object_indices.setInputCloud (input_); 00182 extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices)); 00183 extract_object_indices.filter (*cloud_objects_); 00184 00185 //create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise 00186 pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>); 00187 00188 { 00189 binary_cloud->width = input_->width; 00190 binary_cloud->height = input_->height; 00191 binary_cloud->points.resize (input_->points.size ()); 00192 binary_cloud->is_dense = input_->is_dense; 00193 00194 size_t idx; 00195 for (size_t i = 0; i < cloud_object_indices.indices.size (); ++i) 00196 { 00197 idx = cloud_object_indices.indices[i]; 00198 binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap (); 00199 binary_cloud->points[idx].intensity = 1.0; 00200 } 00201 } 00202 00203 //connected components on the binary image 00204 00205 std::map<float, float> connected_labels; 00206 float c_intensity = 0.1; 00207 float intensity_incr = 0.1; 00208 00209 { 00210 00211 size_t wsize = wsize_; 00212 for (size_t i = 0; i < binary_cloud->width; i++) 00213 { 00214 for (size_t j = 0; j < binary_cloud->height; j++) 00215 { 00216 if (binary_cloud->at (i, j).intensity != 0) 00217 { 00218 //check neighboring pixels, first left and then top 00219 //be aware of margins 00220 00221 if ((i - 1) < 0 && (j - 1) < 0) 00222 { 00223 //top-left pixel 00224 (*binary_cloud) (i, j).intensity = c_intensity; 00225 } 00226 else 00227 { 00228 if ((j - 1) < 0) 00229 { 00230 //top-row, check on the left of pixel to assign a new label or not 00231 int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00232 if (left) 00233 { 00234 //Nothing found on the left, check bigger window 00235 00236 bool found = false; 00237 for (size_t kk = 2; kk < wsize && !found; kk++) 00238 { 00239 if ((i - kk) < 0) 00240 continue; 00241 00242 int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00243 if (left == 0) 00244 { 00245 found = true; 00246 } 00247 } 00248 00249 if (!found) 00250 { 00251 c_intensity += intensity_incr; 00252 (*binary_cloud) (i, j).intensity = c_intensity; 00253 } 00254 00255 } 00256 } 00257 else 00258 { 00259 if ((i - 1) == 0) 00260 { 00261 //check only top 00262 int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00263 if (top) 00264 { 00265 bool found = false; 00266 for (size_t kk = 2; kk < wsize && !found; kk++) 00267 { 00268 if ((j - kk) < 0) 00269 continue; 00270 00271 int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00272 if (top == 0) 00273 { 00274 found = true; 00275 } 00276 } 00277 00278 if (!found) 00279 { 00280 c_intensity += intensity_incr; 00281 (*binary_cloud) (i, j).intensity = c_intensity; 00282 } 00283 } 00284 00285 } 00286 else 00287 { 00288 //check left and top 00289 int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00290 int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00291 00292 if (left == 0 && top == 0) 00293 { 00294 //both top and left had labels, check if they are different 00295 //if they are, take the smallest one and mark labels to be connected.. 00296 00297 if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity) 00298 { 00299 float smaller_intensity = (*binary_cloud) (i - 1, j).intensity; 00300 float bigger_intensity = (*binary_cloud) (i, j - 1).intensity; 00301 00302 if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity) 00303 { 00304 smaller_intensity = (*binary_cloud) (i, j - 1).intensity; 00305 bigger_intensity = (*binary_cloud) (i - 1, j).intensity; 00306 } 00307 00308 connected_labels[bigger_intensity] = smaller_intensity; 00309 (*binary_cloud) (i, j).intensity = smaller_intensity; 00310 } 00311 } 00312 00313 if (left == 1 && top == 1) 00314 { 00315 //if none had labels, increment c_intensity 00316 //search first on bigger window 00317 bool found = false; 00318 for (size_t dist = 2; dist < wsize && !found; dist++) 00319 { 00320 if (((i - dist) < 0) || ((j - dist) < 0)) 00321 continue; 00322 00323 int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00324 int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_); 00325 00326 if (left == 0 && top == 0) 00327 { 00328 if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity) 00329 { 00330 float smaller_intensity = (*binary_cloud) (i - dist, j).intensity; 00331 float bigger_intensity = (*binary_cloud) (i, j - dist).intensity; 00332 00333 if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity) 00334 { 00335 smaller_intensity = (*binary_cloud) (i, j - dist).intensity; 00336 bigger_intensity = (*binary_cloud) (i - dist, j).intensity; 00337 } 00338 00339 connected_labels[bigger_intensity] = smaller_intensity; 00340 (*binary_cloud) (i, j).intensity = smaller_intensity; 00341 found = true; 00342 } 00343 } 00344 else if (left == 0 || top == 0) 00345 { 00346 //one had label 00347 found = true; 00348 } 00349 } 00350 00351 if (!found) 00352 { 00353 //none had label in the bigger window 00354 c_intensity += intensity_incr; 00355 (*binary_cloud) (i, j).intensity = c_intensity; 00356 } 00357 } 00358 } 00359 } 00360 } 00361 00362 } 00363 } 00364 } 00365 } 00366 00367 std::map<float, pcl::PointIndices> clusters_map; 00368 00369 { 00370 std::map<float, float>::iterator it; 00371 00372 for (size_t i = 0; i < binary_cloud->width; i++) 00373 { 00374 for (size_t j = 0; j < binary_cloud->height; j++) 00375 { 00376 if (binary_cloud->at (i, j).intensity != 0) 00377 { 00378 //check if this is a root label... 00379 it = connected_labels.find (binary_cloud->at (i, j).intensity); 00380 while (it != connected_labels.end ()) 00381 { 00382 //the label is on the list, change pixel intensity until it has a root label 00383 (*binary_cloud) (i, j).intensity = (*it).second; 00384 it = connected_labels.find (binary_cloud->at (i, j).intensity); 00385 } 00386 00387 std::map<float, pcl::PointIndices>::iterator it_indices; 00388 it_indices = clusters_map.find (binary_cloud->at (i, j).intensity); 00389 if (it_indices == clusters_map.end ()) 00390 { 00391 pcl::PointIndices indices; 00392 clusters_map[binary_cloud->at (i, j).intensity] = indices; 00393 } 00394 00395 clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (j * binary_cloud->width + i); 00396 } 00397 } 00398 } 00399 } 00400 00401 clusters.resize (clusters_map.size ()); 00402 00403 std::map<float, pcl::PointIndices>::iterator it_indices; 00404 int k = 0; 00405 for (it_indices = clusters_map.begin (); it_indices != clusters_map.end (); it_indices++) 00406 { 00407 00408 if ((*it_indices).second.indices.size () >= object_cluster_min_size_) 00409 { 00410 00411 clusters[k] = (CloudPtr)(new Cloud ()); 00412 pcl::copyPointCloud (*input_, (*it_indices).second.indices, *clusters[k]); 00413 k++; 00414 } 00415 } 00416 00417 clusters.resize (k); 00418 00419 } 00420 00421 template<typename PointType> 00422 void 00423 pcl::apps::DominantPlaneSegmentation<PointType>::compute (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters) 00424 { 00425 00426 // Has the input dataset been set already? 00427 if (!input_) 00428 { 00429 PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n"); 00430 return; 00431 } 00432 00433 CloudConstPtr cloud_; 00434 CloudPtr cloud_filtered_ (new Cloud ()); 00435 CloudPtr cloud_downsampled_ (new Cloud ()); 00436 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ()); 00437 pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ()); 00438 pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ()); 00439 CloudPtr table_projected_ (new Cloud ()); 00440 CloudPtr table_hull_ (new Cloud ()); 00441 CloudPtr cloud_objects_ (new Cloud ()); 00442 CloudPtr cluster_object_ (new Cloud ()); 00443 00444 typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>); 00445 typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>); 00446 clusters_tree_->setEpsilon (1); 00447 00448 // Normal estimation parameters 00449 n3d_.setKSearch (k_); 00450 n3d_.setSearchMethod (normals_tree_); 00451 00452 // Table model fitting parameters 00453 seg_.setDistanceThreshold (sac_distance_threshold_); 00454 seg_.setMaxIterations (2000); 00455 seg_.setNormalDistanceWeight (0.1); 00456 seg_.setOptimizeCoefficients (true); 00457 seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00458 seg_.setMethodType (pcl::SAC_RANSAC); 00459 seg_.setProbability (0.99); 00460 00461 proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00462 bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00463 00464 prism_.setHeightLimits (object_min_height_, object_max_height_); 00465 00466 // Clustering parameters 00467 cluster_.setClusterTolerance (object_cluster_tolerance_); 00468 cluster_.setMinClusterSize (object_cluster_min_size_); 00469 cluster_.setSearchMethod (clusters_tree_); 00470 00471 // ---[ PassThroughFilter 00472 pass_.setFilterLimits (min_z_bounds_, max_z_bounds_); 00473 pass_.setFilterFieldName ("z"); 00474 pass_.setInputCloud (input_); 00475 pass_.filter (*cloud_filtered_); 00476 00477 if ((int)cloud_filtered_->points.size () < k_) 00478 { 00479 PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.", 00480 (int)cloud_filtered_->points.size ()); 00481 return; 00482 } 00483 00484 // Downsample the point cloud 00485 grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_); 00486 grid_.setDownsampleAllData (false); 00487 grid_.setInputCloud (cloud_filtered_); 00488 grid_.filter (*cloud_downsampled_); 00489 00490 PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering (%f -> %f): %d out of %d\n", 00491 min_z_bounds_, max_z_bounds_, (int)cloud_downsampled_->points.size (), (int)input_->points.size ()); 00492 00493 // ---[ Estimate the point normals 00494 n3d_.setInputCloud (cloud_downsampled_); 00495 n3d_.compute (*cloud_normals_); 00496 00497 PCL_INFO ("[DominantPlaneSegmentation] %d normals estimated. \n", (int)cloud_normals_->points.size ()); 00498 00499 // ---[ Perform segmentation 00500 seg_.setInputCloud (cloud_downsampled_); 00501 seg_.setInputNormals (cloud_normals_); 00502 seg_.segment (*table_inliers_, *table_coefficients_); 00503 00504 if (table_inliers_->indices.size () == 0) 00505 { 00506 PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting."); 00507 return; 00508 } 00509 00510 // ---[ Extract the plane 00511 proj_.setInputCloud (cloud_downsampled_); 00512 proj_.setIndices (table_inliers_); 00513 proj_.setModelCoefficients (table_coefficients_); 00514 proj_.filter (*table_projected_); 00515 00516 // ---[ Estimate the convex hull 00517 std::vector<pcl::Vertices> polygons; 00518 CloudPtr table_hull (new Cloud ()); 00519 hull_.setInputCloud (table_projected_); 00520 hull_.reconstruct (*table_hull, polygons); 00521 00522 // Compute the plane coefficients 00523 Eigen::Vector4f model_coefficients; 00524 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00525 00526 model_coefficients[0] = table_coefficients_->values[0]; 00527 model_coefficients[1] = table_coefficients_->values[1]; 00528 model_coefficients[2] = table_coefficients_->values[2]; 00529 model_coefficients[3] = table_coefficients_->values[3]; 00530 00531 // Need to flip the plane normal towards the viewpoint 00532 Eigen::Vector4f vp (0, 0, 0, 0); 00533 // See if we need to flip any plane normals 00534 vp -= table_hull->points[0].getVector4fMap (); 00535 vp[3] = 0; 00536 // Dot product between the (viewpoint - point) and the plane normal 00537 float cos_theta = vp.dot (model_coefficients); 00538 // Flip the plane normal 00539 if (cos_theta < 0) 00540 { 00541 model_coefficients *= -1; 00542 model_coefficients[3] = 0; 00543 // Hessian form (D = nc . p_plane (centroid here) + p) 00544 model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ())); 00545 } 00546 00547 //Set table_coeffs 00548 table_coeffs_ = model_coefficients; 00549 00550 // ---[ Get the objects on top of the table 00551 pcl::PointIndices cloud_object_indices; 00552 prism_.setInputCloud (cloud_downsampled_); 00553 prism_.setInputPlanarHull (table_hull); 00554 prism_.segment (cloud_object_indices); 00555 00556 pcl::ExtractIndices<PointType> extract_object_indices; 00557 extract_object_indices.setInputCloud (cloud_downsampled_); 00558 extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices)); 00559 extract_object_indices.filter (*cloud_objects_); 00560 00561 if (cloud_objects_->points.size () == 0) 00562 return; 00563 00564 //down_.reset(new Cloud(*cloud_downsampled_)); 00565 00566 // ---[ Split the objects into Euclidean clusters 00567 std::vector<pcl::PointIndices> clusters2; 00568 cluster_.setInputCloud (cloud_downsampled_); 00569 cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices)); 00570 cluster_.extract (clusters2); 00571 00572 PCL_INFO ("[DominantPlaneSegmentation::compute()] Number of clusters found matching the given constraints: %d.\n", 00573 (int)clusters2.size ()); 00574 00575 clusters.resize (clusters2.size ()); 00576 00577 for (size_t i = 0; i < clusters2.size (); ++i) 00578 { 00579 clusters[i] = (CloudPtr)(new Cloud ()); 00580 pcl::copyPointCloud (*cloud_downsampled_, clusters2[i].indices, *clusters[i]); 00581 } 00582 } 00583 00584 template<typename PointType> 00585 void 00586 pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters) 00587 { 00588 00589 // Has the input dataset been set already? 00590 if (!input_) 00591 { 00592 PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n"); 00593 return; 00594 } 00595 00596 CloudConstPtr cloud_; 00597 CloudPtr cloud_filtered_ (new Cloud ()); 00598 CloudPtr cloud_downsampled_ (new Cloud ()); 00599 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ()); 00600 pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ()); 00601 pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ()); 00602 CloudPtr table_projected_ (new Cloud ()); 00603 CloudPtr table_hull_ (new Cloud ()); 00604 CloudPtr cloud_objects_ (new Cloud ()); 00605 CloudPtr cluster_object_ (new Cloud ()); 00606 00607 typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>); 00608 typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>); 00609 clusters_tree_->setEpsilon (1); 00610 00611 // Normal estimation parameters 00612 n3d_.setKSearch (10); 00613 n3d_.setSearchMethod (normals_tree_); 00614 00615 // Table model fitting parameters 00616 seg_.setDistanceThreshold (sac_distance_threshold_); 00617 seg_.setMaxIterations (2000); 00618 seg_.setNormalDistanceWeight (0.1); 00619 seg_.setOptimizeCoefficients (true); 00620 seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00621 seg_.setMethodType (pcl::SAC_MSAC); 00622 seg_.setProbability (0.98); 00623 00624 proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00625 bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 00626 00627 prism_.setHeightLimits (object_min_height_, object_max_height_); 00628 00629 // Clustering parameters 00630 cluster_.setClusterTolerance (object_cluster_tolerance_); 00631 cluster_.setMinClusterSize (object_cluster_min_size_); 00632 cluster_.setSearchMethod (clusters_tree_); 00633 00634 // ---[ PassThroughFilter 00635 pass_.setFilterLimits (min_z_bounds_, max_z_bounds_); 00636 pass_.setFilterFieldName ("z"); 00637 pass_.setInputCloud (input_); 00638 pass_.filter (*cloud_filtered_); 00639 00640 if ((int)cloud_filtered_->points.size () < k_) 00641 { 00642 PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.", 00643 (int)cloud_filtered_->points.size ()); 00644 return; 00645 } 00646 00647 // Downsample the point cloud 00648 grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_); 00649 grid_.setDownsampleAllData (false); 00650 grid_.setInputCloud (cloud_filtered_); 00651 grid_.filter (*cloud_downsampled_); 00652 00653 PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %d out of %d\n", 00654 min_z_bounds_, max_z_bounds_, (int)cloud_downsampled_->points.size (), (int)input_->points.size ()); 00655 00656 // ---[ Estimate the point normals 00657 n3d_.setInputCloud (cloud_downsampled_); 00658 n3d_.compute (*cloud_normals_); 00659 00660 PCL_INFO ("[DominantPlaneSegmentation] %d normals estimated. \n", (int)cloud_normals_->points.size ()); 00661 00662 // ---[ Perform segmentation 00663 seg_.setInputCloud (cloud_downsampled_); 00664 seg_.setInputNormals (cloud_normals_); 00665 seg_.segment (*table_inliers_, *table_coefficients_); 00666 00667 if (table_inliers_->indices.size () == 0) 00668 { 00669 PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting."); 00670 return; 00671 } 00672 00673 // ---[ Extract the plane 00674 proj_.setInputCloud (cloud_downsampled_); 00675 proj_.setIndices (table_inliers_); 00676 proj_.setModelCoefficients (table_coefficients_); 00677 proj_.filter (*table_projected_); 00678 00679 // ---[ Estimate the convex hull 00680 std::vector<pcl::Vertices> polygons; 00681 CloudPtr table_hull (new Cloud ()); 00682 hull_.setInputCloud (table_projected_); 00683 hull_.reconstruct (*table_hull, polygons); 00684 00685 // Compute the plane coefficients 00686 Eigen::Vector4f model_coefficients; 00687 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00688 00689 model_coefficients[0] = table_coefficients_->values[0]; 00690 model_coefficients[1] = table_coefficients_->values[1]; 00691 model_coefficients[2] = table_coefficients_->values[2]; 00692 model_coefficients[3] = table_coefficients_->values[3]; 00693 00694 // Need to flip the plane normal towards the viewpoint 00695 Eigen::Vector4f vp (0, 0, 0, 0); 00696 // See if we need to flip any plane normals 00697 vp -= table_hull->points[0].getVector4fMap (); 00698 vp[3] = 0; 00699 // Dot product between the (viewpoint - point) and the plane normal 00700 float cos_theta = vp.dot (model_coefficients); 00701 // Flip the plane normal 00702 if (cos_theta < 0) 00703 { 00704 model_coefficients *= -1; 00705 model_coefficients[3] = 0; 00706 // Hessian form (D = nc . p_plane (centroid here) + p) 00707 model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ())); 00708 } 00709 00710 //Set table_coeffs 00711 table_coeffs_ = model_coefficients; 00712 00713 // ---[ Get the objects on top of the table 00714 pcl::PointIndices cloud_object_indices; 00715 prism_.setInputCloud (cloud_filtered_); 00716 prism_.setInputPlanarHull (table_hull); 00717 prism_.segment (cloud_object_indices); 00718 00719 pcl::ExtractIndices<PointType> extract_object_indices; 00720 extract_object_indices.setInputCloud (cloud_downsampled_); 00721 extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices)); 00722 extract_object_indices.filter (*cloud_objects_); 00723 00724 if (cloud_objects_->points.size () == 0) 00725 return; 00726 00727 // ---[ Split the objects into Euclidean clusters 00728 std::vector<pcl::PointIndices> clusters2; 00729 cluster_.setInputCloud (cloud_filtered_); 00730 cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices)); 00731 cluster_.extract (clusters2); 00732 00733 PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %d.\n", 00734 (int)clusters2.size ()); 00735 00736 clusters.resize (clusters2.size ()); 00737 00738 for (size_t i = 0; i < clusters2.size (); ++i) 00739 { 00740 clusters[i] = (CloudPtr)(new Cloud ()); 00741 pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]); 00742 } 00743 } 00744 00745 #define PCL_INSTANTIATE_DominantPlaneSegmentation(T) template class PCL_EXPORTS pcl::apps::DominantPlaneSegmentation<T>;