Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 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 00045 ////////////////////////////////////////////////////////////////////////// 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 computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid); 00055 00056 // Compute the model coefficients 00057 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00058 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00059 eigen33 (covariance_matrix, eigen_value, eigen_vector); 00060 00061 model_coefficients[0] = eigen_vector [0]; 00062 model_coefficients[1] = eigen_vector [1]; 00063 model_coefficients[2] = eigen_vector [2]; 00064 model_coefficients[3] = 0; 00065 00066 // Hessian form (D = nc . p_plane (centroid here) + p) 00067 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); 00068 00069 float distance_to_plane = model_coefficients[0] * point.x + 00070 model_coefficients[1] * point.y + 00071 model_coefficients[2] * point.z + 00072 model_coefficients[3]; 00073 PointT ppoint; 00074 // Calculate the projection of the point on the plane 00075 ppoint.x = point.x - distance_to_plane * model_coefficients[0]; 00076 ppoint.y = point.y - distance_to_plane * model_coefficients[1]; 00077 ppoint.z = point.z - distance_to_plane * model_coefficients[2]; 00078 00079 // Create a X-Y projected representation for within bounds polygonal checking 00080 int k0, k1, k2; 00081 // Determine the best plane to project points onto 00082 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; 00083 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; 00084 k1 = (k0 + 1) % 3; 00085 k2 = (k0 + 2) % 3; 00086 // Project the convex hull 00087 pcl::PointCloud<PointT> xy_polygon; 00088 xy_polygon.points.resize (polygon.points.size ()); 00089 for (size_t i = 0; i < polygon.points.size (); ++i) 00090 { 00091 Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0); 00092 xy_polygon.points[i].x = pt[k1]; 00093 xy_polygon.points[i].y = pt[k2]; 00094 xy_polygon.points[i].z = 0; 00095 } 00096 PointT xy_point; 00097 xy_point.z = 0; 00098 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0); 00099 xy_point.x = pt[k1]; 00100 xy_point.y = pt[k2]; 00101 00102 return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon)); 00103 } 00104 00105 ////////////////////////////////////////////////////////////////////////// 00106 template <typename PointT> bool 00107 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon) 00108 { 00109 bool in_poly = false; 00110 double x1, x2, y1, y2; 00111 00112 int nr_poly_points = static_cast<int> (polygon.points.size ()); 00113 double xold = polygon.points[nr_poly_points - 1].x; 00114 double yold = polygon.points[nr_poly_points - 1].y; 00115 for (int i = 0; i < nr_poly_points; i++) 00116 { 00117 double xnew = polygon.points[i].x; 00118 double ynew = polygon.points[i].y; 00119 if (xnew > xold) 00120 { 00121 x1 = xold; 00122 x2 = xnew; 00123 y1 = yold; 00124 y2 = ynew; 00125 } 00126 else 00127 { 00128 x1 = xnew; 00129 x2 = xold; 00130 y1 = ynew; 00131 y2 = yold; 00132 } 00133 00134 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) ) 00135 { 00136 in_poly = !in_poly; 00137 } 00138 xold = xnew; 00139 yold = ynew; 00140 } 00141 00142 // And a last check for the polygon line formed by the last and the first points 00143 double xnew = polygon.points[0].x; 00144 double ynew = polygon.points[0].y; 00145 if (xnew > xold) 00146 { 00147 x1 = xold; 00148 x2 = xnew; 00149 y1 = yold; 00150 y2 = ynew; 00151 } 00152 else 00153 { 00154 x1 = xnew; 00155 x2 = xold; 00156 y1 = ynew; 00157 y2 = yold; 00158 } 00159 00160 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) ) 00161 { 00162 in_poly = !in_poly; 00163 } 00164 00165 return (in_poly); 00166 } 00167 00168 ////////////////////////////////////////////////////////////////////////// 00169 template <typename PointT> void 00170 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output) 00171 { 00172 output.header = input_->header; 00173 00174 if (!initCompute ()) 00175 { 00176 output.indices.clear (); 00177 return; 00178 } 00179 00180 if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_) 00181 { 00182 PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); 00183 output.indices.clear (); 00184 return; 00185 } 00186 00187 // Compute the plane coefficients 00188 Eigen::Vector4f model_coefficients; 00189 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00190 Eigen::Vector4f xyz_centroid; 00191 00192 computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid); 00193 00194 // Compute the model coefficients 00195 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00196 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00197 eigen33 (covariance_matrix, eigen_value, eigen_vector); 00198 00199 model_coefficients[0] = eigen_vector [0]; 00200 model_coefficients[1] = eigen_vector [1]; 00201 model_coefficients[2] = eigen_vector [2]; 00202 model_coefficients[3] = 0; 00203 00204 // Hessian form (D = nc . p_plane (centroid here) + p) 00205 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); 00206 00207 // Need to flip the plane normal towards the viewpoint 00208 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0); 00209 // See if we need to flip any plane normals 00210 vp -= planar_hull_->points[0].getVector4fMap (); 00211 vp[3] = 0; 00212 // Dot product between the (viewpoint - point) and the plane normal 00213 float cos_theta = vp.dot (model_coefficients); 00214 // Flip the plane normal 00215 if (cos_theta < 0) 00216 { 00217 model_coefficients *= -1; 00218 model_coefficients[3] = 0; 00219 // Hessian form (D = nc . p_plane (centroid here) + p) 00220 model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ())); 00221 } 00222 00223 // Project all points 00224 PointCloud projected_points; 00225 SampleConsensusModelPlane<PointT> sacmodel (input_); 00226 sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false); 00227 00228 // Create a X-Y projected representation for within bounds polygonal checking 00229 int k0, k1, k2; 00230 // Determine the best plane to project points onto 00231 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; 00232 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; 00233 k1 = (k0 + 1) % 3; 00234 k2 = (k0 + 2) % 3; 00235 // Project the convex hull 00236 pcl::PointCloud<PointT> polygon; 00237 polygon.points.resize (planar_hull_->points.size ()); 00238 for (size_t i = 0; i < planar_hull_->points.size (); ++i) 00239 { 00240 Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0); 00241 polygon.points[i].x = pt[k1]; 00242 polygon.points[i].y = pt[k2]; 00243 polygon.points[i].z = 0; 00244 } 00245 00246 PointT pt_xy; 00247 pt_xy.z = 0; 00248 00249 output.indices.resize (indices_->size ()); 00250 int l = 0; 00251 for (size_t i = 0; i < projected_points.points.size (); ++i) 00252 { 00253 // Check the distance to the user imposed limits from the table planar model 00254 double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients); 00255 if (distance < height_limit_min_ || distance > height_limit_max_) 00256 continue; 00257 00258 // Check what points are inside the hull 00259 Eigen::Vector4f pt (projected_points.points[i].x, 00260 projected_points.points[i].y, 00261 projected_points.points[i].z, 0); 00262 pt_xy.x = pt[k1]; 00263 pt_xy.y = pt[k2]; 00264 00265 if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon)) 00266 continue; 00267 00268 output.indices[l++] = (*indices_)[i]; 00269 } 00270 output.indices.resize (l); 00271 00272 deinitCompute (); 00273 } 00274 00275 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>; 00276 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &); 00277 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &); 00278 00279 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ 00280