Point Cloud Library (PCL)  1.11.1-dev
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 //////////////////////////////////////////////////////////////////////////
47 template <typename PointT> bool
48 pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
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 //////////////////////////////////////////////////////////////////////////
107 template <typename PointT> bool
108 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
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 //////////////////////////////////////////////////////////////////////////
148 template <typename PointT> void
149 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
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
217  pcl::PointCloud<PointT> polygon;
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  output.indices.resize (indices_->size ());
231  int l = 0;
232  for (std::size_t i = 0; i < projected_points.size (); ++i)
233  {
234  // Check the distance to the user imposed limits from the table planar model
235  double distance = pointToPlaneDistanceSigned ((*input_)[(*indices_)[i]], model_coefficients);
236  if (distance < height_limit_min_ || distance > height_limit_max_)
237  continue;
238 
239  // Check what points are inside the hull
240  Eigen::Vector4f pt (projected_points[i].x,
241  projected_points[i].y,
242  projected_points[i].z, 0);
243  pt_xy.x = pt[k1];
244  pt_xy.y = pt[k2];
245 
246  if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
247  continue;
248 
249  output.indices[l++] = (*indices_)[i];
250  }
251  output.indices.resize (l);
252 
253  deinitCompute ();
254 }
255 
256 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
257 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
258 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
259 
260 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
261 
pcl::ExtractPolygonalPrismData::segment
void segment(PointIndices &output)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
Definition: extract_polygonal_prism_data.hpp:149
pcl::computeMeanAndCovarianceMatrix
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:485
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::SampleConsensusModelPlane
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Definition: sac_model_plane.h:142
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::isXYPointIn2DXYPolygon
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.
Definition: extract_polygonal_prism_data.hpp:108
pcl::PointIndices::header
::pcl::PCLHeader header
Definition: PointIndices.h:19
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::eigen33
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:296
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::SampleConsensusModelPlane::projectPoints
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.
Definition: sac_model_plane.hpp:386
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::isPointIn2DPolygon
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.
Definition: extract_polygonal_prism_data.hpp:48
centroid.h
pcl::pointToPlaneDistanceSigned
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.
Definition: sac_model_plane.h:89