38 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
39 #define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
41 #include <pcl/segmentation/extract_polygonal_prism_data.h>
42 #include <pcl/common/centroid.h>
43 #include <pcl/common/eigen.h>
46 template <
typename Po
intT>
bool
50 Eigen::Vector4f model_coefficients;
51 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
52 Eigen::Vector4f xyz_centroid;
57 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
58 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
59 eigen33 (covariance_matrix, eigen_value, eigen_vector);
61 model_coefficients[0] = eigen_vector [0];
62 model_coefficients[1] = eigen_vector [1];
63 model_coefficients[2] = eigen_vector [2];
64 model_coefficients[3] = 0;
67 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
69 float distance_to_plane = model_coefficients[0] * point.x +
70 model_coefficients[1] * point.y +
71 model_coefficients[2] * point.z +
72 model_coefficients[3];
75 ppoint.x = point.x - distance_to_plane * model_coefficients[0];
76 ppoint.y = point.y - distance_to_plane * model_coefficients[1];
77 ppoint.z = point.z - distance_to_plane * model_coefficients[2];
82 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
83 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
89 for (std::size_t i = 0; i < polygon.
points.size (); ++i)
92 xy_polygon.
points[i].x = pt[k1];
93 xy_polygon.
points[i].y = pt[k2];
94 xy_polygon.
points[i].z = 0;
98 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
106 template <
typename Po
intT>
bool
109 bool in_poly =
false;
110 double x1, x2, y1, y2;
112 int nr_poly_points =
static_cast<int> (polygon.
points.size ());
114 double xold = polygon.
points[nr_poly_points - 1].x;
115 double yold = polygon.
points[nr_poly_points - 1].y;
116 for (
int i = 0; i < nr_poly_points; i++)
118 double xnew = polygon.
points[i].x;
119 double ynew = polygon.
points[i].y;
135 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
147 template <
typename Po
intT>
void
150 output.
header = input_->header;
158 if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_)
160 PCL_ERROR (
"[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ());
166 Eigen::Vector4f model_coefficients;
167 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
168 Eigen::Vector4f xyz_centroid;
173 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
174 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
175 eigen33 (covariance_matrix, eigen_value, eigen_vector);
177 model_coefficients[0] = eigen_vector [0];
178 model_coefficients[1] = eigen_vector [1];
179 model_coefficients[2] = eigen_vector [2];
180 model_coefficients[3] = 0;
183 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
186 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
188 vp -= planar_hull_->points[0].getVector4fMap ();
191 float cos_theta = vp.dot (model_coefficients);
195 model_coefficients *= -1;
196 model_coefficients[3] = 0;
198 model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
204 sacmodel.
projectPoints (*indices_, model_coefficients, projected_points,
false);
209 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
210 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
215 polygon.
points.resize (planar_hull_->points.size ());
216 for (std::size_t i = 0; i < planar_hull_->points.size (); ++i)
218 Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
219 polygon.
points[i].x = pt[k1];
220 polygon.
points[i].y = pt[k2];
227 output.
indices.resize (indices_->size ());
229 for (std::size_t i = 0; i < projected_points.
points.size (); ++i)
233 if (distance < height_limit_min_ || distance > height_limit_max_)
237 Eigen::Vector4f pt (projected_points.
points[i].x,
238 projected_points.
points[i].y,
239 projected_points.
points[i].z, 0);
246 output.
indices[l++] = (*indices_)[i];
253 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
254 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
255 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
257 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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. ...
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the plane model.
A point structure representing Euclidean xyz coordinates, and the RGB color.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
bool isPointIn2DPolygon(const PointT &point, const pcl::PointCloud< PointT > &polygon)
General purpose method for checking if a 3D point is inside or outside a given 2D polygon...
bool isXYPointIn2DXYPolygon(const PointT &point, const pcl::PointCloud< PointT > &polygon)
Check if a 2d point (X and Y coordinates considered only!) is inside or outside a given polygon...