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
00029
00030
00042
00043 #include <ros/ros.h>
00044
00045 #include <Eigen/QR>
00046 #include <Eigen/Eigenvalues>
00047
00048 #include <sac.h>
00049 #include <ransac.h>
00050 #include <sac_model_line.h>
00051
00052 #include <tf/transform_listener.h>
00053 #include "tf/message_filter.h"
00054 #include "message_filters/subscriber.h"
00055
00056 #include <sys/time.h>
00057
00058 #include <boost/thread.hpp>
00059
00060 #include <pcl_ros/transforms.h>
00061
00062 using namespace std;
00063
00064 class IncGroundRemoval
00065 {
00066 protected:
00067 ros::NodeHandle& node_;
00068
00069 public:
00070
00071
00072 sample_consensus::PointCloud laser_cloud_, cloud_, cloud_noground_;
00073
00074 tf::TransformListener tf_;
00075 geometry_msgs::PointStamped viewpoint_cloud_;
00076 tf::MessageFilter<sensor_msgs::PointCloud2>* cloud_notifier_;
00077 message_filters::Subscriber<sensor_msgs::PointCloud2>* cloud_subscriber_;
00078
00079
00080 double z_threshold_, ground_slope_threshold_;
00081 int sac_min_points_per_model_, sac_max_iterations_;
00082 double sac_distance_threshold_;
00083 double sac_fitting_distance_threshold_;
00084 int planar_refine_;
00085 std::string robot_footprint_frame_, laser_tilt_mount_frame_;
00086
00087 ros::Publisher cloud_publisher_;
00088
00090 IncGroundRemoval (ros::NodeHandle& anode) : node_ (anode)
00091 {
00092 node_.param ("z_threshold", z_threshold_, 0.1);
00093 node_.param ("ground_slope_threshold", ground_slope_threshold_, 0.0);
00094 node_.param ("sac_distance_threshold", sac_distance_threshold_, 0.03);
00095 node_.param ("sac_fitting_distance_threshold", sac_fitting_distance_threshold_, 0.015);
00096
00097 node_.param ("planar_refine", planar_refine_, 1);
00098 node_.param ("sac_min_points_per_model", sac_min_points_per_model_, 6);
00099 node_.param ("sac_max_iterations", sac_max_iterations_, 200);
00100 node_.param ("robot_footprint_frame", robot_footprint_frame_, std::string("base_footprint"));
00101 node_.param ("laser_tilt_mount_frame", laser_tilt_mount_frame_, std::string("laser_tilt_mount_link"));
00102
00103 string cloud_topic ("tilt_laser_cloud_filtered");
00104
00105 bool topic_found = false;
00106 std::vector<ros::master::TopicInfo> t_list;
00107 ros::master::getTopics (t_list);
00108 for (vector<ros::master::TopicInfo>::iterator it = t_list.begin (); it != t_list.end (); it++)
00109 {
00110 if (it->name == cloud_topic)
00111 {
00112 topic_found = true;
00113 break;
00114 }
00115 }
00116 if (!topic_found)
00117 ROS_WARN ("Trying to subscribe to %s, but the topic doesn't exist!", cloud_topic.c_str ());
00118
00119 ros::NodeHandle public_node;
00120
00121
00122 cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(public_node,cloud_topic,50);
00123 cloud_notifier_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*cloud_subscriber_,tf_,"odom_combined",50);
00124 cloud_notifier_->registerCallback(boost::bind(&IncGroundRemoval::cloud_cb,this,_1));
00125
00126
00127
00128
00129 cloud_publisher_ = public_node.advertise<sensor_msgs::PointCloud2> ("cloud_ground_filtered", 1);
00130 }
00131
00133 virtual ~IncGroundRemoval () { delete cloud_notifier_; }
00134
00136 void
00137 updateParametersFromServer ()
00138 {
00139 node_.getParam ("z_threshold", z_threshold_);
00140 node_.getParam ("ground_slope_threshold", ground_slope_threshold_);
00141 node_.getParam ("sac_fitting_distance_threshold", sac_fitting_distance_threshold_);
00142 node_.getParam ("sac_distance_threshold", sac_distance_threshold_);
00143 }
00144
00146
00151 bool
00152 fitSACLine (sample_consensus::PointCloud *points, vector<int> *indices, vector<int> &inliers)
00153 {
00154 if ((int)indices->size () < sac_min_points_per_model_)
00155 return (false);
00156
00157
00158 sample_consensus::SACModelLine *model = new sample_consensus::SACModelLine ();
00159 sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, sac_fitting_distance_threshold_);
00160 sac->setMaxIterations (sac_max_iterations_);
00161 sac->setProbability (0.99);
00162
00163 model->setDataSet (points, *indices);
00164
00165 vector<double> line_coeff;
00166
00167 if (sac->computeModel (0))
00168 {
00169
00170 if ((int)sac->getInliers ().size () < sac_min_points_per_model_)
00171 return (false);
00172
00173
00174 sac->computeCoefficients (line_coeff);
00175 sac->refineCoefficients (line_coeff);
00176 model->selectWithinDistance (line_coeff, sac_distance_threshold_, inliers);
00177
00178
00179
00180 }
00181 else
00182 return (false);
00183
00184 delete sac;
00185 delete model;
00186 return (true);
00187 }
00188
00190
00195 inline void
00196 flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const pcl::PointXYZ &point, const geometry_msgs::PointStamped &viewpoint)
00197 {
00198
00199 float vp_m[3];
00200 vp_m[0] = viewpoint.point.x - point.x;
00201 vp_m[1] = viewpoint.point.y - point.y;
00202 vp_m[2] = viewpoint.point.z - point.z;
00203
00204
00205 double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));
00206
00207
00208 if (cos_theta < 0)
00209 {
00210 for (int d = 0; d < 3; d++)
00211 normal (d) *= -1;
00212
00213 normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
00214 }
00215 }
00216
00218
00223 inline void
00224 computeCentroid (const sample_consensus::PointCloud &points, const std::vector<int> &indices, pcl::PointXYZ ¢roid)
00225 {
00226 centroid.x = centroid.y = centroid.z = 0;
00227
00228 for (unsigned int i = 0; i < indices.size (); i++)
00229 {
00230 centroid.x += points.points.at (indices.at (i)).x;
00231 centroid.y += points.points.at (indices.at (i)).y;
00232 centroid.z += points.points.at (indices.at (i)).z;
00233 }
00234
00235 centroid.x /= indices.size ();
00236 centroid.y /= indices.size ();
00237 centroid.z /= indices.size ();
00238 }
00239
00241
00249 inline void
00250 computeCovarianceMatrix (const sample_consensus::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, pcl::PointXYZ ¢roid)
00251 {
00252 computeCentroid (points, indices, centroid);
00253
00254
00255 covariance_matrix = Eigen::Matrix3d::Zero ();
00256
00257 for (unsigned int j = 0; j < indices.size (); j++)
00258 {
00259 covariance_matrix (0, 0) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].x - centroid.x);
00260 covariance_matrix (0, 1) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].y - centroid.y);
00261 covariance_matrix (0, 2) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].z - centroid.z);
00262
00263 covariance_matrix (1, 0) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].x - centroid.x);
00264 covariance_matrix (1, 1) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].y - centroid.y);
00265 covariance_matrix (1, 2) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].z - centroid.z);
00266
00267 covariance_matrix (2, 0) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].x - centroid.x);
00268 covariance_matrix (2, 1) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].y - centroid.y);
00269 covariance_matrix (2, 2) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].z - centroid.z);
00270 }
00271 }
00272
00274
00284 void
00285 computePointNormal (const sample_consensus::PointCloud &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature)
00286 {
00287 pcl::PointXYZ centroid;
00288
00289 Eigen::Matrix3d covariance_matrix;
00290 computeCovarianceMatrix (points, indices, covariance_matrix, centroid);
00291
00292
00293 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
00294 Eigen::Vector3d eigen_values = ei_symm.eigenvalues ();
00295 Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();
00296
00297
00298
00299 double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
00300 eigen_vectors (1, 0) * eigen_vectors (1, 0) +
00301 eigen_vectors (2, 0) * eigen_vectors (2, 0));
00302 plane_parameters (0) = eigen_vectors (0, 0) / norm;
00303 plane_parameters (1) = eigen_vectors (1, 0) / norm;
00304 plane_parameters (2) = eigen_vectors (2, 0) / norm;
00305
00306
00307 plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters (1) * centroid.y + plane_parameters (2) * centroid.z);
00308
00309
00310 curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
00311 }
00312
00314
00315 void cloud_cb (const sensor_msgs::PointCloud2::ConstPtr& msg)
00316 {
00317 pcl::fromROSMsg(*msg,laser_cloud_);
00318
00319 if(laser_cloud_.points.empty()){
00320 ROS_DEBUG("Received an empty point cloud");
00321 cloud_publisher_.publish(msg);
00322 return;
00323 }
00324
00325 try{
00326 pcl_ros::transformPointCloud("odom_combined", laser_cloud_, cloud_, tf_);
00327 }
00328 catch(tf::TransformException &ex){
00329 ROS_ERROR("Can't transform cloud for ground plane detection");
00330 return;
00331 }
00332
00333 ROS_DEBUG ("Received %d data points.", (int)cloud_.points.size ());
00334 if (cloud_.points.size () == 0)
00335 return;
00336
00337
00338
00339 cloud_noground_.header = cloud_.header;
00340
00341 timeval t1, t2;
00342 gettimeofday (&t1, NULL);
00343
00344
00345 getCloudViewPoint (cloud_.header.frame_id, viewpoint_cloud_, &tf_);
00346
00347
00348 std_msgs::Header header = pcl_conversions::fromPCL(cloud_.header);
00349 double z_threshold_cloud = transformDoubleValueTF (z_threshold_, robot_footprint_frame_, cloud_.header.frame_id, header.stamp, &tf_);
00350
00351
00352 vector<int> possible_ground_indices (cloud_.points.size ());
00353 vector<int> all_indices (cloud_.points.size ());
00354 int nr_p = 0;
00355 for (unsigned int cp = 0; cp < cloud_.points.size (); cp++)
00356 {
00357 all_indices[cp] = cp;
00358 if (fabs (cloud_.points[cp].z) < z_threshold_cloud ||
00359 cloud_.points[cp].z*cloud_.points[cp].z < ground_slope_threshold_ * (cloud_.points[cp].x*cloud_.points[cp].x + cloud_.points[cp].y*cloud_.points[cp].y))
00360 {
00361 possible_ground_indices[nr_p] = cp;
00362 nr_p++;
00363 }
00364 }
00365 possible_ground_indices.resize (nr_p);
00366
00367 ROS_DEBUG ("Number of possible ground indices: %d.", (int)possible_ground_indices.size ());
00368
00369 vector<int> ground_inliers;
00370
00371
00372 fitSACLine (&cloud_, &possible_ground_indices, ground_inliers);
00373
00374
00375 if (ground_inliers.size () == 0){
00376 ROS_DEBUG ("Couldn't fit a model to the scan.");
00377
00378 ground_inliers = possible_ground_indices;
00379 }
00380
00381 ROS_DEBUG ("Total number of ground inliers before refinement: %d.", (int)ground_inliers.size ());
00382
00383
00384 if (planar_refine_ > 0)
00385 {
00386
00387 vector<int> remaining_possible_ground_indices;
00388 sort (all_indices.begin (), all_indices.end ());
00389 sort (ground_inliers.begin (), ground_inliers.end ());
00390 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00391 inserter (remaining_possible_ground_indices, remaining_possible_ground_indices.begin ()));
00392
00393
00394 Eigen::Vector4d plane_parameters;
00395 double curvature;
00396 computePointNormal (cloud_, ground_inliers, plane_parameters, curvature);
00397
00398
00399 if (!ground_inliers.empty ())
00400 {
00401 flipNormalTowardsViewpoint (plane_parameters, cloud_.points.at (ground_inliers[0]), viewpoint_cloud_);
00402
00403
00404 for (unsigned int i = 0; i < remaining_possible_ground_indices.size (); i++)
00405 {
00406 double distance_to_ground = pointToPlaneDistanceSigned (cloud_.points.at (remaining_possible_ground_indices[i]), plane_parameters);
00407 if (distance_to_ground >= 1e-6){
00408 continue;
00409 }
00410 ground_inliers.push_back (remaining_possible_ground_indices[i]);
00411 }
00412 }
00413 }
00414 ROS_DEBUG ("Total number of ground inliers after refinement: %d.", (int)ground_inliers.size ());
00415
00416
00417 vector<int> remaining_indices;
00418 sort (ground_inliers.begin (), ground_inliers.end ());
00419 sort (all_indices.begin(), all_indices.end());
00420 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00421 inserter (remaining_indices, remaining_indices.begin ()));
00422
00423
00424 int nr_remaining_pts = remaining_indices.size ();
00425 cloud_noground_.points.resize (nr_remaining_pts);
00426
00427 for (unsigned int i = 0; i < remaining_indices.size (); i++)
00428 {
00429 cloud_noground_.points[i].x = cloud_.points.at (remaining_indices[i]).x;
00430 cloud_noground_.points[i].y = cloud_.points.at (remaining_indices[i]).y;
00431 cloud_noground_.points[i].z = cloud_.points.at (remaining_indices[i]).z;
00432 }
00433
00434 gettimeofday (&t2, NULL);
00435 double time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00436 ROS_DEBUG ("Number of points found on ground plane: %d ; remaining: %d (%g seconds).", (int)ground_inliers.size (),
00437 (int)remaining_indices.size (), time_spent);
00438 sensor_msgs::PointCloud2 out;
00439 pcl::toROSMsg(cloud_noground_, out);
00440 cloud_publisher_.publish (out);
00441 }
00442
00443
00445
00449 inline double
00450 pointToPlaneDistanceSigned (const pcl::PointXYZ &p, const Eigen::Vector4d &plane_coefficients)
00451 {
00452 return ( plane_coefficients (0) * p.x + plane_coefficients (1) * p.y + plane_coefficients (2) * p.z + plane_coefficients (3) );
00453 }
00454
00456
00461 void
00462 getCloudViewPoint (string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, tf::TransformListener *tf)
00463 {
00464
00465 geometry_msgs::PointStamped viewpoint_laser;
00466 viewpoint_laser.header.frame_id = laser_tilt_mount_frame_;
00467
00468 viewpoint_laser.point.x = viewpoint_laser.point.y = viewpoint_laser.point.z = 0.0;
00469
00470 try
00471 {
00472 tf->transformPoint (cloud_frame, viewpoint_laser, viewpoint_cloud);
00473 ROS_DEBUG ("Cloud view point in frame %s is: %g, %g, %g.", cloud_frame.c_str (),
00474 viewpoint_cloud.point.x, viewpoint_cloud.point.y, viewpoint_cloud.point.z);
00475 }
00476 catch (tf::ConnectivityException)
00477 {
00478 ROS_WARN ("Could not transform a point from frame %s to frame %s!", viewpoint_laser.header.frame_id.c_str (), cloud_frame.c_str ());
00479
00480 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00481 }
00482 catch(tf::TransformException &ex){
00483 ROS_ERROR("Can't transform viewpoint for ground plan detection");
00484 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00485 }
00486 }
00487
00489
00495 inline void
00496 transformPoint (tf::TransformListener *tf, const std::string &target_frame,
00497 const tf::Stamped< pcl::PointXYZ > &stamped_in, tf::Stamped< pcl::PointXYZ > &stamped_out)
00498 {
00499 tf::Stamped<tf::Point> tmp;
00500 tmp.stamp_ = stamped_in.stamp_;
00501 tmp.frame_id_ = stamped_in.frame_id_;
00502 tmp[0] = stamped_in.x;
00503 tmp[1] = stamped_in.y;
00504 tmp[2] = stamped_in.z;
00505
00506 try{
00507 tf->transformPoint (target_frame, tmp, tmp);
00508 }
00509 catch(tf::TransformException &ex){
00510 ROS_ERROR("Can't transform cloud for ground plane detection");
00511 return;
00512 }
00513
00514 stamped_out.stamp_ = tmp.stamp_;
00515 stamped_out.frame_id_ = tmp.frame_id_;
00516 stamped_out.x = tmp[0];
00517 stamped_out.y = tmp[1];
00518 stamped_out.z = tmp[2];
00519 }
00520
00522
00529 inline double
00530 transformDoubleValueTF (double val, std::string src_frame, std::string tgt_frame, ros::Time stamp, tf::TransformListener *tf)
00531 {
00532 pcl::PointXYZ temp;
00533 temp.x = temp.y = 0;
00534 temp.z = val;
00535 tf::Stamped<pcl::PointXYZ> temp_stamped (temp, stamp, src_frame);
00536 transformPoint (tf, tgt_frame, temp_stamped, temp_stamped);
00537 return (temp_stamped.z);
00538 }
00539
00540 };
00541
00542
00543 int
00544 main (int argc, char** argv)
00545 {
00546 ros::init (argc, argv, "sac_ground_removal");
00547
00548 ros::NodeHandle ros_node ("~");
00549
00550
00551 IncGroundRemoval p (ros_node);
00552 ros::spin ();
00553
00554 return (0);
00555 }
00556
00557