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>
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.
size (); ++i)
91 Eigen::Vector4f pt (polygon[i].x, polygon[i].y, polygon[i].z, 0);
92 xy_polygon[i].x = pt[k1];
93 xy_polygon[i].y = pt[k2];
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 const auto nr_poly_points = polygon.
size ();
114 double xold = polygon[nr_poly_points - 1].x;
115 double yold = polygon[nr_poly_points - 1].y;
116 for (std::size_t i = 0; i < nr_poly_points; i++)
118 double xnew = polygon[i].x;
119 double ynew = polygon[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_->size ()) < min_pts_hull_)
160 PCL_ERROR(
"[pcl::%s::segment] Not enough points (%zu) in the hull!\n",
161 getClassName().c_str(),
162 static_cast<std::size_t
>(planar_hull_->size()));
168 Eigen::Vector4f model_coefficients;
169 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
170 Eigen::Vector4f xyz_centroid;
175 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
176 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
177 eigen33 (covariance_matrix, eigen_value, eigen_vector);
179 model_coefficients[0] = eigen_vector [0];
180 model_coefficients[1] = eigen_vector [1];
181 model_coefficients[2] = eigen_vector [2];
182 model_coefficients[3] = 0;
185 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
188 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
190 vp -= (*planar_hull_)[0].getVector4fMap ();
193 float cos_theta = vp.dot (model_coefficients);
197 model_coefficients *= -1;
198 model_coefficients[3] = 0;
200 model_coefficients[3] = -1 * (model_coefficients.dot ((*planar_hull_)[0].getVector4fMap ()));
206 sacmodel.
projectPoints (*indices_, model_coefficients, projected_points,
false);
211 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
212 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
217 polygon.
resize (planar_hull_->size ());
218 for (std::size_t i = 0; i < planar_hull_->size (); ++i)
220 Eigen::Vector4f pt ((*planar_hull_)[i].x, (*planar_hull_)[i].y, (*planar_hull_)[i].z, 0);
221 polygon[i].x = pt[k1];
222 polygon[i].y = pt[k2];
229 output.
indices.resize (indices_->size ());
231 for (std::size_t i = 0; i < projected_points.
size (); ++i)
235 if (distance < height_limit_min_ || distance > height_limit_max_)
239 Eigen::Vector4f pt (projected_points[i].x,
240 projected_points[i].y,
241 projected_points[i].z, 0);
248 output.
indices[l++] = (*indices_)[i];
255 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
256 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
257 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
259 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_