40 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41 #define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/organized_connected_component_segmentation.h>
45 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
46 #include <pcl/common/centroid.h>
47 #include <pcl/common/eigen.h>
51 projectToPlaneFromViewpoint (
pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
53 Eigen::Vector3f norm (normal[0], normal[1], normal[2]);
56 for (
size_t i = 0; i < cloud.
points.size (); i++)
60 float u = norm.dot ((centroid - vp)) / norm.dot ((pt - vp));
61 Eigen::Vector3f intersection (vp + u * (pt - vp));
62 projected_cloud[i].x = intersection[0];
63 projected_cloud[i].y = intersection[1];
64 projected_cloud[i].z = intersection[2];
67 return (projected_cloud);
71 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
73 std::vector<PointIndices>& inlier_indices)
76 std::vector<pcl::PointIndices> label_indices;
77 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
78 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
79 segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
83 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
85 std::vector<PointIndices>& inlier_indices,
86 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
87 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
89 std::vector<pcl::PointIndices>& label_indices)
95 if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
97 PCL_ERROR (
"[pcl::%s::segment] Number of points in input cloud (%lu) and normal cloud (%lu) do not match!\n",
98 getClassName ().c_str (), input_->points.size (),
99 normals_->points.size ());
104 if (!input_->isOrganized ())
106 PCL_ERROR (
"[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
107 getClassName ().c_str ());
112 std::vector<float> plane_d (input_->points.size ());
114 for (
unsigned int i = 0; i < input_->size (); ++i)
115 plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
119 compare_->setPlaneCoeffD (plane_d);
120 compare_->setInputCloud (input_);
121 compare_->setInputNormals (normals_);
122 compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
123 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_),
true);
128 connected_component.
segment (labels, label_indices);
130 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
131 Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
132 Eigen::Matrix3f clust_cov;
137 for (
size_t i = 0; i < label_indices.size (); i++)
139 if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
142 Eigen::Vector4f plane_params;
147 plane_params[0] = eigen_vector[0];
148 plane_params[1] = eigen_vector[1];
149 plane_params[2] = eigen_vector[2];
151 plane_params[3] = -1 * plane_params.dot (clust_centroid);
153 vp -= clust_centroid;
154 float cos_theta = vp.dot (plane_params);
159 plane_params[3] = -1 * plane_params.dot (clust_centroid);
164 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
166 curvature = fabsf (eigen_value / eig_sum);
170 if (curvature < maximum_curvature_)
172 model.
values[0] = plane_params[0];
173 model.
values[1] = plane_params[1];
174 model.
values[2] = plane_params[2];
175 model.
values[3] = plane_params[3];
176 model_coefficients.push_back (model);
177 inlier_indices.push_back (label_indices[i]);
178 centroids.push_back (clust_centroid);
179 covariances.push_back (clust_cov);
187 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
190 std::vector<ModelCoefficients> model_coefficients;
191 std::vector<PointIndices> inlier_indices;
193 std::vector<pcl::PointIndices> label_indices;
194 std::vector<pcl::PointIndices> boundary_indices;
196 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
197 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
198 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
199 regions.resize (model_coefficients.size ());
200 boundary_indices.resize (model_coefficients.size ());
202 for (
size_t i = 0; i < model_coefficients.size (); i++)
204 boundary_cloud.
resize (0);
206 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
207 for (
unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
208 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
210 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
211 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
212 model_coefficients[i].values[1],
213 model_coefficients[i].values[2],
214 model_coefficients[i].values[3]);
217 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
224 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
227 std::vector<ModelCoefficients> model_coefficients;
228 std::vector<PointIndices> inlier_indices;
230 std::vector<pcl::PointIndices> label_indices;
231 std::vector<pcl::PointIndices> boundary_indices;
233 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
234 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
235 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
236 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
237 regions.resize (model_coefficients.size ());
238 boundary_indices.resize (model_coefficients.size ());
240 for (
size_t i = 0; i < model_coefficients.size (); i++)
242 boundary_cloud.
resize (0);
243 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
245 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
246 for (
unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
247 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
249 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
250 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
251 model_coefficients[i].values[1],
252 model_coefficients[i].values[2],
253 model_coefficients[i].values[3]);
255 Eigen::Vector3f vp (0.0, 0.0, 0.0);
257 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
261 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
268 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
270 std::vector<ModelCoefficients>& model_coefficients,
271 std::vector<PointIndices>& inlier_indices,
273 std::vector<pcl::PointIndices>& label_indices,
274 std::vector<pcl::PointIndices>& boundary_indices)
277 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
278 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
279 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
280 refine (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices);
281 regions.resize (model_coefficients.size ());
282 boundary_indices.resize (model_coefficients.size ());
284 for (
size_t i = 0; i < model_coefficients.size (); i++)
286 boundary_cloud.
resize (0);
287 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
289 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
290 for (
unsigned j = 0; j < boundary_indices[i].indices.size (); j++)
291 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
293 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
294 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
295 model_coefficients[i].values[1],
296 model_coefficients[i].values[2],
297 model_coefficients[i].values[3]);
299 Eigen::Vector3f vp (0.0, 0.0, 0.0);
300 if (project_points_ && boundary_cloud.
points.size () > 0)
301 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
305 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
312 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
314 std::vector<PointIndices>& inlier_indices,
315 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&,
316 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&,
318 std::vector<pcl::PointIndices>& label_indices)
321 std::vector<bool> grow_labels;
322 std::vector<int> label_to_model;
323 grow_labels.resize (label_indices.size (),
false);
324 label_to_model.resize (label_indices.size (), 0);
326 for (
size_t i = 0; i < model_coefficients.size (); i++)
328 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
329 label_to_model[model_label] =
static_cast<int> (i);
330 grow_labels[model_label] =
true;
334 refinement_compare_->setInputCloud (input_);
335 refinement_compare_->setLabels (labels);
336 refinement_compare_->setModelCoefficients (model_coefficients);
337 refinement_compare_->setRefineLabels (grow_labels);
338 refinement_compare_->setLabelToModel (label_to_model);
341 unsigned int current_row = 0;
342 unsigned int next_row = labels->width;
343 for (
size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
346 for (
unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
348 int current_label = (*labels)[current_row+colIdx].label;
349 int right_label = (*labels)[current_row+colIdx+1].label;
350 if (current_label < 0 || right_label < 0)
355 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
358 labels->points[current_row+colIdx+1].label = current_label;
359 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
360 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
363 int lower_label = (*labels)[next_row+colIdx].label;
368 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
370 labels->points[next_row+colIdx].label = current_label;
371 label_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
372 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
379 current_row = labels->width * (labels->height - 1);
380 unsigned int prev_row = current_row - labels->width;
381 for (
size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
383 for (
int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
385 int current_label = (*labels)[current_row+colIdx].label;
386 int left_label = (*labels)[current_row+colIdx-1].label;
387 if (current_label < 0 || left_label < 0)
391 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
393 labels->points[current_row+colIdx-1].label = current_label;
394 label_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
395 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
398 int upper_label = (*labels)[prev_row+colIdx].label;
402 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
404 labels->points[prev_row+colIdx].label = current_label;
405 label_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
406 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
412 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
414 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
void segment(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > ¢roids, 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() ...
struct pcl::_PointXYZHSV EIGEN_ALIGN16
OrganizedConnectedComponentSegmentation allows connected components to be found within organized poin...
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void segmentAndRefine(std::vector< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > ®ions)
Perform a segmentation, as well as an additional refinement step.
std::vector< float > values
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
PointCloudL::Ptr PointCloudLPtr
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void refine(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > ¢roids, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &covariances, PointCloudLPtr &labels, std::vector< pcl::PointIndices > &label_indices)
Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by ...
PlanarRegion represents a set of points that lie in a plane.
void resize(size_t n)
Resize the cloud.