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 (std::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)
97 PCL_ERROR(
"[pcl::%s::segment] Must specify normals.\n", getClassName().c_str());
102 if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
104 PCL_ERROR (
"[pcl::%s::segment] Number of points in input cloud (%lu) and normal cloud (%lu) do not match!\n",
105 getClassName ().c_str (), input_->points.size (),
106 normals_->points.size ());
111 if (!input_->isOrganized ())
113 PCL_ERROR (
"[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
114 getClassName ().c_str ());
119 std::vector<float> plane_d (input_->points.size ());
121 for (std::size_t i = 0; i < input_->size (); ++i)
122 plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
126 compare_->setPlaneCoeffD (plane_d);
127 compare_->setInputCloud (input_);
128 compare_->setInputNormals (normals_);
129 compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
130 compare_->setDistanceThreshold (static_cast<float> (distance_threshold_),
true);
135 connected_component.
segment (labels, label_indices);
137 Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
138 Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
139 Eigen::Matrix3f clust_cov;
144 for (
const auto &label_index : label_indices)
146 if (static_cast<unsigned> (label_index.indices.size ()) > min_inliers_)
149 Eigen::Vector4f plane_params;
151 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
152 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
154 plane_params[0] = eigen_vector[0];
155 plane_params[1] = eigen_vector[1];
156 plane_params[2] = eigen_vector[2];
158 plane_params[3] = -1 * plane_params.dot (clust_centroid);
160 vp -= clust_centroid;
161 float cos_theta = vp.dot (plane_params);
166 plane_params[3] = -1 * plane_params.dot (clust_centroid);
171 float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
173 curvature = std::abs (eigen_value / eig_sum);
177 if (curvature < maximum_curvature_)
179 model.
values[0] = plane_params[0];
180 model.
values[1] = plane_params[1];
181 model.
values[2] = plane_params[2];
182 model.
values[3] = plane_params[3];
183 model_coefficients.push_back (model);
184 inlier_indices.push_back (label_index);
185 centroids.push_back (clust_centroid);
186 covariances.push_back (clust_cov);
194 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
197 std::vector<ModelCoefficients> model_coefficients;
198 std::vector<PointIndices> inlier_indices;
200 std::vector<pcl::PointIndices> label_indices;
201 std::vector<pcl::PointIndices> boundary_indices;
203 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
204 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
205 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
206 regions.resize (model_coefficients.size ());
207 boundary_indices.resize (model_coefficients.size ());
209 for (std::size_t i = 0; i < model_coefficients.size (); i++)
211 boundary_cloud.
resize (0);
213 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
214 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
215 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
217 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
218 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
219 model_coefficients[i].values[1],
220 model_coefficients[i].values[2],
221 model_coefficients[i].values[3]);
224 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
231 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
234 std::vector<ModelCoefficients> model_coefficients;
235 std::vector<PointIndices> inlier_indices;
237 std::vector<pcl::PointIndices> label_indices;
238 std::vector<pcl::PointIndices> boundary_indices;
240 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
241 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
242 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
243 refine (model_coefficients, inlier_indices, labels, label_indices);
244 regions.resize (model_coefficients.size ());
245 boundary_indices.resize (model_coefficients.size ());
247 for (std::size_t i = 0; i < model_coefficients.size (); i++)
249 boundary_cloud.
resize (0);
250 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
252 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
253 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
254 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
256 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
257 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
258 model_coefficients[i].values[1],
259 model_coefficients[i].values[2],
260 model_coefficients[i].values[3]);
262 Eigen::Vector3f vp (0.0, 0.0, 0.0);
264 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
268 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
275 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
277 std::vector<ModelCoefficients>& model_coefficients,
278 std::vector<PointIndices>& inlier_indices,
280 std::vector<pcl::PointIndices>& label_indices,
281 std::vector<pcl::PointIndices>& boundary_indices)
284 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
285 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
286 segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices);
287 refine (model_coefficients, inlier_indices, labels, label_indices);
288 regions.resize (model_coefficients.size ());
289 boundary_indices.resize (model_coefficients.size ());
291 for (std::size_t i = 0; i < model_coefficients.size (); i++)
293 boundary_cloud.
resize (0);
294 int max_inlier_idx =
static_cast<int> (inlier_indices[i].indices.size ()) - 1;
296 boundary_cloud.
points.resize (boundary_indices[i].indices.size ());
297 for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++)
298 boundary_cloud.
points[j] = input_->points[boundary_indices[i].indices[j]];
300 Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]);
301 Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0],
302 model_coefficients[i].values[1],
303 model_coefficients[i].values[2],
304 model_coefficients[i].values[3]);
306 Eigen::Vector3f vp (0.0, 0.0, 0.0);
307 if (project_points_ && !boundary_cloud.
points.empty ())
308 boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp);
312 static_cast<unsigned int> (inlier_indices[i].indices.size ()),
319 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
321 std::vector<PointIndices>& inlier_indices,
323 std::vector<pcl::PointIndices>& label_indices)
326 std::vector<bool> grow_labels;
327 std::vector<int> label_to_model;
328 grow_labels.resize (label_indices.size (),
false);
329 label_to_model.resize (label_indices.size (), 0);
331 for (std::size_t i = 0; i < model_coefficients.size (); i++)
333 int model_label = (*labels)[inlier_indices[i].indices[0]].label;
334 label_to_model[model_label] =
static_cast<int> (i);
335 grow_labels[model_label] =
true;
339 refinement_compare_->setInputCloud (input_);
340 refinement_compare_->setLabels (labels);
341 refinement_compare_->setModelCoefficients (model_coefficients);
342 refinement_compare_->setRefineLabels (grow_labels);
343 refinement_compare_->setLabelToModel (label_to_model);
346 unsigned int current_row = 0;
347 unsigned int next_row = labels->width;
348 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width)
351 for (
unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx)
353 int current_label = (*labels)[current_row+colIdx].label;
354 int right_label = (*labels)[current_row+colIdx+1].label;
355 if (current_label < 0 || right_label < 0)
360 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1))
363 labels->points[current_row+colIdx+1].label = current_label;
364 label_indices[current_label].indices.push_back (current_row+colIdx+1);
365 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1);
368 int lower_label = (*labels)[next_row+colIdx].label;
373 if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx))
375 labels->points[next_row+colIdx].label = current_label;
376 label_indices[current_label].indices.push_back (next_row+colIdx);
377 inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx);
384 current_row = labels->width * (labels->height - 1);
385 unsigned int prev_row = current_row - labels->width;
386 for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width)
388 for (
int colIdx = labels->width - 1; colIdx >= 0; --colIdx)
390 int current_label = (*labels)[current_row+colIdx].label;
391 int left_label = (*labels)[current_row+colIdx-1].label;
392 if (current_label < 0 || left_label < 0)
396 if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1))
398 labels->points[current_row+colIdx-1].label = current_label;
399 label_indices[current_label].indices.push_back (current_row+colIdx-1);
400 inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1);
403 int upper_label = (*labels)[prev_row+colIdx].label;
407 if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx))
409 labels->points[prev_row+colIdx].label = current_label;
410 label_indices[current_label].indices.push_back (prev_row+colIdx);
411 inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx);
417 #define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation<T,NT,LT>;
419 #endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_
typename PointCloudL::Ptr PointCloudLPtr
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() ...
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.
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 ...
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 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.
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
PlanarRegion represents a set of points that lie in a plane.
void resize(std::size_t n)
Resize the cloud.