00001 #include <plane_extractor.h>
00002
00003 geometry_msgs::Point32 calcCentroid(const pcl::PointCloud<Point>& cloud)
00004 {
00005 geometry_msgs::Point32 centroid;
00006 centroid.x = 0.0;
00007 centroid.y = 0.0;
00008 centroid.z = 0.0;
00009
00010 for(size_t i = 0; i < cloud.points.size(); i++){
00011 centroid.x += cloud.points[i].x;
00012 centroid.y += cloud.points[i].y;
00013 centroid.z += cloud.points[i].z;
00014 }
00015
00016 centroid.x /= cloud.points.size();
00017 centroid.y /= cloud.points.size();
00018 centroid.z /= cloud.points.size();
00019
00020 return centroid;
00021 }
00022
00023
00024 book_stacking_msgs::ObjectInfos getObjectsOverPlane(const book_stacking_msgs::PlaneInfo& plane, const pcl::PointCloud<Point>& cloud,double prism_min_height, double prism_max_height)
00025 {
00026 book_stacking_msgs::ObjectInfos objects_msg;
00027
00028
00029
00030 pcl::PointIndices prism_indices;
00031 pcl::ExtractPolygonalPrismData<Point> prism_extract;
00032 prism_extract.setHeightLimits(prism_min_height,prism_max_height);
00033 prism_extract.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
00034 pcl::PointCloud<Point> plane_hull;
00035 pcl::fromROSMsg(plane.hull,plane_hull);
00036 prism_extract.setInputPlanarHull(boost::make_shared<pcl::PointCloud<Point> >(plane_hull));
00037 prism_extract.segment(prism_indices);
00038
00039 pcl::PointCloud<Point> prism_cloud;
00040 pcl::ExtractIndices<Point> extract_prism_indices;
00041 extract_prism_indices.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
00042 extract_prism_indices.setIndices(boost::make_shared<const pcl::PointIndices>(prism_indices));
00043 extract_prism_indices.filter(prism_cloud);
00044
00045 if(prism_cloud.points.size() < 20){
00046 ROS_WARN("getObjectsOverPlane: Not enough points in prism.");
00047 return objects_msg;
00048 }
00049
00050
00051 pcl::EuclideanClusterExtraction<Point> clusterer;
00052 clusterer.setClusterTolerance(0.05);
00053 clusterer.setMinClusterSize(100);
00054 clusterer.setMaxClusterSize(8000);
00055
00056
00057
00058
00059
00060 std::vector<pcl::PointIndices> clusters;
00061 clusterer.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(prism_cloud));
00062 clusterer.extract(clusters);
00063
00064 for(size_t i = 0; i < clusters.size(); i++){
00065 book_stacking_msgs::ObjectInfo object_msg;
00066 pcl::PointCloud<Point> object_cloud;
00067 pcl::copyPointCloud(prism_cloud,clusters[i],object_cloud);
00068
00069 Eigen::Vector4f min_point;
00070 Eigen::Vector4f max_point;
00071 pcl::getMinMax3D(prism_cloud,clusters[i],min_point,max_point);
00072
00073
00074
00075 sensor_msgs::PointCloud2 object_cloud_msg;
00076 pcl::toROSMsg(object_cloud,object_cloud_msg);
00077 object_cloud_msg.header = cloud.header;
00078 object_msg.header = cloud.header;
00079 object_msg.cloud = object_cloud_msg;
00080 geometry_msgs::Point32 min_pt;
00081 min_pt.x = min_point.x();
00082 min_pt.y = min_point.y();
00083 min_pt.z = min_point.z();
00084 geometry_msgs::Point32 max_pt;
00085 max_pt.x = max_point.x();
00086 max_pt.y = max_point.y();
00087 max_pt.z = max_point.z();
00088 object_msg.bbox_min = min_pt;
00089 object_msg.bbox_max = max_pt;
00090 object_msg.centroid = calcCentroid(object_cloud);
00091 object_msg.grasp_width = max_point.y() - min_point.y();
00092 objects_msg.objects.push_back(object_msg);
00093
00094
00095
00096 }
00097
00098 return objects_msg;
00099 }
00100
00101
00102 void drawObjectPrisms(const book_stacking_msgs::ObjectInfos& objects, const ros::Publisher& object_pub, const book_stacking_msgs::PlaneInfo& plane, float r, float g, float b)
00103 {
00104 visualization_msgs::Marker marker;
00105 marker.header = plane.header;
00106 marker.ns = "object_prisms";
00107 marker.id = 1;
00108 marker.type = visualization_msgs::Marker::LINE_LIST;
00109 marker.action = visualization_msgs::Marker::ADD;
00110 marker.color.r = r;
00111 marker.color.g = g;
00112 marker.color.b = b;
00113 marker.color.a = 0.5;
00114 marker.scale.x = 0.02;
00115 marker.lifetime = ros::Duration(60.0);
00116
00117 for(size_t i = 0; i < objects.objects.size(); i++){
00118 pcl::PointCloud<Point> object_cloud;
00119 pcl::PointCloud<Point> object_cloud_projected;
00120 pcl::PointCloud<Point> object_hull;
00121 pcl::fromROSMsg(objects.objects[i].cloud,object_cloud);
00122
00123 pcl::ProjectInliers<Point> proj;
00124 proj.setModelType(pcl::SACMODEL_PLANE);
00125 proj.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(object_cloud));
00126 proj.setModelCoefficients(boost::make_shared<pcl::ModelCoefficients>(plane.model));
00127
00128 proj.filter(object_cloud_projected);
00129
00130 pcl::ConvexHull<Point> hull;
00131 hull.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(object_cloud_projected));
00132 hull.reconstruct(object_hull);
00133
00134 Eigen::Vector4f min_point;
00135 Eigen::Vector4f max_point;
00136 pcl::getMinMax3D(object_cloud,min_point,max_point);
00137
00138 for(size_t j = 0; j < object_hull.points.size()-1; j++){
00139
00140
00141 geometry_msgs::Point pt1;
00142 pt1.x = object_hull.points[j].x;
00143 pt1.y = object_hull.points[j].y;
00144 pt1.z = object_hull.points[j].z;
00145 geometry_msgs::Point pt2;
00146 pt2.x = object_hull.points[j+1].x;
00147 pt2.y = object_hull.points[j+1].y;
00148 pt2.z = object_hull.points[j+1].z;
00149
00150 marker.points.push_back(pt1);
00151 marker.points.push_back(pt2);
00152
00153
00154 geometry_msgs::Point pt3;
00155 pt3.x = object_hull.points[j].x;
00156 pt3.y = object_hull.points[j].y;
00157 pt3.z = max_point.z();
00158
00159 geometry_msgs::Point pt4;
00160 pt4.x = object_hull.points[j+1].x;
00161 pt4.y = object_hull.points[j+1].y;
00162 pt4.z = max_point.z();
00163
00164 marker.points.push_back(pt3);
00165 marker.points.push_back(pt4);
00166
00167
00168 marker.points.push_back(pt1);
00169 marker.points.push_back(pt3);
00170
00171 }
00172
00173
00174 }
00175 object_pub.publish(marker);
00176 }
00177
00178 void drawPlaneMarkers(const std::vector<book_stacking_msgs::PlaneInfo>& planes, const ros::Publisher& plane_pub, float r, float g, float b)
00179 {
00180 book_stacking_msgs::PlaneInfos planeinfos;
00181 planeinfos.planes = planes;
00182 drawPlaneMarkers(planeinfos,plane_pub,r,g,b);
00183 }
00184 void drawPlaneMarkers(const book_stacking_msgs::PlaneInfos& planes, const ros::Publisher& plane_pub, float r, float g, float b)
00185 {
00186
00187
00188 visualization_msgs::MarkerArray markers;
00189 for (size_t i = 0; i < planes.planes.size(); i++){
00190
00191 pcl::PointCloud<Point> hull_pts;
00192 pcl::fromROSMsg(planes.planes[i].hull,hull_pts);
00193 visualization_msgs::Marker marker;
00194 marker.header = planes.planes[i].header;
00195 marker.ns = "plane_hulls";
00196 marker.id = i;
00197 marker.type = visualization_msgs::Marker::LINE_STRIP;
00198 marker.action = visualization_msgs::Marker::ADD;
00199 marker.color.r = r;
00200 marker.color.g = g;
00201 marker.color.b = b;
00202 marker.color.a = 0.5;
00203 marker.scale.x = 0.05;
00204 marker.scale.y = 0.05;
00205 marker.scale.z = 0.05;
00206 marker.pose.position.x = 0.0;
00207 marker.pose.position.y = 0.0;
00208 marker.pose.position.z = 0.0;
00209 marker.pose.orientation.x = 0.0;
00210 marker.pose.orientation.y = 0.0;
00211 marker.pose.orientation.z = 0.0;
00212 marker.pose.orientation.w = 1.0;
00213 marker.lifetime = ros::Duration(60.0*10.0);
00214
00215 for(size_t j = 0; j < hull_pts.points.size(); j++){
00216 geometry_msgs::Point pt;
00217 pt.x = hull_pts.points[j].x;
00218 pt.y = hull_pts.points[j].y;
00219 pt.z = hull_pts.points[j].z;
00220
00221 marker.points.push_back(pt);
00222 }
00223 geometry_msgs::Point pt;
00224 pt.x = hull_pts.points[0].x;
00225 pt.y = hull_pts.points[0].y;
00226 pt.z = hull_pts.points[0].z;
00227 marker.points.push_back(pt);
00228 markers.markers.push_back(marker);
00229 }
00230 plane_pub.publish(markers);
00231
00232 }
00233
00234 book_stacking_msgs::PlaneInfos getPlanesByNormals(const pcl::PointCloud<Point>& cloud,
00235 unsigned int max_planes, bool cluster_planes,
00236 bool use_concave, bool use_omp,
00237 double dist_thresh,
00238 double max_sac_iterations,
00239 double sac_probability,
00240 unsigned int min_inliers,
00241 double search_radius,
00242 double cluster_tolerance)
00243 {
00244
00245
00246 book_stacking_msgs::PlaneInfos all_planes;
00247 pcl::toROSMsg(cloud,all_planes.full_cloud);
00248
00249 bool more_planes = true;
00250 pcl::PointCloud<Point> remaining_cloud = cloud;
00251
00252
00253
00254
00255
00256 pcl::PointCloud<pcl::Normal> normals;
00257 if(use_omp){
00258 pcl::NormalEstimationOMP<Point,pcl::Normal> ne;
00259 ne.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
00260 pcl::KdTreeFLANN<Point>::Ptr tree (new pcl::KdTreeFLANN<Point>());
00261 ne.setSearchMethod(tree);
00262 ne.setRadiusSearch(search_radius);
00263 ne.compute(normals);
00264 } else {
00265 pcl::NormalEstimation<Point,pcl::Normal> ne;
00266 ne.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(cloud));
00267 pcl::KdTreeFLANN<Point>::Ptr tree (new pcl::KdTreeFLANN<Point>());
00268 ne.setSearchMethod(tree);
00269 ne.setRadiusSearch(search_radius);
00270 ne.compute(normals);
00271 }
00272
00273 pcl::PointCloud<pcl::Normal> remaining_normals = normals;
00274
00275 while(more_planes && (all_planes.planes.size() < max_planes) && (remaining_cloud.points.size() > min_inliers)){
00276
00277
00278 pcl::PointIndices plane_inliers;
00279 pcl::ModelCoefficients plane_coefficients;
00280
00281
00282 pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
00283 seg_.setDistanceThreshold(dist_thresh);
00284 seg_.setMaxIterations(max_sac_iterations);
00285 seg_.setNormalDistanceWeight(0.1);
00286 seg_.setOptimizeCoefficients(true);
00287 seg_.setModelType(pcl::SACMODEL_NORMAL_PLANE);
00288 seg_.setMethodType(pcl::SAC_RANSAC);
00289 seg_.setProbability(sac_probability);
00290 seg_.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(remaining_cloud));
00291 seg_.setInputNormals(boost::make_shared<pcl::PointCloud<pcl::Normal> >(remaining_normals));
00292 seg_.segment(plane_inliers,plane_coefficients);
00293
00294 if(plane_coefficients.values[3] < 0.0){
00295 plane_coefficients.values[0] = -plane_coefficients.values[0];
00296 plane_coefficients.values[1] = -plane_coefficients.values[1];
00297 plane_coefficients.values[2] = -plane_coefficients.values[2];
00298 plane_coefficients.values[3] = -plane_coefficients.values[3];
00299 }
00300
00301 if(plane_inliers.indices.size() < min_inliers){
00302 more_planes = false;
00303 } else {
00304
00305
00306 pcl::ExtractIndices<Point> get_plane_cloud;
00307 pcl::PointCloud<Point> plane_cloud;
00308 get_plane_cloud.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(remaining_cloud));
00309 get_plane_cloud.setIndices(boost::make_shared<pcl::PointIndices>(plane_inliers));
00310 get_plane_cloud.setNegative(false);
00311 get_plane_cloud.filter(plane_cloud);
00312
00313 pcl::EuclideanClusterExtraction<Point> plane_cluster;
00314 plane_cluster.setClusterTolerance(cluster_tolerance);
00315 plane_cluster.setMinClusterSize(min_inliers);
00316 std::vector<pcl::PointIndices> clusters;
00317 plane_cluster.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(plane_cloud));
00318 plane_cluster.extract(clusters);
00319
00320 if(clusters.size() == 0){
00321 more_planes = false;
00322 }
00323
00324 for(size_t i = 0; i < clusters.size(); i++){
00325 pcl::PointCloud<Point> plane_clust_cloud;
00326 pcl::copyPointCloud(plane_cloud,clusters[i],plane_clust_cloud);
00327
00328 book_stacking_msgs::PlaneInfo p;
00329 p.header = cloud.header;
00330 pcl::toROSMsg(plane_clust_cloud,p.cloud);
00331 p.model = plane_coefficients;
00332 p.inliers = clusters[i];
00333 p.normal[0] = plane_coefficients.values[0];
00334 p.normal[1] = plane_coefficients.values[1];
00335 p.normal[2] = plane_coefficients.values[2];
00336 p.normal[3] = plane_coefficients.values[3];
00337
00338
00339
00340
00341
00342 pcl::PointCloud<Point> projected_plane;
00343 pcl::ProjectInliers<Point> proj;
00344 proj.setModelType(pcl::SACMODEL_PLANE);
00345 proj.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(plane_cloud));
00346 proj.setIndices(boost::make_shared<pcl::PointIndices>(clusters[i]));
00347 proj.setModelCoefficients(boost::make_shared<pcl::ModelCoefficients>(plane_coefficients));
00348 proj.filter(projected_plane);
00349
00350 pcl::PointCloud<Point> plane_hull;
00351 if(use_concave){
00352 std::vector<pcl::Vertices> pgons;
00353 pcl::ConcaveHull<Point> hull;
00354 hull.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(projected_plane));
00355 hull.setAlpha(0.1);
00356 hull.reconstruct(plane_hull,pgons);
00357 pcl::toROSMsg(plane_hull,p.hull);
00358 } else {
00359 pcl::ConvexHull<Point> hull;
00360 hull.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(projected_plane));
00361 hull.reconstruct(plane_hull);
00362 pcl::toROSMsg(plane_hull,p.hull);
00363 }
00364
00365 ROS_INFO("Got plane with %u points",(unsigned int) p.inliers.indices.size());
00366
00367 all_planes.planes.push_back(p);
00368
00369
00370 pcl::PointCloud<Point> plane_removed_cloud;
00371 pcl::ExtractIndices<Point> extract_plane_indices;
00372 extract_plane_indices.setNegative(true);
00373 extract_plane_indices.setInputCloud(boost::make_shared<pcl::PointCloud<Point> >(remaining_cloud));
00374 extract_plane_indices.setIndices(boost::make_shared<const pcl::PointIndices>(plane_inliers));
00375 extract_plane_indices.filter(plane_removed_cloud);
00376 remaining_cloud = plane_removed_cloud;
00377
00378
00379 pcl::PointCloud<pcl::Normal> plane_removed_normals;
00380 pcl::ExtractIndices<pcl::Normal> extract_normal_indices;
00381 extract_normal_indices.setNegative(true);
00382 extract_normal_indices.setInputCloud(boost::make_shared<const pcl::PointCloud<pcl::Normal> >(remaining_normals));
00383 extract_normal_indices.setIndices(boost::make_shared<const pcl::PointIndices>(plane_inliers));
00384 extract_normal_indices.filter(plane_removed_normals);
00385 remaining_normals = plane_removed_normals;
00386
00387
00388
00389 }
00390
00391 }
00392
00393
00394
00395 }
00396
00397 return all_planes;
00398 }