Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
organized_multi_plane_segmentation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 *
37 *
38 */
39
40#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41#define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
42
43#include <pcl/segmentation/organized_connected_component_segmentation.h>
44#include <pcl/segmentation/organized_multi_plane_segmentation.h>
45#include <pcl/common/centroid.h>
46#include <pcl/common/eigen.h>
47
48/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointT> pcl::PointCloud<PointT>
50projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
51{
52 Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]);
53 pcl::PointCloud<PointT> projected_cloud;
54 projected_cloud.resize (cloud.size ());
55 for (std::size_t i = 0; i < cloud.size (); i++)
56 {
57 Eigen::Vector3f pt (cloud[i].x, cloud[i].y, cloud[i].z);
58 //Eigen::Vector3f intersection = (vp, pt, norm, centroid);
59 float u = norm.dot ((centroid - vp)) / norm.dot ((pt - vp));
60 Eigen::Vector3f intersection (vp + u * (pt - vp));
61 projected_cloud[i].x = intersection[0];
62 projected_cloud[i].y = intersection[1];
63 projected_cloud[i].z = intersection[2];
64 }
65
66 return (projected_cloud);
67}
68
69//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70template<typename PointT, typename PointNT, typename PointLT> void
71pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
72 std::vector<PointIndices>& inlier_indices)
73{
75 std::vector<pcl::PointIndices> label_indices;
76 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
77 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
78 segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
79}
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template<typename PointT, typename PointNT, typename PointLT> void
83pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
84 std::vector<PointIndices>& inlier_indices,
85 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
86 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
88 std::vector<pcl::PointIndices>& label_indices)
89{
90 if (!initCompute ())
91 return;
92
93 // Check that the normals are present
94 if (!normals_)
95 {
96 PCL_ERROR( "[pcl::%s::segment] Must specify normals.\n", getClassName().c_str());
97 return;
98 }
99
100 // Check that we got the same number of points and normals
101 if (normals_->size () != input_->size ())
102 {
103 PCL_ERROR("[pcl::%s::segment] Number of points in input cloud (%zu) and normal "
104 "cloud (%zu) do not match!\n",
105 getClassName().c_str(),
106 static_cast<std::size_t>(input_->size()),
107 static_cast<std::size_t>(normals_->size()));
108 return;
109 }
110
111 // Check that the cloud is organized
112 if (!input_->isOrganized ())
113 {
114 PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
115 getClassName ().c_str ());
116 return;
117 }
118
119 // Calculate range part of planes' hessian normal form
120 std::vector<float> plane_d (input_->size ());
121
122 for (std::size_t i = 0; i < input_->size (); ++i)
123 plane_d[i] = (*input_)[i].getVector3fMap ().dot ((*normals_)[i].getNormalVector3fMap ());
124
125 // Make a comparator
126 //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
127 compare_->setPlaneCoeffD (plane_d);
128 compare_->setInputCloud (input_);
129 compare_->setInputNormals (normals_);
130 compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
131 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);
132
133 // Set up the output
134 OrganizedConnectedComponentSegmentation<PointT,PointLT> connected_component (compare_);
135 connected_component.setInputCloud (input_);
136 connected_component.segment (labels, label_indices);
137
138 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
139 Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
140 Eigen::Matrix3f clust_cov;
142 model.values.resize (4);
143
144 // Fit Planes to each cluster
145 for (const auto &label_index : label_indices)
146 {
147 if (static_cast<unsigned> (label_index.indices.size ()) > min_inliers_)
148 {
149 pcl::computeMeanAndCovarianceMatrix (*input_, label_index.indices, clust_cov, clust_centroid);
150 Eigen::Vector4f plane_params;
151
152 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
153 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
154 pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
155 plane_params[0] = eigen_vector[0];
156 plane_params[1] = eigen_vector[1];
157 plane_params[2] = eigen_vector[2];
158 plane_params[3] = 0;
159 plane_params[3] = -1 * plane_params.dot (clust_centroid);
160
161 vp -= clust_centroid;
162 float cos_theta = vp.dot (plane_params);
163 if (cos_theta < 0)
164 {
165 plane_params *= -1;
166 plane_params[3] = 0;
167 plane_params[3] = -1 * plane_params.dot (clust_centroid);
168 }
169
170 // Compute the curvature surface change
171 float curvature;
172 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
173 if (eig_sum != 0)
174 curvature = std::abs (eigen_value / eig_sum);
175 else
176 curvature = 0;
177
178 if (curvature < maximum_curvature_)
179 {
180 model.values[0] = plane_params[0];
181 model.values[1] = plane_params[1];
182 model.values[2] = plane_params[2];
183 model.values[3] = plane_params[3];
184 model_coefficients.push_back (model);
185 inlier_indices.push_back (label_index);
186 centroids.push_back (clust_centroid);
187 covariances.push_back (clust_cov);
188 }
189 }
190 }
191 deinitCompute ();
192}
193
194//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
195template<typename PointT, typename PointNT, typename PointLT> void
197{
198 std::vector<ModelCoefficients> model_coefficients;
199 std::vector<PointIndices> inlier_indices;
200 PointCloudLPtr labels (new PointCloudL);
201 std::vector<pcl::PointIndices> label_indices;
202 std::vector<pcl::PointIndices> boundary_indices;
203 pcl::PointCloud<PointT> boundary_cloud;
204 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
205 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
206 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
207 regions.resize (model_coefficients.size ());
208 boundary_indices.resize (model_coefficients.size ());
209
210 for (std::size_t i = 0; i < model_coefficients.size (); i++)
211 {
212 boundary_cloud.resize (0);
213 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]);
214 boundary_cloud.resize (boundary_indices[i].indices.size ());
215 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
216 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
217
218 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
219 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
220 model_coefficients[i].values[1],
221 model_coefficients[i].values[2],
222 model_coefficients[i].values[3]);
223 regions[i] = PlanarRegion<PointT> (centroid,
224 covariances[i],
225 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
226 boundary_cloud.points,
227 model);
228 }
229}
230
231//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
232template<typename PointT, typename PointNT, typename PointLT> void
234{
235 std::vector<ModelCoefficients> model_coefficients;
236 std::vector<PointIndices> inlier_indices;
237 PointCloudLPtr labels (new PointCloudL);
238 std::vector<pcl::PointIndices> label_indices;
239 std::vector<pcl::PointIndices> boundary_indices;
240 pcl::PointCloud<PointT> boundary_cloud;
241 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
242 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
243 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
244 refine (model_coefficients, inlier_indices, labels, label_indices);
245 regions.resize (model_coefficients.size ());
246 boundary_indices.resize (model_coefficients.size ());
247
248 for (std::size_t i = 0; i < model_coefficients.size (); i++)
249 {
250 boundary_cloud.resize (0);
251 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
252 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
253 boundary_cloud.resize (boundary_indices[i].indices.size ());
254 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
255 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
256
257 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
258 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
259 model_coefficients[i].values[1],
260 model_coefficients[i].values[2],
261 model_coefficients[i].values[3]);
262
263 Eigen::Vector3f vp (0.0, 0.0, 0.0);
264 if (project_points_)
265 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
266
267 regions[i] = PlanarRegion<PointT> (centroid,
268 covariances[i],
269 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
270 boundary_cloud.points,
271 model);
272 }
273}
275//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
276template<typename PointT, typename PointNT, typename PointLT> void
278 std::vector<ModelCoefficients>& model_coefficients,
279 std::vector<PointIndices>& inlier_indices,
280 PointCloudLPtr& labels,
281 std::vector<pcl::PointIndices>& label_indices,
282 std::vector<pcl::PointIndices>& boundary_indices)
283{
284 pcl::PointCloud<PointT> boundary_cloud;
285 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
286 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
287 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
288 refine (model_coefficients, inlier_indices, labels, label_indices);
289 regions.resize (model_coefficients.size ());
290 boundary_indices.resize (model_coefficients.size ());
291
292 for (std::size_t i = 0; i < model_coefficients.size (); i++)
293 {
294 boundary_cloud.resize (0);
295 int max_inlier_idx = static_cast<int> (inlier_indices[i].indices.size ()) - 1;
296 pcl::OrganizedConnectedComponentSegmentation<PointT,PointLT>::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]);
297 boundary_cloud.resize (boundary_indices[i].indices.size ());
298 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
299 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
300
301 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
302 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
303 model_coefficients[i].values[1],
304 model_coefficients[i].values[2],
305 model_coefficients[i].values[3]);
306
307 Eigen::Vector3f vp (0.0, 0.0, 0.0);
308 if (project_points_ && !boundary_cloud.empty ())
309 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
310
311 regions[i] = PlanarRegion<PointT> (centroid,
312 covariances[i],
313 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
314 boundary_cloud.points,
315 model);
316 }
317}
318
319//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320template<typename PointT, typename PointNT, typename PointLT> void
321pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::refine (std::vector<ModelCoefficients>& model_coefficients,
322 std::vector<PointIndices>& inlier_indices,
323 PointCloudLPtr& labels,
324 std::vector<pcl::PointIndices>& label_indices)
325{
326 //List of labels to grow, and index of model corresponding to each label
327 std::vector<bool> grow_labels;
328 std::vector<int> label_to_model;
329 grow_labels.resize (label_indices.size (), false);
330 label_to_model.resize (label_indices.size (), 0);
331
332 for (std::size_t i = 0; i < model_coefficients.size (); i++)
333 {
334 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
335 label_to_model[model_label] = static_cast<int> (i);
336 grow_labels[model_label] = true;
337 }
338
339 //refinement_compare_->setDistanceThreshold (0.015f, true);
340 refinement_compare_->setInputCloud (input_);
341 refinement_compare_->setLabels (labels);
342 refinement_compare_->setModelCoefficients (model_coefficients);
343 refinement_compare_->setRefineLabels (grow_labels);
344 refinement_compare_->setLabelToModel (label_to_model);
345
346 //Do a first pass over the image, top to bottom, left to right
347 unsigned int current_row = 0;
348 unsigned int next_row = labels->width;
349 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
350 {
351
352 for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
353 {
354 int current_label = (*labels)[current_row+colIdx].label;
355 int right_label = (*labels)[current_row+colIdx+1].label;
356 if (current_label < 0 || right_label < 0)
357 continue;
358
359 //Check right
360 //bool test1 = false;
361 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
362 {
363 //test1 = true;
364 (*labels)[current_row+colIdx+1].label = current_label;
365 label_indices[current_label].indices.push_back (current_row+colIdx+1);
366 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
367 }
368
369 int lower_label = (*labels)[next_row+colIdx].label;
370 if (lower_label < 0)
371 continue;
372
373 //Check down
374 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
375 {
376 (*labels)[next_row+colIdx].label = current_label;
377 label_indices[current_label].indices.push_back (next_row+colIdx);
378 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
379 }
380
381 }//col
382 }//row
383
384 //Do a second pass over the image
385 current_row = labels->width * (labels->height - 1);
386 unsigned int prev_row = current_row - labels->width;
387 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
388 {
389 for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
390 {
391 int current_label = (*labels)[current_row+colIdx].label;
392 int left_label = (*labels)[current_row+colIdx-1].label;
393 if (current_label < 0 || left_label < 0)
394 continue;
395
396 //Check left
397 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
398 {
399 (*labels)[current_row+colIdx-1].label = current_label;
400 label_indices[current_label].indices.push_back (current_row+colIdx-1);
401 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
402 }
403
404 int upper_label = (*labels)[prev_row+colIdx].label;
405 if (upper_label < 0)
406 continue;
407 //Check up
408 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
409 {
410 (*labels)[prev_row+colIdx].label = current_label;
411 label_indices[current_label].indices.push_back (prev_row+colIdx);
412 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
413 }
414 }//col
415 }//row
416}
417
418#define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
419
420#endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
Define methods for centroid estimation and covariance matrix calculus.
OrganizedConnectedComponentSegmentation allows connected components to be found within organized poin...
static void findLabeledRegionBoundary(int start_idx, PointCloudLPtr labels, pcl::PointIndices &boundary_indices)
Find the boundary points / contour of a connected component.
void segment(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the connected component segmentation.
void segment(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &centroids, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &covariances, pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices)
Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
void segmentAndRefine(std::vector< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > &regions)
Perform a segmentation, as well as an additional refinement step.
void refine(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, PointCloudLPtr &labels, std::vector< pcl::PointIndices > &label_indices)
Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
PlanarRegion represents a set of points that lie in a plane.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool empty() const
void resize(std::size_t count)
Resizes the container to contain count elements.
std::size_t size() const
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition centroid.hpp:508
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition eigen.hpp:296
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
std::vector< float > values