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>
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 (std::size_t i = 0; i < cloud.
size (); i++)
58 Eigen::Vector3f pt (cloud[i].x, cloud[i].y, cloud[i].z);
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)
97 PCL_ERROR(
"[pcl::%s::segment] Must specify normals.\n", getClassName().c_str());
102 if (normals_->size () != input_->size ())
104 PCL_ERROR(
"[pcl::%s::segment] Number of points in input cloud (%zu) and normal "
105 "cloud (%zu) do not match!\n",
106 getClassName().c_str(),
107 static_cast<std::size_t
>(input_->size()),
108 static_cast<std::size_t
>(normals_->size()));
113 if (!input_->isOrganized ())
115 PCL_ERROR (
"[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
116 getClassName ().c_str ());
121 std::vector<float> plane_d (input_->size ());
123 for (std::size_t i = 0; i < input_->size (); ++i)
124 plane_d[i] = (*input_)[i].getVector3fMap ().dot ((*normals_)[i].getNormalVector3fMap ());
128 compare_->setPlaneCoeffD (plane_d);
129 compare_->setInputCloud (input_);
130 compare_->setInputNormals (normals_);
131 compare_->setAngularThreshold (
static_cast<float> (angular_threshold_));
132 compare_->setDistanceThreshold (
static_cast<float> (distance_threshold_),
true);
137 connected_component.
segment (labels, label_indices);
139 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
140 Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
141 Eigen::Matrix3f clust_cov;
146 for (
const auto &label_index : label_indices)
148 if (
static_cast<unsigned> (label_index.indices.size ()) > min_inliers_)
151 Eigen::Vector4f plane_params;
153 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
154 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
156 plane_params[0] = eigen_vector[0];
157 plane_params[1] = eigen_vector[1];
158 plane_params[2] = eigen_vector[2];
160 plane_params[3] = -1 * plane_params.dot (clust_centroid);
162 vp -= clust_centroid;
163 float cos_theta = vp.dot (plane_params);
168 plane_params[3] = -1 * plane_params.dot (clust_centroid);
173 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
175 curvature = std::abs (eigen_value / eig_sum);
179 if (curvature < maximum_curvature_)
181 model.
values[0] = plane_params[0];
182 model.
values[1] = plane_params[1];
183 model.
values[2] = plane_params[2];
184 model.
values[3] = plane_params[3];
185 model_coefficients.push_back (model);
186 inlier_indices.push_back (label_index);
187 centroids.push_back (clust_centroid);
188 covariances.push_back (clust_cov);
196 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
199 std::vector<ModelCoefficients> model_coefficients;
200 std::vector<PointIndices> inlier_indices;
202 std::vector<pcl::PointIndices> label_indices;
203 std::vector<pcl::PointIndices> boundary_indices;
205 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
206 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
207 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
208 regions.resize (model_coefficients.size ());
209 boundary_indices.resize (model_coefficients.size ());
211 for (std::size_t i = 0; i < model_coefficients.size (); i++)
213 boundary_cloud.
resize (0);
215 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
216 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
217 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
219 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
220 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
221 model_coefficients[i].values[1],
222 model_coefficients[i].values[2],
223 model_coefficients[i].values[3]);
226 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
233 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
236 std::vector<ModelCoefficients> model_coefficients;
237 std::vector<PointIndices> inlier_indices;
239 std::vector<pcl::PointIndices> label_indices;
240 std::vector<pcl::PointIndices> boundary_indices;
242 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
243 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
244 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
245 refine (model_coefficients, inlier_indices, labels, label_indices);
246 regions.resize (model_coefficients.size ());
247 boundary_indices.resize (model_coefficients.size ());
249 for (std::size_t i = 0; i < model_coefficients.size (); i++)
251 boundary_cloud.
resize (0);
252 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
254 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
255 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
256 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
258 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
259 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
260 model_coefficients[i].values[1],
261 model_coefficients[i].values[2],
262 model_coefficients[i].values[3]);
264 Eigen::Vector3f vp (0.0, 0.0, 0.0);
266 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
270 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
277 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
279 std::vector<ModelCoefficients>& model_coefficients,
280 std::vector<PointIndices>& inlier_indices,
282 std::vector<pcl::PointIndices>& label_indices,
283 std::vector<pcl::PointIndices>& boundary_indices)
286 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
287 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
288 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
289 refine (model_coefficients, inlier_indices, labels, label_indices);
290 regions.resize (model_coefficients.size ());
291 boundary_indices.resize (model_coefficients.size ());
293 for (std::size_t i = 0; i < model_coefficients.size (); i++)
295 boundary_cloud.
resize (0);
296 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
298 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
299 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
300 boundary_cloud[j] = (*input_)[boundary_indices[i].indices[j]];
302 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
303 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
304 model_coefficients[i].values[1],
305 model_coefficients[i].values[2],
306 model_coefficients[i].values[3]);
308 Eigen::Vector3f vp (0.0, 0.0, 0.0);
309 if (project_points_ && !boundary_cloud.
points.empty ())
310 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
314 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
321 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
323 std::vector<PointIndices>& inlier_indices,
325 std::vector<pcl::PointIndices>& label_indices)
328 std::vector<bool> grow_labels;
329 std::vector<int> label_to_model;
330 grow_labels.resize (label_indices.size (),
false);
331 label_to_model.resize (label_indices.size (), 0);
333 for (std::size_t i = 0; i < model_coefficients.size (); i++)
335 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
336 label_to_model[model_label] =
static_cast<int> (i);
337 grow_labels[model_label] =
true;
341 refinement_compare_->setInputCloud (input_);
342 refinement_compare_->setLabels (labels);
343 refinement_compare_->setModelCoefficients (model_coefficients);
344 refinement_compare_->setRefineLabels (grow_labels);
345 refinement_compare_->setLabelToModel (label_to_model);
348 unsigned int current_row = 0;
349 unsigned int next_row = labels->width;
350 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
353 for (
unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
355 int current_label = (*labels)[current_row+colIdx].label;
356 int right_label = (*labels)[current_row+colIdx+1].label;
357 if (current_label < 0 || right_label < 0)
362 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
365 (*labels)[current_row+colIdx+1].label = current_label;
366 label_indices[current_label].indices.push_back (current_row+colIdx+1);
367 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
370 int lower_label = (*labels)[next_row+colIdx].label;
375 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
377 (*labels)[next_row+colIdx].label = current_label;
378 label_indices[current_label].indices.push_back (next_row+colIdx);
379 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
386 current_row = labels->width * (labels->height - 1);
387 unsigned int prev_row = current_row - labels->width;
388 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
390 for (
int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
392 int current_label = (*labels)[current_row+colIdx].label;
393 int left_label = (*labels)[current_row+colIdx-1].label;
394 if (current_label < 0 || left_label < 0)
398 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
400 (*labels)[current_row+colIdx-1].label = current_label;
401 label_indices[current_label].indices.push_back (current_row+colIdx-1);
402 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
405 int upper_label = (*labels)[prev_row+colIdx].label;
409 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
411 (*labels)[prev_row+colIdx].label = current_label;
412 label_indices[current_label].indices.push_back (prev_row+colIdx);
413 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
419 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
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< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > ®ions)
Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
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()
typename PointCloudL::Ptr PointCloudLPtr
void segmentAndRefine(std::vector< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > ®ions)
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.
PlanarRegion represents a set of points that lie in a plane.
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
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 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
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...
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
std::vector< float > values