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 #ifndef _plane_model_segmentation_H_
00028 #define _plane_model_segmentation_H_
00029
00030 #include <ros/ros.h>
00031 #include <sensor_msgs/PointCloud2.h>
00032
00033
00034 #include <tf/transform_broadcaster.h>
00035 #include <tf/transform_listener.h>
00036
00037
00038 #include <pcl/point_cloud.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl/ros/conversions.h>
00041 #include <pcl/filters/voxel_grid.h>
00042 #include <pcl_ros/transforms.h>
00043 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00044 #include <pcl/filters/extract_indices.h>
00045 #include <pcl/segmentation/sac_segmentation.h>
00046 #include <pcl/sample_consensus/method_types.h>
00047 #include <pcl/sample_consensus/model_types.h>
00048 #include <pcl/filters/project_inliers.h>
00049
00050 using namespace std;
00051
00052 #define pi 3.1415926
00053
00070 template <class T>
00071 class plane_model_road
00072 {
00073 public:
00074
00086 bool find_road_frame(pcl::PointCloud<T> cloud_in);
00087
00094 bool find_road_frame(const sensor_msgs::PointCloud2ConstPtr& msg);
00095
00107 tf::Transform find_and_publish_road_frame(pcl::PointCloud<T> cloud_in);
00108
00115 tf::Transform find_and_publish_road_frame(const sensor_msgs::PointCloud2ConstPtr& msg);
00116
00131 pcl::PointCloud<T> get_plane_coefficients(pcl::PointCloud<T> cloud_in, int iterations, float threshold, pcl::ModelCoefficients::Ptr coefficients);
00132
00133
00134 float threshold;
00135 int iterations;
00136 bool debug;
00137 T point_origin;
00138 T point_x;
00139 T point_y;
00140 tf::TransformListener *listener_atc_ground_ptr;
00141 tf::TransformBroadcaster *broadcast_ptr;
00142 std::string pub_frame_name;
00143 std::string reference_ground_frame;
00144 pcl::ModelCoefficients::Ptr coef;
00145
00146
00147 plane_model_road()
00148 {
00149 threshold=0.01;
00150 iterations=50;
00151 debug=false;
00152
00153 point_origin.x=2; point_origin.y=0; point_origin.z=0;
00154 point_x.x=4; point_x.y=0; point_x.z=0;
00155 point_y.x=4; point_y.y=0; point_y.z=0;
00156 pub_frame_name="/environment/road_frame";
00157 reference_ground_frame="/atc/vehicle/ground";
00158 };
00159
00160 ~plane_model_road()
00161 {
00162
00163 };
00164
00165 private:
00166
00167
00168 pcl::PointCloud<T> outliers;
00169 T origin_projected;
00170 T pt_x_projected;
00171 T pt_y_projected;
00172
00173
00181 T project_point_to_plane(const T ptin, const pcl::ModelCoefficients::Ptr coeff);
00182
00191 T transform_pcl_point(std::string frame1, std::string frame2, T pt_origin);
00192
00200 inline bool check_distance(double distance, double threshold)
00201 {
00202 if (distance<=threshold)
00203 {
00204 if (debug)
00205 cout << "PMS_DEBUG: ROAD FRAME FOUND" << endl;
00206 return true;
00207 }
00208 else
00209 return false;
00210 };
00211
00212
00221 vector<double> find_RP_angle(T origin_projected, T pt_x_projected, T pt_y_projected);
00222
00231 tf::Transform get_transform(T origin_projected, T pt_x_projected, T pt_y_projected);
00232
00233 };
00234
00235 #include "plane_model_road_segmentation_implementation.hpp"
00236
00237 #endif