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;
52 Eigen::Vector4f xyz_centroid;
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 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
83 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
89 for (
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;
168 Eigen::Vector4f xyz_centroid;
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 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
210 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
215 polygon.
points.resize (planar_hull_->points.size ());
216 for (
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 (
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
struct pcl::PointXYZIEdge EIGEN_ALIGN16
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< int > indices
void projectPoints(const std::vector< int > &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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
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...