|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.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 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 5026 2012-03-12 02:51:44Z rusu $ 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 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 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 return (in_poly); 00142 } 00143 00145 template <typename PointT> void 00146 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output) 00147 { 00148 output.header = input_->header; 00149 00150 if (!initCompute ()) 00151 { 00152 output.indices.clear (); 00153 return; 00154 } 00155 00156 if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_) 00157 { 00158 PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); 00159 output.indices.clear (); 00160 return; 00161 } 00162 00163 // Compute the plane coefficients 00164 Eigen::Vector4f model_coefficients; 00165 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00166 Eigen::Vector4f xyz_centroid; 00167 00168 computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid); 00169 00170 // Compute the model coefficients 00171 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00172 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00173 eigen33 (covariance_matrix, eigen_value, eigen_vector); 00174 00175 model_coefficients[0] = eigen_vector [0]; 00176 model_coefficients[1] = eigen_vector [1]; 00177 model_coefficients[2] = eigen_vector [2]; 00178 model_coefficients[3] = 0; 00179 00180 // Hessian form (D = nc . p_plane (centroid here) + p) 00181 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); 00182 00183 // Need to flip the plane normal towards the viewpoint 00184 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0); 00185 // See if we need to flip any plane normals 00186 vp -= planar_hull_->points[0].getVector4fMap (); 00187 vp[3] = 0; 00188 // Dot product between the (viewpoint - point) and the plane normal 00189 float cos_theta = vp.dot (model_coefficients); 00190 // Flip the plane normal 00191 if (cos_theta < 0) 00192 { 00193 model_coefficients *= -1; 00194 model_coefficients[3] = 0; 00195 // Hessian form (D = nc . p_plane (centroid here) + p) 00196 model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ())); 00197 } 00198 00199 // Project all points 00200 PointCloud projected_points; 00201 SampleConsensusModelPlane<PointT> sacmodel (input_); 00202 sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false); 00203 00204 // Create a X-Y projected representation for within bounds polygonal checking 00205 int k0, k1, k2; 00206 // Determine the best plane to project points onto 00207 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; 00208 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; 00209 k1 = (k0 + 1) % 3; 00210 k2 = (k0 + 2) % 3; 00211 // Project the convex hull 00212 pcl::PointCloud<PointT> polygon; 00213 polygon.points.resize (planar_hull_->points.size ()); 00214 for (size_t i = 0; i < planar_hull_->points.size (); ++i) 00215 { 00216 Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0); 00217 polygon.points[i].x = pt[k1]; 00218 polygon.points[i].y = pt[k2]; 00219 polygon.points[i].z = 0; 00220 } 00221 00222 PointT pt_xy; 00223 pt_xy.z = 0; 00224 00225 output.indices.resize (indices_->size ()); 00226 int l = 0; 00227 for (size_t i = 0; i < projected_points.points.size (); ++i) 00228 { 00229 // Check the distance to the user imposed limits from the table planar model 00230 double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients); 00231 if (distance < height_limit_min_ || distance > height_limit_max_) 00232 continue; 00233 00234 // Check what points are inside the hull 00235 Eigen::Vector4f pt (projected_points.points[i].x, 00236 projected_points.points[i].y, 00237 projected_points.points[i].z, 0); 00238 pt_xy.x = pt[k1]; 00239 pt_xy.y = pt[k2]; 00240 00241 if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon)) 00242 continue; 00243 00244 output.indices[l++] = (*indices_)[i]; 00245 } 00246 output.indices.resize (l); 00247 00248 deinitCompute (); 00249 } 00250 00251 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>; 00252 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &); 00253 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &); 00254 00255 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ 00256
1.7.6.1
