Point Cloud Library (PCL) 1.12.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
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;
79}
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template<typename PointT, typename PointNT, typename PointLT> void
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
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 {
150 Eigen::Vector4f plane_params;
151
152 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
153 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
158 plane_params[3] = 0;
160
162 float cos_theta = vp.dot (plane_params);
163 if (cos_theta < 0)
164 {
165 plane_params *= -1;
166 plane_params[3] = 0;
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;
204 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
205 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
207 regions.resize (model_coefficients.size ());
209
210 for (std::size_t i = 0; i < model_coefficients.size (); i++)
211 {
212 boundary_cloud.resize (0);
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;
238 std::vector<pcl::PointIndices> label_indices;
239 std::vector<pcl::PointIndices> boundary_indices;
241 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
242 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
245 regions.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;
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]);
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}
274
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{
285 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
286 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
289 regions.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;
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
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);
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);
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);
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);
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.
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
static void findLabeledRegionBoundary(int start_idx, PointCloudLPtr labels, pcl::PointIndices &boundary_indices)
Find the boundary points / contour of a connected component.
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 ...
PointCloud represents the base class in PCL for storing collections of 3D points.
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:485
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)