Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
extract_polygonal_prism_data.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#ifndef PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
39#define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
40
41#include <pcl/segmentation/extract_polygonal_prism_data.h>
42#include <pcl/sample_consensus/sac_model_plane.h> // for SampleConsensusModelPlane
43#include <pcl/common/centroid.h>
44#include <pcl/common/eigen.h>
45
46//////////////////////////////////////////////////////////////////////////
47template <typename PointT> bool
49{
50 // Compute the plane coefficients
51 Eigen::Vector4f model_coefficients;
52 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
53 Eigen::Vector4f xyz_centroid;
54
55 computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid);
56
57 // Compute the model coefficients
58 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
59 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
60 eigen33 (covariance_matrix, eigen_value, eigen_vector);
61
62 model_coefficients[0] = eigen_vector [0];
63 model_coefficients[1] = eigen_vector [1];
64 model_coefficients[2] = eigen_vector [2];
65 model_coefficients[3] = 0;
66
67 // Hessian form (D = nc . p_plane (centroid here) + p)
68 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
69
70 float distance_to_plane = model_coefficients[0] * point.x +
71 model_coefficients[1] * point.y +
72 model_coefficients[2] * point.z +
73 model_coefficients[3];
74 PointT ppoint;
75 // Calculate the projection of the point on the plane
76 ppoint.x = point.x - distance_to_plane * model_coefficients[0];
77 ppoint.y = point.y - distance_to_plane * model_coefficients[1];
78 ppoint.z = point.z - distance_to_plane * model_coefficients[2];
79
80 // Create a X-Y projected representation for within bounds polygonal checking
81 int k0, k1, k2;
82 // Determine the best plane to project points onto
83 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
84 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
85 k1 = (k0 + 1) % 3;
86 k2 = (k0 + 2) % 3;
87 // Project the convex hull
88 pcl::PointCloud<PointT> xy_polygon;
89 xy_polygon.resize (polygon.size ());
90 for (std::size_t i = 0; i < polygon.size (); ++i)
91 {
92 Eigen::Vector4f pt (polygon[i].x, polygon[i].y, polygon[i].z, 0);
93 xy_polygon[i].x = pt[k1];
94 xy_polygon[i].y = pt[k2];
95 xy_polygon[i].z = 0;
96 }
97 PointT xy_point;
98 xy_point.z = 0;
99 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
100 xy_point.x = pt[k1];
101 xy_point.y = pt[k2];
102
103 return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
104}
105
106//////////////////////////////////////////////////////////////////////////
107template <typename PointT> bool
109{
110 bool in_poly = false;
111 double x1, x2, y1, y2;
112
113 const auto nr_poly_points = polygon.size ();
114 // start with the last point to make the check last point<->first point the first one
115 double xold = polygon[nr_poly_points - 1].x;
116 double yold = polygon[nr_poly_points - 1].y;
117 for (std::size_t i = 0; i < nr_poly_points; i++)
118 {
119 double xnew = polygon[i].x;
120 double ynew = polygon[i].y;
121 if (xnew > xold)
122 {
123 x1 = xold;
124 x2 = xnew;
125 y1 = yold;
126 y2 = ynew;
127 }
128 else
129 {
130 x1 = xnew;
131 x2 = xold;
132 y1 = ynew;
133 y2 = yold;
134 }
135
136 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
137 {
138 in_poly = !in_poly;
139 }
140 xold = xnew;
141 yold = ynew;
142 }
143
144 return (in_poly);
145}
146
147//////////////////////////////////////////////////////////////////////////
148template <typename PointT> void
150{
151 output.header = input_->header;
152
153 if (!initCompute ())
154 {
155 output.indices.clear ();
156 return;
157 }
158
159 if (static_cast<int> (planar_hull_->size ()) < min_pts_hull_)
160 {
161 PCL_ERROR("[pcl::%s::segment] Not enough points (%zu) in the hull!\n",
162 getClassName().c_str(),
163 static_cast<std::size_t>(planar_hull_->size()));
164 output.indices.clear ();
165 return;
166 }
167
168 // Compute the plane coefficients
169 Eigen::Vector4f model_coefficients;
170 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
171 Eigen::Vector4f xyz_centroid;
172
173 computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid);
174
175 // Compute the model coefficients
176 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
177 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
178 eigen33 (covariance_matrix, eigen_value, eigen_vector);
179
180 model_coefficients[0] = eigen_vector [0];
181 model_coefficients[1] = eigen_vector [1];
182 model_coefficients[2] = eigen_vector [2];
183 model_coefficients[3] = 0;
184
185 // Hessian form (D = nc . p_plane (centroid here) + p)
186 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
187
188 // Need to flip the plane normal towards the viewpoint
189 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
190 // See if we need to flip any plane normals
191 vp -= (*planar_hull_)[0].getVector4fMap ();
192 vp[3] = 0;
193 // Dot product between the (viewpoint - point) and the plane normal
194 float cos_theta = vp.dot (model_coefficients);
195 // Flip the plane normal
196 if (cos_theta < 0)
197 {
198 model_coefficients *= -1;
199 model_coefficients[3] = 0;
200 // Hessian form (D = nc . p_plane (centroid here) + p)
201 model_coefficients[3] = -1 * (model_coefficients.dot ((*planar_hull_)[0].getVector4fMap ()));
202 }
203
204 // Project all points
205 PointCloud projected_points;
206 SampleConsensusModelPlane<PointT> sacmodel (input_);
207 sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
208
209 // Create a X-Y projected representation for within bounds polygonal checking
210 int k0, k1, k2;
211 // Determine the best plane to project points onto
212 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
213 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
214 k1 = (k0 + 1) % 3;
215 k2 = (k0 + 2) % 3;
216 // Project the convex hull
218 polygon.resize (planar_hull_->size ());
219 for (std::size_t i = 0; i < planar_hull_->size (); ++i)
220 {
221 Eigen::Vector4f pt ((*planar_hull_)[i].x, (*planar_hull_)[i].y, (*planar_hull_)[i].z, 0);
222 polygon[i].x = pt[k1];
223 polygon[i].y = pt[k2];
224 polygon[i].z = 0;
225 }
226
227 PointT pt_xy;
228 pt_xy.z = 0;
229
230 std::vector<pcl::PointCloud<PointT>> polygons(polygons_.size());
231 if (polygons_.empty()) {
232 polygons.push_back(polygon);
233 }
234 else { // incase of concave hull, prepare separate polygons
235 for (size_t i = 0; i < polygons_.size(); i++) {
236 const auto& polygon_i = polygons_[i];
237 polygons[i].reserve(polygon_i.vertices.size());
238 for (const auto& pointIdx : polygon_i.vertices) {
239 polygons[i].points.push_back(polygon[pointIdx]);
240 }
241 }
242 }
243
244 output.indices.resize (indices_->size ());
245 int l = 0;
246 for (std::size_t i = 0; i < projected_points.size (); ++i)
247 {
248 // Check the distance to the user imposed limits from the table planar model
249 double distance = pointToPlaneDistanceSigned ((*input_)[(*indices_)[i]], model_coefficients);
250 if (distance < height_limit_min_ || distance > height_limit_max_)
251 continue;
252
253 // Check what points are inside the hull
254 Eigen::Vector4f pt (projected_points[i].x,
255 projected_points[i].y,
256 projected_points[i].z, 0);
257 pt_xy.x = pt[k1];
258 pt_xy.y = pt[k2];
259
260 bool in_poly = false;
261 for (const auto& poly : polygons) {
262 in_poly ^= pcl::isXYPointIn2DXYPolygon(pt_xy, poly);
263 }
264
265 if (!in_poly) {
266 continue;
267 }
268
269 output.indices[l++] = (*indices_)[i];
270 }
271 output.indices.resize (l);
272
273 deinitCompute ();
274}
275
276#define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
277#define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
278#define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
279
280#endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
281
Define methods for centroid estimation and covariance matrix calculus.
void segment(PointIndices &output)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::size_t size() const
SampleConsensusModelPlane defines a model for 3D plane segmentation.
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.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition centroid.hpp:509
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...
Definition eigen.hpp:295
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.
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.
::pcl::PCLHeader header
A point structure representing Euclidean xyz coordinates, and the RGB color.