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-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 the copyright holder(s) 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/boost.h> 00044 #include <pcl/segmentation/organized_connected_component_segmentation.h> 00045 #include <pcl/segmentation/organized_multi_plane_segmentation.h> 00046 #include <pcl/common/centroid.h> 00047 #include <pcl/common/eigen.h> 00048 00049 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00050 template <typename PointT> pcl::PointCloud<PointT> 00051 projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp) 00052 { 00053 Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]); 00054 pcl::PointCloud<PointT> projected_cloud; 00055 projected_cloud.resize (cloud.points.size ()); 00056 for (size_t i = 0; i < cloud.points.size (); i++) 00057 { 00058 Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); 00059 //Eigen::Vector3f intersection = (vp, pt, norm, centroid); 00060 float u = norm.dot ((centroid - vp)) / norm.dot ((pt - vp)); 00061 Eigen::Vector3f intersection (vp + u * (pt - vp)); 00062 projected_cloud[i].x = intersection[0]; 00063 projected_cloud[i].y = intersection[1]; 00064 projected_cloud[i].z = intersection[2]; 00065 } 00066 00067 return (projected_cloud); 00068 } 00069 00070 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00071 template<typename PointT, typename PointNT, typename PointLT> void 00072 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, 00073 std::vector<PointIndices>& inlier_indices) 00074 { 00075 pcl::PointCloud<pcl::Label> labels; 00076 std::vector<pcl::PointIndices> label_indices; 00077 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00078 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00079 segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00080 } 00081 00082 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00083 template<typename PointT, typename PointNT, typename PointLT> void 00084 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, 00085 std::vector<PointIndices>& inlier_indices, 00086 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids, 00087 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances, 00088 pcl::PointCloud<PointLT>& labels, 00089 std::vector<pcl::PointIndices>& label_indices) 00090 { 00091 if (!initCompute ()) 00092 return; 00093 00094 // Check that we got the same number of points and normals 00095 if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ())) 00096 { 00097 PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n", 00098 getClassName ().c_str (), input_->points.size (), 00099 normals_->points.size ()); 00100 return; 00101 } 00102 00103 // Check that the cloud is organized 00104 if (!input_->isOrganized ()) 00105 { 00106 PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n", 00107 getClassName ().c_str ()); 00108 return; 00109 } 00110 00111 // Calculate range part of planes' hessian normal form 00112 std::vector<float> plane_d (input_->points.size ()); 00113 00114 for (unsigned int i = 0; i < input_->size (); ++i) 00115 plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ()); 00116 00117 // Make a comparator 00118 //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d); 00119 compare_->setPlaneCoeffD (plane_d); 00120 compare_->setInputCloud (input_); 00121 compare_->setInputNormals (normals_); 00122 compare_->setAngularThreshold (static_cast<float> (angular_threshold_)); 00123 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true); 00124 00125 // Set up the output 00126 OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_); 00127 connected_component.setInputCloud (input_); 00128 connected_component.segment (labels, label_indices); 00129 00130 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero (); 00131 Eigen::Vector4f vp = Eigen::Vector4f::Zero (); 00132 Eigen::Matrix3f clust_cov; 00133 pcl::ModelCoefficients model; 00134 model.values.resize (4); 00135 00136 // Fit Planes to each cluster 00137 for (size_t i = 0; i < label_indices.size (); i++) 00138 { 00139 if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_) 00140 { 00141 pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid); 00142 Eigen::Vector4f plane_params; 00143 00144 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00145 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00146 pcl::eigen33 (clust_cov, eigen_value, eigen_vector); 00147 plane_params[0] = eigen_vector[0]; 00148 plane_params[1] = eigen_vector[1]; 00149 plane_params[2] = eigen_vector[2]; 00150 plane_params[3] = 0; 00151 plane_params[3] = -1 * plane_params.dot (clust_centroid); 00152 00153 vp -= clust_centroid; 00154 float cos_theta = vp.dot (plane_params); 00155 if (cos_theta < 0) 00156 { 00157 plane_params *= -1; 00158 plane_params[3] = 0; 00159 plane_params[3] = -1 * plane_params.dot (clust_centroid); 00160 } 00161 00162 // Compute the curvature surface change 00163 float curvature; 00164 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8); 00165 if (eig_sum != 0) 00166 curvature = fabsf (eigen_value / eig_sum); 00167 else 00168 curvature = 0; 00169 00170 if (curvature < maximum_curvature_) 00171 { 00172 model.values[0] = plane_params[0]; 00173 model.values[1] = plane_params[1]; 00174 model.values[2] = plane_params[2]; 00175 model.values[3] = plane_params[3]; 00176 model_coefficients.push_back (model); 00177 inlier_indices.push_back (label_indices[i]); 00178 centroids.push_back (clust_centroid); 00179 covariances.push_back (clust_cov); 00180 } 00181 } 00182 } 00183 deinitCompute (); 00184 } 00185 00186 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00187 template<typename PointT, typename PointNT, typename PointLT> void 00188 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions) 00189 { 00190 std::vector<ModelCoefficients> model_coefficients; 00191 std::vector<PointIndices> inlier_indices; 00192 PointCloudLPtr labels (new PointCloudL); 00193 std::vector<pcl::PointIndices> label_indices; 00194 std::vector<pcl::PointIndices> boundary_indices; 00195 pcl::PointCloud<PointT> boundary_cloud; 00196 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00197 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00198 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00199 regions.resize (model_coefficients.size ()); 00200 boundary_indices.resize (model_coefficients.size ()); 00201 00202 for (size_t i = 0; i < model_coefficients.size (); i++) 00203 { 00204 boundary_cloud.resize (0); 00205 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]); 00206 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00207 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00208 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00209 00210 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00211 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00212 model_coefficients[i].values[1], 00213 model_coefficients[i].values[2], 00214 model_coefficients[i].values[3]); 00215 regions[i] = PlanarRegion<PointT> (centroid, 00216 covariances[i], 00217 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00218 boundary_cloud.points, 00219 model); 00220 } 00221 } 00222 00223 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00224 template<typename PointT, typename PointNT, typename PointLT> void 00225 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions) 00226 { 00227 std::vector<ModelCoefficients> model_coefficients; 00228 std::vector<PointIndices> inlier_indices; 00229 PointCloudLPtr labels (new PointCloudL); 00230 std::vector<pcl::PointIndices> label_indices; 00231 std::vector<pcl::PointIndices> boundary_indices; 00232 pcl::PointCloud<PointT> boundary_cloud; 00233 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00234 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00235 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00236 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00237 regions.resize (model_coefficients.size ()); 00238 boundary_indices.resize (model_coefficients.size ()); 00239 00240 for (size_t i = 0; i < model_coefficients.size (); i++) 00241 { 00242 boundary_cloud.resize (0); 00243 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1; 00244 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); 00245 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00246 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00247 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00248 00249 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00250 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00251 model_coefficients[i].values[1], 00252 model_coefficients[i].values[2], 00253 model_coefficients[i].values[3]); 00254 00255 Eigen::Vector3f vp (0.0, 0.0, 0.0); 00256 if (project_points_) 00257 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); 00258 00259 regions[i] = PlanarRegion<PointT> (centroid, 00260 covariances[i], 00261 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00262 boundary_cloud.points, 00263 model); 00264 } 00265 } 00266 00267 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00268 template<typename PointT, typename PointNT, typename PointLT> void 00269 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions, 00270 std::vector<ModelCoefficients>& model_coefficients, 00271 std::vector<PointIndices>& inlier_indices, 00272 PointCloudLPtr& labels, 00273 std::vector<pcl::PointIndices>& label_indices, 00274 std::vector<pcl::PointIndices>& boundary_indices) 00275 { 00276 pcl::PointCloud<PointT> boundary_cloud; 00277 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids; 00278 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances; 00279 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); 00280 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); 00281 regions.resize (model_coefficients.size ()); 00282 boundary_indices.resize (model_coefficients.size ()); 00283 00284 for (size_t i = 0; i < model_coefficients.size (); i++) 00285 { 00286 boundary_cloud.resize (0); 00287 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1; 00288 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); 00289 boundary_cloud.points.resize (boundary_indices[i].indices.size ()); 00290 for (unsigned j = 0; j < boundary_indices[i].indices.size (); j++) 00291 boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; 00292 00293 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); 00294 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], 00295 model_coefficients[i].values[1], 00296 model_coefficients[i].values[2], 00297 model_coefficients[i].values[3]); 00298 00299 Eigen::Vector3f vp (0.0, 0.0, 0.0); 00300 if (project_points_ && boundary_cloud.points.size () > 0) 00301 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); 00302 00303 regions[i] = PlanarRegion<PointT> (centroid, 00304 covariances[i], 00305 static_cast<unsigned int> (inlier_indices[i].indices.size ()), 00306 boundary_cloud.points, 00307 model); 00308 } 00309 } 00310 00311 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00312 template<typename PointT, typename PointNT, typename PointLT> void 00313 pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::refine (std::vector<ModelCoefficients>& model_coefficients, 00314 std::vector<PointIndices>& inlier_indices, 00315 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&, 00316 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&, 00317 PointCloudLPtr& labels, 00318 std::vector<pcl::PointIndices>& label_indices) 00319 { 00320 //List of lables to grow, and index of model corresponding to each label 00321 std::vector<bool> grow_labels; 00322 std::vector<int> label_to_model; 00323 grow_labels.resize (label_indices.size (), false); 00324 label_to_model.resize (label_indices.size (), 0); 00325 00326 for (size_t i = 0; i < model_coefficients.size (); i++) 00327 { 00328 int model_label = (*labels)[inlier_indices[i].indices[0]].label; 00329 label_to_model[model_label] = static_cast<int> (i); 00330 grow_labels[model_label] = true; 00331 } 00332 00333 //refinement_compare_->setDistanceThreshold (0.015f, true); 00334 refinement_compare_->setInputCloud (input_); 00335 refinement_compare_->setLabels (labels); 00336 refinement_compare_->setModelCoefficients (model_coefficients); 00337 refinement_compare_->setRefineLabels (grow_labels); 00338 refinement_compare_->setLabelToModel (label_to_model); 00339 00340 //Do a first pass over the image, top to bottom, left to right 00341 unsigned int current_row = 0; 00342 unsigned int next_row = labels->width; 00343 for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width) 00344 { 00345 00346 for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx) 00347 { 00348 int current_label = (*labels)[current_row+colIdx].label; 00349 int right_label = (*labels)[current_row+colIdx+1].label; 00350 if (current_label < 0 || right_label < 0) 00351 continue; 00352 00353 //Check right 00354 //bool test1 = false; 00355 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1)) 00356 { 00357 //test1 = true; 00358 labels->points[current_row+colIdx+1].label = current_label; 00359 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1); 00360 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1); 00361 } 00362 00363 int lower_label = (*labels)[next_row+colIdx].label; 00364 if (lower_label < 0) 00365 continue; 00366 00367 //Check down 00368 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx)) 00369 { 00370 labels->points[next_row+colIdx].label = current_label; 00371 label_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx); 00372 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx); 00373 } 00374 00375 }//col 00376 }//row 00377 00378 //Do a second pass over the image 00379 current_row = labels->width * (labels->height - 1); 00380 unsigned int prev_row = current_row - labels->width; 00381 for (size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width) 00382 { 00383 for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx) 00384 { 00385 int current_label = (*labels)[current_row+colIdx].label; 00386 int left_label = (*labels)[current_row+colIdx-1].label; 00387 if (current_label < 0 || left_label < 0) 00388 continue; 00389 00390 //Check left 00391 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1)) 00392 { 00393 labels->points[current_row+colIdx-1].label = current_label; 00394 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1); 00395 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1); 00396 } 00397 00398 int upper_label = (*labels)[prev_row+colIdx].label; 00399 if (upper_label < 0) 00400 continue; 00401 //Check up 00402 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx)) 00403 { 00404 labels->points[prev_row+colIdx].label = current_label; 00405 label_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx); 00406 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx); 00407 } 00408 }//col 00409 }//row 00410 } 00411 00412 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>; 00413 00414 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_