40#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41#define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
43#include <pcl/segmentation/organized_connected_component_segmentation.h>
44#include <pcl/segmentation/organized_multi_plane_segmentation.h>
46#include <pcl/common/eigen.h>
50projectToPlaneFromViewpoint (
pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp)
52 Eigen::Vector3f
norm (normal[0], normal[1], normal[2]);
54 projected_cloud.resize (cloud.
size ());
55 for (std::size_t i = 0; i < cloud.
size (); i++)
57 Eigen::Vector3f pt (cloud[i].x, cloud[i].y, cloud[i].z);
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];
66 return (projected_cloud);
70template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
76 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
centroids;
77 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >
covariances;
82template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
85 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >&
centroids,
86 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >&
covariances,
96 PCL_ERROR(
"[pcl::%s::segment] Must specify normals.\n", getClassName().
c_str());
101 if (normals_->size () != input_->size ())
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()));
112 if (!input_->isOrganized ())
114 PCL_ERROR (
"[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
115 getClassName ().
c_str ());
120 std::vector<float>
plane_d (input_->size ());
122 for (std::size_t i = 0; i < input_->size (); ++i)
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);
139 Eigen::Vector4f
vp = Eigen::Vector4f::Zero ();
142 model.values.resize (4);
147 if (
static_cast<unsigned> (
label_index.indices.
size ()) > min_inliers_)
178 if (curvature < maximum_curvature_)
195template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
204 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
centroids;
205 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >
covariances;
232template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
241 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
centroids;
242 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >
covariances;
263 Eigen::Vector3f
vp (0.0, 0.0, 0.0);
276template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
285 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
centroids;
286 std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >
covariances;
307 Eigen::Vector3f
vp (0.0, 0.0, 0.0);
320template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
340 refinement_compare_->setInputCloud (input_);
341 refinement_compare_->setLabels (labels);
343 refinement_compare_->setRefineLabels (
grow_labels);
348 unsigned int next_row = labels->width;
385 current_row = labels->width * (labels->height - 1);
418#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.
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 > > ¢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 ...
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 > ¢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)