00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00044 template <typename PointT> bool
00045 pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
00046 {
00047
00048 Eigen::Vector4f model_coefficients;
00049 float curvature;
00050 pcl::computePointNormal<PointT> (polygon, model_coefficients, curvature);
00051
00052 float distance_to_plane = model_coefficients[0] * point.x +
00053 model_coefficients[1] * point.y +
00054 model_coefficients[2] * point.z +
00055 model_coefficients[3];
00056 PointT ppoint;
00057
00058 ppoint.x = point.x - distance_to_plane * model_coefficients[0];
00059 ppoint.y = point.y - distance_to_plane * model_coefficients[1];
00060 ppoint.z = point.z - distance_to_plane * model_coefficients[2];
00061
00062
00063 int k0, k1, k2;
00064
00065 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
00066 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00067 k1 = (k0 + 1) % 3;
00068 k2 = (k0 + 2) % 3;
00069
00070 pcl::PointCloud<PointT> xy_polygon;
00071 xy_polygon.points.resize (polygon.points.size ());
00072 for (size_t i = 0; i < polygon.points.size (); ++i)
00073 {
00074 Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
00075 xy_polygon.points[i].x = pt[k1];
00076 xy_polygon.points[i].y = pt[k2];
00077 xy_polygon.points[i].z = 0;
00078 }
00079 PointT xy_point;
00080 xy_point.z = 0;
00081 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
00082 xy_point.x = pt[k1];
00083 xy_point.y = pt[k2];
00084
00085 return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
00086 }
00087
00089 template <typename PointT> bool
00090 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
00091 {
00092 bool in_poly = false;
00093 double x1, x2, y1, y2;
00094
00095 int nr_poly_points = polygon.points.size ();
00096 double xold = polygon.points[nr_poly_points - 1].x;
00097 double yold = polygon.points[nr_poly_points - 1].y;
00098 for (int i = 0; i < nr_poly_points; i++)
00099 {
00100 double xnew = polygon.points[i].x;
00101 double ynew = polygon.points[i].y;
00102 if (xnew > xold)
00103 {
00104 x1 = xold;
00105 x2 = xnew;
00106 y1 = yold;
00107 y2 = ynew;
00108 }
00109 else
00110 {
00111 x1 = xnew;
00112 x2 = xold;
00113 y1 = ynew;
00114 y2 = yold;
00115 }
00116
00117 if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
00118 {
00119 in_poly = !in_poly;
00120 }
00121 xold = xnew;
00122 yold = ynew;
00123 }
00124 return (in_poly);
00125 }
00126
00128 template <typename PointT> void
00129 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
00130 {
00131 output.header = input_->header;
00132
00133 if (!initCompute ())
00134 {
00135 output.indices.clear ();
00136 return;
00137 }
00138
00139 if ((int)planar_hull_->points.size () < min_pts_hull_)
00140 {
00141 ROS_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!", getClassName ().c_str (), planar_hull_->points.size ());
00142 output.indices.clear ();
00143 return;
00144 }
00145
00146
00147 Eigen::Vector4f model_coefficients;
00148 float curvature;
00149 pcl::computePointNormal<PointT> (*planar_hull_, model_coefficients, curvature);
00150
00151
00152 pcl::flipNormalTowardsViewpoint (planar_hull_->points[0], vpx_, vpy_, vpz_, model_coefficients);
00153
00154
00155 PointCloud projected_points;
00156 SampleConsensusModelPlane<PointT> sacmodel = SampleConsensusModelPlane<PointT> (input_);
00157 sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
00158
00159
00160 int k0, k1, k2;
00161
00162 k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
00163 k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00164 k1 = (k0 + 1) % 3;
00165 k2 = (k0 + 2) % 3;
00166
00167 pcl::PointCloud<PointT> polygon;
00168 polygon.points.resize (planar_hull_->points.size ());
00169 for (size_t i = 0; i < planar_hull_->points.size (); ++i)
00170 {
00171 Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
00172 polygon.points[i].x = pt[k1];
00173 polygon.points[i].y = pt[k2];
00174 polygon.points[i].z = 0;
00175 }
00176
00177 PointT pt_xy;
00178 pt_xy.z = 0;
00179
00180 output.indices.resize (indices_->size ());
00181 int l = 0;
00182 for (size_t i = 0; i < projected_points.points.size (); ++i)
00183 {
00184
00185 double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
00186 if (distance < height_limit_min_ || distance > height_limit_max_)
00187 continue;
00188
00189
00190 Eigen::Vector4f pt (projected_points.points[i].x,
00191 projected_points.points[i].y,
00192 projected_points.points[i].z, 0);
00193 pt_xy.x = pt[k1];
00194 pt_xy.y = pt[k2];
00195
00196 if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
00197 continue;
00198
00199 output.indices[l++] = (*indices_)[i];
00200 }
00201 output.indices.resize (l);
00202
00203 deinitCompute ();
00204 }
00205
00206 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class pcl::ExtractPolygonalPrismData<T>;
00207 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
00208 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
00209
00210 #endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00211