Point Cloud Library (PCL)  1.7.0
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/common/centroid.h>
43 #include <pcl/common/eigen.h>
44 
45 //////////////////////////////////////////////////////////////////////////
46 template <typename PointT> bool
48 {
49  // Compute the plane coefficients
50  Eigen::Vector4f model_coefficients;
51  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
52  Eigen::Vector4f xyz_centroid;
53 
54  computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid);
55 
56  // Compute the model coefficients
57  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
58  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
59  eigen33 (covariance_matrix, eigen_value, eigen_vector);
60 
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;
65 
66  // Hessian form (D = nc . p_plane (centroid here) + p)
67  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
68 
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];
73  PointT ppoint;
74  // Calculate the projection of the point on the plane
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];
78 
79  // Create a X-Y projected representation for within bounds polygonal checking
80  int k0, k1, k2;
81  // Determine the best plane to project points onto
82  k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
83  k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
84  k1 = (k0 + 1) % 3;
85  k2 = (k0 + 2) % 3;
86  // Project the convex hull
87  pcl::PointCloud<PointT> xy_polygon;
88  xy_polygon.points.resize (polygon.points.size ());
89  for (size_t i = 0; i < polygon.points.size (); ++i)
90  {
91  Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
92  xy_polygon.points[i].x = pt[k1];
93  xy_polygon.points[i].y = pt[k2];
94  xy_polygon.points[i].z = 0;
95  }
96  PointT xy_point;
97  xy_point.z = 0;
98  Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
99  xy_point.x = pt[k1];
100  xy_point.y = pt[k2];
101 
102  return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
103 }
104 
105 //////////////////////////////////////////////////////////////////////////
106 template <typename PointT> bool
108 {
109  bool in_poly = false;
110  double x1, x2, y1, y2;
111 
112  int nr_poly_points = static_cast<int> (polygon.points.size ());
113  double xold = polygon.points[nr_poly_points - 1].x;
114  double yold = polygon.points[nr_poly_points - 1].y;
115  for (int i = 0; i < nr_poly_points; i++)
116  {
117  double xnew = polygon.points[i].x;
118  double ynew = polygon.points[i].y;
119  if (xnew > xold)
120  {
121  x1 = xold;
122  x2 = xnew;
123  y1 = yold;
124  y2 = ynew;
125  }
126  else
127  {
128  x1 = xnew;
129  x2 = xold;
130  y1 = ynew;
131  y2 = yold;
132  }
133 
134  if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
135  {
136  in_poly = !in_poly;
137  }
138  xold = xnew;
139  yold = ynew;
140  }
141 
142  // And a last check for the polygon line formed by the last and the first points
143  double xnew = polygon.points[0].x;
144  double ynew = polygon.points[0].y;
145  if (xnew > xold)
146  {
147  x1 = xold;
148  x2 = xnew;
149  y1 = yold;
150  y2 = ynew;
151  }
152  else
153  {
154  x1 = xnew;
155  x2 = xold;
156  y1 = ynew;
157  y2 = yold;
158  }
159 
160  if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
161  {
162  in_poly = !in_poly;
163  }
164 
165  return (in_poly);
166 }
167 
168 //////////////////////////////////////////////////////////////////////////
169 template <typename PointT> void
171 {
172  output.header = input_->header;
173 
174  if (!initCompute ())
175  {
176  output.indices.clear ();
177  return;
178  }
179 
180  if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_)
181  {
182  PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ());
183  output.indices.clear ();
184  return;
185  }
186 
187  // Compute the plane coefficients
188  Eigen::Vector4f model_coefficients;
189  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
190  Eigen::Vector4f xyz_centroid;
191 
192  computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid);
193 
194  // Compute the model coefficients
195  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
196  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
197  eigen33 (covariance_matrix, eigen_value, eigen_vector);
198 
199  model_coefficients[0] = eigen_vector [0];
200  model_coefficients[1] = eigen_vector [1];
201  model_coefficients[2] = eigen_vector [2];
202  model_coefficients[3] = 0;
203 
204  // Hessian form (D = nc . p_plane (centroid here) + p)
205  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
206 
207  // Need to flip the plane normal towards the viewpoint
208  Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
209  // See if we need to flip any plane normals
210  vp -= planar_hull_->points[0].getVector4fMap ();
211  vp[3] = 0;
212  // Dot product between the (viewpoint - point) and the plane normal
213  float cos_theta = vp.dot (model_coefficients);
214  // Flip the plane normal
215  if (cos_theta < 0)
216  {
217  model_coefficients *= -1;
218  model_coefficients[3] = 0;
219  // Hessian form (D = nc . p_plane (centroid here) + p)
220  model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
221  }
222 
223  // Project all points
224  PointCloud projected_points;
225  SampleConsensusModelPlane<PointT> sacmodel (input_);
226  sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
227 
228  // Create a X-Y projected representation for within bounds polygonal checking
229  int k0, k1, k2;
230  // Determine the best plane to project points onto
231  k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
232  k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
233  k1 = (k0 + 1) % 3;
234  k2 = (k0 + 2) % 3;
235  // Project the convex hull
236  pcl::PointCloud<PointT> polygon;
237  polygon.points.resize (planar_hull_->points.size ());
238  for (size_t i = 0; i < planar_hull_->points.size (); ++i)
239  {
240  Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
241  polygon.points[i].x = pt[k1];
242  polygon.points[i].y = pt[k2];
243  polygon.points[i].z = 0;
244  }
245 
246  PointT pt_xy;
247  pt_xy.z = 0;
248 
249  output.indices.resize (indices_->size ());
250  int l = 0;
251  for (size_t i = 0; i < projected_points.points.size (); ++i)
252  {
253  // Check the distance to the user imposed limits from the table planar model
254  double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
255  if (distance < height_limit_min_ || distance > height_limit_max_)
256  continue;
257 
258  // Check what points are inside the hull
259  Eigen::Vector4f pt (projected_points.points[i].x,
260  projected_points.points[i].y,
261  projected_points.points[i].z, 0);
262  pt_xy.x = pt[k1];
263  pt_xy.y = pt[k2];
264 
265  if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
266  continue;
267 
268  output.indices[l++] = (*indices_)[i];
269  }
270  output.indices.resize (l);
271 
272  deinitCompute ();
273 }
274 
275 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
276 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
277 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
278 
279 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
280