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 #ifndef _PLANE_MODEL_ROAD_SEGMENTATION_HPP_
00029 #define _PLANE_MODEL_ROAD_SEGMENTATION_HPP_
00030
00037
00038
00039 template <class T>
00040 tf::Transform plane_model_road<T>::get_transform(T origin_projected, T pt_x_projected, T pt_y_projected)
00041 {
00042 tf::Transform transform2;
00043 tf::Quaternion quaternion;
00044 transform2.setOrigin( tf::Vector3((float)origin_projected.x, (float)origin_projected.y, (float)origin_projected.z) );
00045
00046
00047
00048 vector<double> RP=find_RP_angle(origin_projected, pt_x_projected, pt_y_projected);
00049
00050 if (debug)
00051 {
00052 cout << "PMS_DEBUG: R= " << RP[0]<< " P= " << RP[1]<< " Y= "<< RP[3] << endl;
00053 }
00054
00055 quaternion.setRPY(RP[0]-(pi/2),RP[1]-(pi/2),0.0);
00056 transform2.setRotation(quaternion);
00057 return transform2;
00058 }
00060
00061 template<class T>
00062 bool plane_model_road<T>::find_road_frame(pcl::PointCloud<T> cloud_in)
00063 {
00064
00065 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00066 plane_model_road::outliers=plane_model_road::get_plane_coefficients(cloud_in, plane_model_road::iterations, plane_model_road::threshold ,coefficients);
00067
00068 plane_model_road::coef=coefficients;
00069
00070
00071 T origin_projected=plane_model_road::project_point_to_plane(plane_model_road::point_origin, coefficients);
00072 T pt_x_projected=plane_model_road::project_point_to_plane(plane_model_road::point_x, coefficients);
00073 T pt_y_projected=plane_model_road::project_point_to_plane(plane_model_road::point_y, coefficients);
00074
00075 plane_model_road::origin_projected=origin_projected;
00076 plane_model_road::pt_x_projected=pt_x_projected;
00077 plane_model_road::pt_y_projected=pt_y_projected;
00078
00079
00080 T point2 = plane_model_road::transform_pcl_point(cloud_in.header.frame_id,plane_model_road::reference_ground_frame,plane_model_road::point_origin);
00081
00082
00083 double distance=point2.z-origin_projected.z;
00084 distance=abs(distance);
00085 if (debug)
00086 cout << "PMS_DEBUG: Distance to reference frame "<< distance << endl;
00087
00088 bool succeed=plane_model_road::check_distance(distance,threshold);
00089 return succeed;
00090 }
00092
00093 template<class T>
00094 bool plane_model_road<T>::find_road_frame(const sensor_msgs::PointCloud2ConstPtr& msg)
00095 {
00096 pcl::PointCloud<T> cloud_in;
00097 pcl::fromROSMsg(*msg,cloud_in);
00098 return plane_model_road::find_road_frame(cloud_in);
00099 };
00101
00102 template<class T>
00103 tf::Transform plane_model_road<T>::find_and_publish_road_frame(pcl::PointCloud<T> cloud_in)
00104 {
00105 plane_model_road::outliers=cloud_in;
00106 bool succeed=plane_model_road::find_road_frame(plane_model_road::outliers);
00107 static tf::Transform transform;
00108 transform=plane_model_road::get_transform(plane_model_road::origin_projected, plane_model_road::pt_x_projected, plane_model_road::pt_y_projected);
00109 if (succeed)
00110 {
00111 plane_model_road::broadcast_ptr->sendTransform(tf::StampedTransform(transform, ros::Time::now(),cloud_in.header.frame_id, plane_model_road::pub_frame_name));
00112 }
00113 return transform;
00114 };
00116
00117 template<class T>
00118 tf::Transform plane_model_road<T>::find_and_publish_road_frame(const sensor_msgs::PointCloud2ConstPtr& msg)
00119 {
00120 pcl::PointCloud<T> cloud_in;
00121 pcl::fromROSMsg(*msg,cloud_in);
00122 return plane_model_road::find_and_publish_road_frame(cloud_in);
00123 };
00125
00126 template<class T>
00127 pcl::PointCloud<T> plane_model_road<T>::get_plane_coefficients(pcl::PointCloud<T> cloud_in, int iterations, float threshold, pcl::ModelCoefficients::Ptr coefficients)
00128 {
00129 typename pcl::PointCloud<T>::Ptr cloud_in_ptr (new pcl::PointCloud<T> (cloud_in));
00130 pcl::PointCloud<T> cloud_out;
00131
00132 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00133
00134
00135 pcl::SACSegmentation<T> seg;
00136 pcl::ExtractIndices<T> extract;
00137
00138 seg.setOptimizeCoefficients (true);
00139
00140 seg.setModelType (pcl::SACMODEL_PLANE);
00141 seg.setMethodType (pcl::SAC_RANSAC);
00142 seg.setMaxIterations (iterations);
00143 seg.setDistanceThreshold (threshold);
00144 seg.setInputCloud (cloud_in_ptr);
00145 seg.segment (*inliers, *coefficients);
00146 extract.setInputCloud (cloud_in_ptr);
00147 extract.setIndices (inliers);
00148 extract.setNegative (true);
00149 extract.filter (cloud_out);
00150
00151 cloud_in_ptr.reset();
00152 inliers.reset();
00153 return cloud_out;
00154 };
00156
00157
00158
00159
00160 template<class T>
00161 T plane_model_road<T>::project_point_to_plane(const T ptin, const pcl::ModelCoefficients::Ptr coeff)
00162 {
00163 pcl::PointCloud<T> pcin;
00164 pcin.points.push_back(ptin);
00165
00166 typename pcl::PointCloud<T>::Ptr pcin_ptr (new pcl::PointCloud<T> (pcin) );
00167
00168
00169 pcl::PointCloud<T> pcout;
00170
00171 pcl::ProjectInliers<T> projection;
00172 projection.setModelType(pcl::SACMODEL_PLANE);
00173 projection.setInputCloud(pcin_ptr);
00174 projection.setModelCoefficients(coeff);
00175 projection.filter(pcout);
00176 T ptout = pcout.points[0];
00177 return ptout;
00178 };
00180
00181 template<class T>
00182 T plane_model_road<T>::transform_pcl_point(std::string frame1, std::string frame2, T pt_origin)
00183 {
00184 tf::StampedTransform transform_ground;
00185 try
00186 {
00187 plane_model_road::listener_atc_ground_ptr->lookupTransform(frame1, frame2, ros::Time(0), transform_ground);
00188 }
00189 catch (tf::TransformException ex)
00190 {
00191 ROS_ERROR("%s",ex.what());
00192 }
00193
00194 pcl::PointCloud<T> pc_point;
00195 pc_point.points.push_back(pt_origin);
00196 pcl_ros::transformPointCloud (pc_point,pc_point,transform_ground);
00197 return pc_point.points[0];
00198 };
00200
00201 template<class T>
00202 vector<double> plane_model_road<T>::find_RP_angle(T origin_projected, T pt_x_projected, T pt_y_projected)
00203 {
00204 double R,P;
00205
00206
00207
00208 vector<double> vect_x;
00209 vect_x.push_back(pt_x_projected.x-origin_projected.x);
00210 vect_x.push_back(pt_x_projected.y-origin_projected.y);
00211 vect_x.push_back(pt_x_projected.z-origin_projected.z);
00212
00213 vector<double> vect_y;
00214 vect_y.push_back(pt_y_projected.x-origin_projected.x);
00215 vect_y.push_back(pt_y_projected.y-origin_projected.y);
00216 vect_y.push_back(pt_y_projected.z-origin_projected.z);
00217
00218 vector<double> vect_z;
00219 vect_z.push_back(0); vect_z.push_back(0); vect_z.push_back(1);
00220
00221 double norm_x=sqrt(pow(vect_x[0],2)+pow(vect_x[1],2)+pow(vect_x[2],2));
00222 double norm_y=sqrt(pow(vect_y[0],2)+pow(vect_y[1],2)+pow(vect_y[2],2));
00223 double norm_z=sqrt(pow(vect_z[0],2)+pow(vect_z[1],2)+pow(vect_z[2],2));
00224
00225 R=acos((vect_y[0]*vect_z[0]+vect_y[1]*vect_z[1]+vect_y[2]*vect_z[2])/norm_y*norm_z);
00226 P=acos((vect_x[0]*vect_z[0]+vect_x[1]*vect_z[1]+vect_x[2]*vect_z[2])/norm_x*norm_z);
00227
00228 vector<double> result;
00229 result.push_back(R); result.push_back(P);
00230
00231 return result;
00232 }
00233
00234 #endif