Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: extract_polygonal_prism_data.hpp 1370 2011-06-19 01:06:01Z jspricke $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ 00039 #define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ 00040 00041 #include "pcl/segmentation/extract_polygonal_prism_data.h" 00042 #include "pcl/common/centroid.h" 00043 #include "pcl/common/eigen.h" 00044 00046 template <typename PointT> bool 00047 pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon) 00048 { 00049 // Compute the plane coefficients 00050 Eigen::Vector4f model_coefficients; 00051 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00052 Eigen::Vector4f xyz_centroid; 00053 00054 // Estimate the XYZ centroid 00055 compute3DCentroid (polygon, xyz_centroid); 00056 00057 // Compute the 3x3 covariance matrix 00058 computeCovarianceMatrix (polygon, xyz_centroid, covariance_matrix); 00059 00060 // Compute the model coefficients 00061 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00062 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00063 eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00064 00065 model_coefficients[0] = eigen_vectors (0, 0); 00066 model_coefficients[1] = eigen_vectors (1, 0); 00067 model_coefficients[2] = eigen_vectors (2, 0); 00068 model_coefficients[3] = 0; 00069 00070 // Hessian form (D = nc . p_plane (centroid here) + p) 00071 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); 00072 00073 float distance_to_plane = model_coefficients[0] * point.x + 00074 model_coefficients[1] * point.y + 00075 model_coefficients[2] * point.z + 00076 model_coefficients[3]; 00077 PointT ppoint; 00078 // Calculate the projection of the point on the plane 00079 ppoint.x = point.x - distance_to_plane * model_coefficients[0]; 00080 ppoint.y = point.y - distance_to_plane * model_coefficients[1]; 00081 ppoint.z = point.z - distance_to_plane * model_coefficients[2]; 00082 00083 // Create a X-Y projected representation for within bounds polygonal checking 00084 int k0, k1, k2; 00085 // Determine the best plane to project points onto 00086 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; 00087 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; 00088 k1 = (k0 + 1) % 3; 00089 k2 = (k0 + 2) % 3; 00090 // Project the convex hull 00091 pcl::PointCloud<PointT> xy_polygon; 00092 xy_polygon.points.resize (polygon.points.size ()); 00093 for (size_t i = 0; i < polygon.points.size (); ++i) 00094 { 00095 Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0); 00096 xy_polygon.points[i].x = pt[k1]; 00097 xy_polygon.points[i].y = pt[k2]; 00098 xy_polygon.points[i].z = 0; 00099 } 00100 PointT xy_point; 00101 xy_point.z = 0; 00102 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0); 00103 xy_point.x = pt[k1]; 00104 xy_point.y = pt[k2]; 00105 00106 return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon)); 00107 } 00108 00110 template <typename PointT> bool 00111 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon) 00112 { 00113 bool in_poly = false; 00114 double x1, x2, y1, y2; 00115 00116 int nr_poly_points = polygon.points.size (); 00117 double xold = polygon.points[nr_poly_points - 1].x; 00118 double yold = polygon.points[nr_poly_points - 1].y; 00119 for (int i = 0; i < nr_poly_points; i++) 00120 { 00121 double xnew = polygon.points[i].x; 00122 double ynew = polygon.points[i].y; 00123 if (xnew > xold) 00124 { 00125 x1 = xold; 00126 x2 = xnew; 00127 y1 = yold; 00128 y2 = ynew; 00129 } 00130 else 00131 { 00132 x1 = xnew; 00133 x2 = xold; 00134 y1 = ynew; 00135 y2 = yold; 00136 } 00137 00138 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) ) 00139 { 00140 in_poly = !in_poly; 00141 } 00142 xold = xnew; 00143 yold = ynew; 00144 } 00145 return (in_poly); 00146 } 00147 00149 template <typename PointT> void 00150 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output) 00151 { 00152 output.header = input_->header; 00153 00154 if (!initCompute ()) 00155 { 00156 output.indices.clear (); 00157 return; 00158 } 00159 00160 if ((int)planar_hull_->points.size () < min_pts_hull_) 00161 { 00162 PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), (unsigned long)planar_hull_->points.size ()); 00163 output.indices.clear (); 00164 return; 00165 } 00166 00167 // Compute the plane coefficients 00168 Eigen::Vector4f model_coefficients; 00169 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00170 Eigen::Vector4f xyz_centroid; 00171 00172 // Estimate the XYZ centroid 00173 compute3DCentroid (*planar_hull_, xyz_centroid); 00174 00175 // Compute the 3x3 covariance matrix 00176 computeCovarianceMatrix (*planar_hull_, xyz_centroid, covariance_matrix); 00177 00178 // Compute the model coefficients 00179 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00180 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00181 eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00182 00183 model_coefficients[0] = eigen_vectors (0, 0); 00184 model_coefficients[1] = eigen_vectors (1, 0); 00185 model_coefficients[2] = eigen_vectors (2, 0); 00186 model_coefficients[3] = 0; 00187 00188 // Hessian form (D = nc . p_plane (centroid here) + p) 00189 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); 00190 00191 // Need to flip the plane normal towards the viewpoint 00192 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0); 00193 // See if we need to flip any plane normals 00194 vp -= planar_hull_->points[0].getVector4fMap (); 00195 vp[3] = 0; 00196 // Dot product between the (viewpoint - point) and the plane normal 00197 float cos_theta = vp.dot (model_coefficients); 00198 // Flip the plane normal 00199 if (cos_theta < 0) 00200 { 00201 model_coefficients *= -1; 00202 model_coefficients[3] = 0; 00203 // Hessian form (D = nc . p_plane (centroid here) + p) 00204 model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ())); 00205 } 00206 00207 // Project all points 00208 PointCloud projected_points; 00209 SampleConsensusModelPlane<PointT> sacmodel (input_); 00210 sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false); 00211 00212 // Create a X-Y projected representation for within bounds polygonal checking 00213 int k0, k1, k2; 00214 // Determine the best plane to project points onto 00215 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; 00216 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; 00217 k1 = (k0 + 1) % 3; 00218 k2 = (k0 + 2) % 3; 00219 // Project the convex hull 00220 pcl::PointCloud<PointT> polygon; 00221 polygon.points.resize (planar_hull_->points.size ()); 00222 for (size_t i = 0; i < planar_hull_->points.size (); ++i) 00223 { 00224 Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0); 00225 polygon.points[i].x = pt[k1]; 00226 polygon.points[i].y = pt[k2]; 00227 polygon.points[i].z = 0; 00228 } 00229 00230 PointT pt_xy; 00231 pt_xy.z = 0; 00232 00233 output.indices.resize (indices_->size ()); 00234 int l = 0; 00235 for (size_t i = 0; i < projected_points.points.size (); ++i) 00236 { 00237 // Check the distance to the user imposed limits from the table planar model 00238 double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients); 00239 if (distance < height_limit_min_ || distance > height_limit_max_) 00240 continue; 00241 00242 // Check what points are inside the hull 00243 Eigen::Vector4f pt (projected_points.points[i].x, 00244 projected_points.points[i].y, 00245 projected_points.points[i].z, 0); 00246 pt_xy.x = pt[k1]; 00247 pt_xy.y = pt[k2]; 00248 00249 if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon)) 00250 continue; 00251 00252 output.indices[l++] = (*indices_)[i]; 00253 } 00254 output.indices.resize (l); 00255 00256 deinitCompute (); 00257 } 00258 00259 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>; 00260 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &); 00261 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &); 00262 00263 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ 00264