28 #ifndef _PLANE_MODEL_ROAD_SEGMENTATION_HPP_
29 #define _PLANE_MODEL_ROAD_SEGMENTATION_HPP_
42 tf::Transform transform2;
43 tf::Quaternion quaternion;
44 transform2.setOrigin( tf::Vector3((
float)origin_projected.x, (
float)origin_projected.y, (
float)origin_projected.z) );
48 vector<double> RP=find_RP_angle(origin_projected, pt_x_projected, pt_y_projected);
52 cout <<
"PMS_DEBUG: R= " << RP[0]<<
" P= " << RP[1]<<
" Y= "<< RP[3] << endl;
55 quaternion.setRPY(RP[0]-(
pi/2),RP[1]-(
pi/2),0.0);
56 transform2.setRotation(quaternion);
65 pcl::ModelCoefficients::Ptr coefficients (
new pcl::ModelCoefficients ());
83 double distance=point2.z-origin_projected.z;
84 distance=abs(distance);
86 cout <<
"PMS_DEBUG: Distance to reference frame "<< distance << endl;
96 pcl::PointCloud<T> cloud_in;
98 pcl::PCLPointCloud2 pcl_pc;
99 pcl_conversions::toPCL(*msg, pcl_pc);
100 pcl::fromPCLPointCloud2(pcl_pc, cloud_in);
111 static tf::Transform transform;
124 pcl::PointCloud<T> cloud_in;
125 pcl::fromROSMsg(*msg,cloud_in);
133 typename pcl::PointCloud<T>::Ptr cloud_in_ptr (
new pcl::PointCloud<T> (cloud_in));
134 pcl::PointCloud<T> cloud_out;
136 pcl::PointIndices::Ptr inliers (
new pcl::PointIndices ());
139 pcl::SACSegmentation<T> seg;
140 pcl::ExtractIndices<T> extract;
142 seg.setOptimizeCoefficients (
true);
144 seg.setModelType (pcl::SACMODEL_PLANE);
145 seg.setMethodType (pcl::SAC_RANSAC);
146 seg.setMaxIterations (iterations);
147 seg.setDistanceThreshold (threshold);
148 seg.setInputCloud (cloud_in_ptr);
149 seg.segment (*inliers, *coefficients);
150 extract.setInputCloud (cloud_in_ptr);
151 extract.setIndices (inliers);
152 extract.setNegative (
true);
153 extract.filter (cloud_out);
155 cloud_in_ptr.reset();
167 pcl::PointCloud<T> pcin;
168 pcin.points.push_back(ptin);
170 typename pcl::PointCloud<T>::Ptr pcin_ptr (
new pcl::PointCloud<T> (pcin) );
173 pcl::PointCloud<T> pcout;
175 pcl::ProjectInliers<T> projection;
176 projection.setModelType(pcl::SACMODEL_PLANE);
177 projection.setInputCloud(pcin_ptr);
178 projection.setModelCoefficients(coeff);
179 projection.filter(pcout);
180 T ptout = pcout.points[0];
188 tf::StampedTransform transform_ground;
193 catch (tf::TransformException ex)
195 ROS_ERROR(
"%s",ex.what());
198 pcl::PointCloud<T> pc_point;
199 pc_point.points.push_back(pt_origin);
200 pcl_ros::transformPointCloud (pc_point,pc_point,transform_ground);
201 return pc_point.points[0];
212 vector<double> vect_x;
213 vect_x.push_back(pt_x_projected.x-origin_projected.x);
214 vect_x.push_back(pt_x_projected.y-origin_projected.y);
215 vect_x.push_back(pt_x_projected.z-origin_projected.z);
217 vector<double> vect_y;
218 vect_y.push_back(pt_y_projected.x-origin_projected.x);
219 vect_y.push_back(pt_y_projected.y-origin_projected.y);
220 vect_y.push_back(pt_y_projected.z-origin_projected.z);
222 vector<double> vect_z;
223 vect_z.push_back(0); vect_z.push_back(0); vect_z.push_back(1);
225 double norm_x=sqrt(pow(vect_x[0],2)+pow(vect_x[1],2)+pow(vect_x[2],2));
226 double norm_y=sqrt(pow(vect_y[0],2)+pow(vect_y[1],2)+pow(vect_y[2],2));
227 double norm_z=sqrt(pow(vect_z[0],2)+pow(vect_z[1],2)+pow(vect_z[2],2));
229 R=acos((vect_y[0]*vect_z[0]+vect_y[1]*vect_z[1]+vect_y[2]*vect_z[2])/norm_y*norm_z);
230 P=acos((vect_x[0]*vect_z[0]+vect_x[1]*vect_z[1]+vect_x[2]*vect_z[2])/norm_x*norm_z);
232 vector<double> result;
233 result.push_back(R); result.push_back(P);
T transform_pcl_point(std::string frame1, std::string frame2, T pt_origin)
Transform a pcl point between two ros frames It uses a tf::Stamped transform to transform a point bet...
std::string pub_frame_name
tf::TransformBroadcaster * broadcast_ptr
pcl::PointCloud< T > outliers
tf::TransformListener * listener_atc_ground_ptr
T project_point_to_plane(const T ptin, const pcl::ModelCoefficients::Ptr coeff)
Project a point on a plane It uses pcl projectinliers to project a point on a plane.
pcl::PointCloud< T > get_plane_coefficients(pcl::PointCloud< T > cloud_in, int iterations, float threshold, pcl::ModelCoefficients::Ptr coefficients)
Calculate plane coefficients It uses pcl RANSAC model to estimate the coeeficients of a plane...
std::string reference_ground_frame
tf::Transform get_transform(T origin_projected, T pt_x_projected, T pt_y_projected)
get the transformation point Gets the road transform to publish
pcl::ModelCoefficients::Ptr coef
bool check_distance(double distance, double threshold)
check if distance matches with threshold If returned true, you probably get the road plane ...
tf::Transform find_and_publish_road_frame(pcl::PointCloud< T > cloud_in)
get and publish the road frame T his f*unction does all the work for you. here are the main steps: ST...
bool find_road_frame(pcl::PointCloud< T > cloud_in)
get the road reference frame
vector< double > find_RP_angle(T origin_projected, T pt_x_projected, T pt_y_projected)
find RP angles for road frame Gets the road frame RP angles. Notice that you don't need the Y...