|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_ 00041 #define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_ 00042 00043 #include <pcl/segmentation/organized_connected_component_segmentation.h> 00044 #include <pcl/segmentation/organized_multi_plane_segmentation.h> 00045 #include <pcl/common/centroid.h> 00046 #include <pcl/common/eigen.h> 00047 #include <boost/make_shared.hpp> 00048 00050 Eigen::Vector3f linePlaneIntersection (Eigen::Vector3f& p1, Eigen::Vector3f& p2, Eigen::Vector3f& norm, Eigen::Vector3f& p3) 00051 { 00052 float u = norm.dot ((p3 - p1)) / norm.dot ((p2 - p1)); 00053 Eigen::Vector3f intersection (p1 + u * (p2 - p1)); 00054 return (intersection); 00055 } 00056 00058 template <typename PointT> pcl::PointCloud<PointT> 00059 projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp) 00060 { 00061 Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]); 00062 pcl::PointCloud<PointT> projected_cloud; 00063 projected_cloud.resize (cloud.points.size ()); 00064 for (size_t i = 0; i < cloud.points.size (); i++) 00065 { 00066 Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); 00067 Eigen::Vector3f intersection = linePlaneIntersection (vp, pt, norm, centroid); 00068 projected_cloud[i].x = intersection[0]; 00069 projected_cloud[i].y = intersection[1]; 00070 projected_cloud[i].z = intersection[2]; 00071 } 00072 00073 return (projected_cloud); 00074 } 00075 00077 template<typename PointT, typename PointNT, typename PointLT> void 00078 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, 00079 std::vector<PointIndices>& inlier_indices) 00080 { 00081 pcl::PointCloud<pcl::Label> labels; 00082 std::vector<pcl::PointIndices> label_indices; 00083 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00084 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00085 segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00086 } 00087 00089 template<typename PointT, typename PointNT, typename PointLT> void 00090 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, 00091 std::vector<PointIndices>& inlier_indices, 00092 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids, 00093 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances, 00094 pcl::PointCloud<PointLT>& labels, 00095 std::vector<pcl::PointIndices>& label_indices) 00096 { 00097 if (!initCompute ()) 00098 return; 00099 00100 // Check that we got the same number of points and normals 00101 if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ())) 00102 { 00103 PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n", 00104 getClassName ().c_str (), input_->points.size (), 00105 normals_->points.size ()); 00106 return; 00107 } 00108 00109 // Check that the cloud is organized 00110 if (!input_->isOrganized ()) 00111 { 00112 PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n", 00113 getClassName ().c_str ()); 00114 return; 00115 } 00116 00117 // Calculate range part of planes' hessian normal form 00118 std::vector<float> plane_d (input_->points.size ()); 00119 00120 for (unsigned int i = 0; i < input_->size (); ++i) 00121 plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ()); 00122 00123 // Make a comparator 00124 //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d); 00125 compare_->setPlaneCoeffD (plane_d); 00126 compare_->setInputCloud (input_); 00127 compare_->setInputNormals (normals_); 00128 compare_->setAngularThreshold (static_cast<float> (angular_threshold_)); 00129 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true); 00130 00131 // Set up the output 00132 OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_); 00133 connected_component.setInputCloud (input_); 00134 connected_component.segment (labels, label_indices); 00135 00136 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero (); 00137 Eigen::Vector4f vp = Eigen::Vector4f::Zero (); 00138 Eigen::Matrix3f clust_cov; 00139 pcl::ModelCoefficients model; 00140 model.values.resize (4); 00141 00142 // Fit Planes to each cluster 00143 for (size_t i = 0; i < label_indices.size (); i++) 00144 { 00145 if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_) 00146 { 00147 pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid); 00148 Eigen::Vector4f plane_params; 00149 00150 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00151 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00152 pcl::eigen33 (clust_cov, eigen_value, eigen_vector); 00153 plane_params[0] = eigen_vector[0]; 00154 plane_params[1] = eigen_vector[1]; 00155 plane_params[2] = eigen_vector[2]; 00156 plane_params[3] = 0; 00157 plane_params[3] = -1 * plane_params.dot (clust_centroid); 00158 00159 vp -= clust_centroid; 00160 float cos_theta = vp.dot (plane_params); 00161 if (cos_theta < 0) 00162 { 00163 plane_params *= -1; 00164 plane_params[3] = 0; 00165 plane_params[3] = -1 * plane_params.dot (clust_centroid); 00166 } 00167 00168 // Compute the curvature surface change 00169 float curvature; 00170 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8); 00171 if (eig_sum != 0) 00172 curvature = fabsf (eigen_value / eig_sum); 00173 else 00174 curvature = 0; 00175 00176 if (curvature < maximum_curvature_) 00177 { 00178 model.values[0] = plane_params[0]; 00179 model.values[1] = plane_params[1]; 00180 model.values[2] = plane_params[2]; 00181 model.values[3] = plane_params[3]; 00182 model_coefficients.push_back (model); 00183 inlier_indices.push_back (label_indices[i]); 00184 centroids.push_back (clust_centroid); 00185 covariances.push_back (clust_cov); 00186 } 00187 } 00188 } 00189 deinitCompute (); 00190 } 00191 00193 template<typename PointT, typename PointNT, typename PointLT> void 00194 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions) 00195 { 00196 std::vector<ModelCoefficients> model_coefficients; 00197 std::vector<PointIndices> inlier_indices; 00198 PointCloudLPtr labels (new PointCloudL); 00199 std::vector<pcl::PointIndices> label_indices; 00200 std::vector<pcl::PointIndices> boundary_indices; 00201 pcl::PointCloud<PointT> boundary_cloud; 00202 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00203 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00204 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00205 regions.resize (model_coefficients.size ()); 00206 boundary_indices.resize (model_coefficients.size ()); 00207 00208 for (size_t i = 0; i < model_coefficients.size (); i++) 00209 { 00210 boundary_cloud.resize (0); 00211 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]); 00212 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00213 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00214 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00215 00216 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00217 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00218 model_coefficients[i].values[1], 00219 model_coefficients[i].values[2], 00220 model_coefficients[i].values[3]); 00221 regions[i] = PlanarRegion<PointT> (centroid, 00222 covariances[i], 00223 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00224 boundary_cloud.points, 00225 model); 00226 } 00227 } 00228 00230 template<typename PointT, typename PointNT, typename PointLT> void 00231 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions) 00232 { 00233 std::vector<ModelCoefficients> model_coefficients; 00234 std::vector<PointIndices> inlier_indices; 00235 PointCloudLPtr labels (new PointCloudL); 00236 std::vector<pcl::PointIndices> label_indices; 00237 std::vector<pcl::PointIndices> boundary_indices; 00238 pcl::PointCloud<PointT> boundary_cloud; 00239 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00240 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00241 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00242 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00243 regions.resize (model_coefficients.size ()); 00244 boundary_indices.resize (model_coefficients.size ()); 00245 00246 for (size_t i = 0; i < model_coefficients.size (); i++) 00247 { 00248 boundary_cloud.resize (0); 00249 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1; 00250 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); 00251 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00252 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00253 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00254 00255 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00256 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00257 model_coefficients[i].values[1], 00258 model_coefficients[i].values[2], 00259 model_coefficients[i].values[3]); 00260 00261 Eigen::Vector3f vp (0.0, 0.0, 0.0); 00262 if (project_points_) 00263 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); 00264 00265 regions[i] = PlanarRegion<PointT> (centroid, 00266 covariances[i], 00267 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00268 boundary_cloud.points, 00269 model); 00270 } 00271 } 00272 00274 template<typename PointT, typename PointNT, typename PointLT> void 00275 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions, 00276 std::vector<ModelCoefficients>& model_coefficients, 00277 std::vector<PointIndices>& inlier_indices, 00278 PointCloudLPtr& labels, 00279 std::vector<pcl::PointIndices>& label_indices, 00280 std::vector<pcl::PointIndices>& boundary_indices) 00281 { 00282 pcl::PointCloud<PointT> boundary_cloud; 00283 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00284 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00285 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00286 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00287 regions.resize (model_coefficients.size ()); 00288 boundary_indices.resize (model_coefficients.size ()); 00289 00290 for (size_t i = 0; i < model_coefficients.size (); i++) 00291 { 00292 boundary_cloud.resize (0); 00293 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1; 00294 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); 00295 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00296 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00297 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00298 00299 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00300 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00301 model_coefficients[i].values[1], 00302 model_coefficients[i].values[2], 00303 model_coefficients[i].values[3]); 00304 00305 Eigen::Vector3f vp (0.0, 0.0, 0.0); 00306 if (project_points_ && boundary_cloud.points.size () > 0) 00307 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); 00308 00309 regions[i] = PlanarRegion<PointT> (centroid, 00310 covariances[i], 00311 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00312 boundary_cloud.points, 00313 model); 00314 } 00315 } 00316 00318 template<typename PointT, typename PointNT, typename PointLT> void 00319 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::refine (std::vector<ModelCoefficients>& model_coefficients, 00320 std::vector<PointIndices>& inlier_indices, 00321 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&, 00322 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&, 00323 PointCloudLPtr& labels, 00324 std::vector<pcl::PointIndices>& label_indices) 00325 { 00326 //List of lables to grow, and index of model corresponding to each label 00327 std::vector<bool> grow_labels; 00328 std::vector<int> label_to_model; 00329 grow_labels.resize (label_indices.size (), false); 00330 label_to_model.resize (label_indices.size (), 0); 00331 00332 for (size_t i = 0; i < model_coefficients.size (); i++) 00333 { 00334 int model_label = (*labels)[inlier_indices[i].indices[0]].label; 00335 label_to_model[model_label] = static_cast<int> (i); 00336 grow_labels[model_label] = true; 00337 } 00338 00339 //refinement_compare_->setDistanceThreshold (0.015f, true); 00340 refinement_compare_->setInputCloud (input_); 00341 refinement_compare_->setLabels (labels); 00342 refinement_compare_->setModelCoefficients (model_coefficients); 00343 refinement_compare_->setRefineLabels (grow_labels); 00344 refinement_compare_->setLabelToModel (label_to_model); 00345 00346 //Do a first pass over the image, top to bottom, left to right 00347 unsigned int current_row = 0; 00348 unsigned int next_row = labels->width; 00349 for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width) 00350 { 00351 00352 for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx) 00353 { 00354 int current_label = (*labels)[current_row+colIdx].label; 00355 int right_label = (*labels)[current_row+colIdx+1].label; 00356 if (current_label < 0 || right_label < 0) 00357 continue; 00358 00359 //Check right 00360 //bool test1 = false; 00361 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1)) 00362 { 00363 //test1 = true; 00364 labels->points[current_row+colIdx+1].label = current_label; 00365 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1); 00366 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1); 00367 } 00368 00369 int lower_label = (*labels)[next_row+colIdx].label; 00370 if (lower_label < 0) 00371 continue; 00372 00373 //Check down 00374 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx)) 00375 { 00376 labels->points[next_row+colIdx].label = current_label; 00377 label_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx); 00378 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx); 00379 } 00380 00381 }//col 00382 }//row 00383 00384 //Do a second pass over the image 00385 current_row = labels->width * (labels->height - 1); 00386 unsigned int prev_row = current_row - labels->width; 00387 for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width) 00388 { 00389 for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx) 00390 { 00391 int current_label = (*labels)[current_row+colIdx].label; 00392 int left_label = (*labels)[current_row+colIdx-1].label; 00393 if (current_label < 0 || left_label < 0) 00394 continue; 00395 00396 //Check left 00397 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1)) 00398 { 00399 labels->points[current_row+colIdx-1].label = current_label; 00400 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1); 00401 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1); 00402 } 00403 00404 int upper_label = (*labels)[prev_row+colIdx].label; 00405 if (upper_label < 0) 00406 continue; 00407 //Check up 00408 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx)) 00409 { 00410 labels->points[prev_row+colIdx].label = current_label; 00411 label_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx); 00412 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx); 00413 } 00414 }//col 00415 }//row 00416 } 00417 00418 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>; 00419 00420 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
1.7.6.1
