32 #ifndef _PLANE_ROAD_EXTRACTION_HPP_
33 #define _PLANE_ROAD_EXTRACTION_HPP_
42 fromROSMsg(*msg,cloud);
44 if ((
int)cloud.points.size()>0)
46 tf::Transform road_tf=pms.find_and_publish_road_frame(cloud);
50 T point_origin_transformed;
52 T point_away_transformed;
55 point_origin.x=0; point_origin.y=0; point_origin.z=0;
56 point_away.x=50; point_away.y=0; point_away.z=0;
57 point_origin_transformed=transform_pcl_point(cloud.header.frame_id, road_tf, point_origin);
58 point_away_transformed=transform_pcl_point(cloud.header.frame_id, road_tf, point_away);
60 PointCloud<T> road_hull;
61 road_hull.header.frame_id=cloud.header.frame_id;
62 pt.x = -2; pt.y=50; pt.z=point_origin_transformed.z;
63 road_hull.points.push_back(pt);
64 pt.x = 50; pt.y=50; pt.z=point_away_transformed.z;
65 road_hull.points.push_back(pt);
66 pt.x = 50; pt.y= -50; pt.z=point_away_transformed.z;
67 road_hull.points.push_back(pt);
68 pt.x = -2; pt.y=-50; pt.z=point_origin_transformed.z;
69 road_hull.points.push_back(pt);
72 PointCloud<T> cloud_road;
73 PointCloud<T> cloud_cluster;
74 points_from_volume<T> pfv_cloud;
75 cloud_road.header.frame_id=cloud.header.frame_id;
76 cloud_cluster.header.frame_id=cloud.header.frame_id;
77 pfv_cloud.set_convex_hull(road_hull);
78 pfv_cloud.convexhull_function(cloud, z_max, z_min,
false);
79 cloud_road=pfv_cloud.get_pc_in_volume();
80 pfv_cloud.convexhull_function(cloud, 20, z_max,
false);
81 cloud_cluster=pfv_cloud.get_pc_in_volume();
84 PointCloud<T> cloud_road_projected;
85 cloud_road_projected = project_cloud_on_plane(cloud_road, pms.coef);
88 PointCloud<T> road_perimeter;
90 chull.setInputCloud (cloud_road_projected.makeShared());
91 chull.reconstruct (road_perimeter);
94 points_from_volume<T> pfv;
95 PointCloud<T> inside_cloud;
96 inside_cloud.header.frame_id=msg->header.frame_id;
97 pfv.set_convex_hull(road_perimeter);
98 pfv.convexhull_function(cloud_cluster, 20, -20,
false);
99 inside_cloud=pfv.get_pc_in_volume();
103 sensor_msgs::PointCloud2 msg_road;
104 sensor_msgs::PointCloud2 msg_cluster;
105 sensor_msgs::PointCloud2 msg_perimeter;
106 toROSMsg(cloud_road_projected,msg_road);
107 toROSMsg(inside_cloud,msg_cluster);
108 toROSMsg(road_perimeter,msg_perimeter);
109 pub_clusters_ptr->publish(msg_cluster);
110 pub_road_ptr->publish(msg_road);
111 pub_road_perimeter_ptr->publish(msg_perimeter);
120 PointCloud<T> pc_point;
121 pc_point.points.push_back(pt_origin);
122 pcl_ros::transformPointCloud (pc_point,pc_point,transform);
123 return pc_point.points[0];
130 typename PointCloud<T>::Ptr cloud_in_ptr (
new PointCloud<T> (cloud_in));
131 PointCloud<T> cloud_out;
134 ProjectInliers<T> projection;
135 projection.setModelType(SACMODEL_PLANE);
137 projection.setInputCloud(cloud_in_ptr);
138 projection.setModelCoefficients(coefficients);
139 projection.filter(cloud_out);
141 cloud_in_ptr.reset();