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<sample_consensus::PointCloud>* cloud_notifier_;
00077 message_filters::Subscriber<sample_consensus::PointCloud>* 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<sample_consensus::PointCloud>(public_node,cloud_topic,50);
00123 cloud_notifier_ = new tf::MessageFilter<sample_consensus::PointCloud>(*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<sample_consensus::PointCloud> ("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 sample_consensus::PointCloud::ConstPtr& msg)
00316 {
00317 laser_cloud_ = *msg;
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 double z_threshold_cloud = transformDoubleValueTF (z_threshold_, robot_footprint_frame_, cloud_.header.frame_id, cloud_.header.stamp, &tf_);
00349
00350
00351 vector<int> possible_ground_indices (cloud_.points.size ());
00352 vector<int> all_indices (cloud_.points.size ());
00353 int nr_p = 0;
00354 for (unsigned int cp = 0; cp < cloud_.points.size (); cp++)
00355 {
00356 all_indices[cp] = cp;
00357 if (fabs (cloud_.points[cp].z) < z_threshold_cloud ||
00358 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))
00359 {
00360 possible_ground_indices[nr_p] = cp;
00361 nr_p++;
00362 }
00363 }
00364 possible_ground_indices.resize (nr_p);
00365
00366 ROS_DEBUG ("Number of possible ground indices: %d.", (int)possible_ground_indices.size ());
00367
00368 vector<int> ground_inliers;
00369
00370
00371 fitSACLine (&cloud_, &possible_ground_indices, ground_inliers);
00372
00373
00374 if (ground_inliers.size () == 0){
00375 ROS_DEBUG ("Couldn't fit a model to the scan.");
00376
00377 ground_inliers = possible_ground_indices;
00378 }
00379
00380 ROS_DEBUG ("Total number of ground inliers before refinement: %d.", (int)ground_inliers.size ());
00381
00382
00383 if (planar_refine_ > 0)
00384 {
00385
00386 vector<int> remaining_possible_ground_indices;
00387 sort (all_indices.begin (), all_indices.end ());
00388 sort (ground_inliers.begin (), ground_inliers.end ());
00389 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00390 inserter (remaining_possible_ground_indices, remaining_possible_ground_indices.begin ()));
00391
00392
00393 Eigen::Vector4d plane_parameters;
00394 double curvature;
00395 computePointNormal (cloud_, ground_inliers, plane_parameters, curvature);
00396
00397
00398 if (!ground_inliers.empty ())
00399 {
00400 flipNormalTowardsViewpoint (plane_parameters, cloud_.points.at (ground_inliers[0]), viewpoint_cloud_);
00401
00402
00403 for (unsigned int i = 0; i < remaining_possible_ground_indices.size (); i++)
00404 {
00405 double distance_to_ground = pointToPlaneDistanceSigned (cloud_.points.at (remaining_possible_ground_indices[i]), plane_parameters);
00406 if (distance_to_ground >= 1e-6){
00407 continue;
00408 }
00409 ground_inliers.push_back (remaining_possible_ground_indices[i]);
00410 }
00411 }
00412 }
00413 ROS_DEBUG ("Total number of ground inliers after refinement: %d.", (int)ground_inliers.size ());
00414
00415
00416 vector<int> remaining_indices;
00417 sort (ground_inliers.begin (), ground_inliers.end ());
00418 sort (all_indices.begin(), all_indices.end());
00419 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00420 inserter (remaining_indices, remaining_indices.begin ()));
00421
00422
00423 int nr_remaining_pts = remaining_indices.size ();
00424 cloud_noground_.points.resize (nr_remaining_pts);
00425
00426 for (unsigned int i = 0; i < remaining_indices.size (); i++)
00427 {
00428 cloud_noground_.points[i].x = cloud_.points.at (remaining_indices[i]).x;
00429 cloud_noground_.points[i].y = cloud_.points.at (remaining_indices[i]).y;
00430 cloud_noground_.points[i].z = cloud_.points.at (remaining_indices[i]).z;
00431 }
00432
00433 gettimeofday (&t2, NULL);
00434 double time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00435 ROS_DEBUG ("Number of points found on ground plane: %d ; remaining: %d (%g seconds).", (int)ground_inliers.size (),
00436 (int)remaining_indices.size (), time_spent);
00437 cloud_publisher_.publish (cloud_noground_);
00438 }
00439
00440
00442
00446 inline double
00447 pointToPlaneDistanceSigned (const pcl::PointXYZ &p, const Eigen::Vector4d &plane_coefficients)
00448 {
00449 return ( plane_coefficients (0) * p.x + plane_coefficients (1) * p.y + plane_coefficients (2) * p.z + plane_coefficients (3) );
00450 }
00451
00453
00458 void
00459 getCloudViewPoint (string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, tf::TransformListener *tf)
00460 {
00461
00462 geometry_msgs::PointStamped viewpoint_laser;
00463 viewpoint_laser.header.frame_id = laser_tilt_mount_frame_;
00464
00465 viewpoint_laser.point.x = viewpoint_laser.point.y = viewpoint_laser.point.z = 0.0;
00466
00467 try
00468 {
00469 tf->transformPoint (cloud_frame, viewpoint_laser, viewpoint_cloud);
00470 ROS_DEBUG ("Cloud view point in frame %s is: %g, %g, %g.", cloud_frame.c_str (),
00471 viewpoint_cloud.point.x, viewpoint_cloud.point.y, viewpoint_cloud.point.z);
00472 }
00473 catch (tf::ConnectivityException)
00474 {
00475 ROS_WARN ("Could not transform a point from frame %s to frame %s!", viewpoint_laser.header.frame_id.c_str (), cloud_frame.c_str ());
00476
00477 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00478 }
00479 catch(tf::TransformException &ex){
00480 ROS_ERROR("Can't transform viewpoint for ground plan detection");
00481 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00482 }
00483 }
00484
00486
00492 inline void
00493 transformPoint (tf::TransformListener *tf, const std::string &target_frame,
00494 const tf::Stamped< pcl::PointXYZ > &stamped_in, tf::Stamped< pcl::PointXYZ > &stamped_out)
00495 {
00496 tf::Stamped<tf::Point> tmp;
00497 tmp.stamp_ = stamped_in.stamp_;
00498 tmp.frame_id_ = stamped_in.frame_id_;
00499 tmp[0] = stamped_in.x;
00500 tmp[1] = stamped_in.y;
00501 tmp[2] = stamped_in.z;
00502
00503 try{
00504 tf->transformPoint (target_frame, tmp, tmp);
00505 }
00506 catch(tf::TransformException &ex){
00507 ROS_ERROR("Can't transform cloud for ground plane detection");
00508 return;
00509 }
00510
00511 stamped_out.stamp_ = tmp.stamp_;
00512 stamped_out.frame_id_ = tmp.frame_id_;
00513 stamped_out.x = tmp[0];
00514 stamped_out.y = tmp[1];
00515 stamped_out.z = tmp[2];
00516 }
00517
00519
00526 inline double
00527 transformDoubleValueTF (double val, std::string src_frame, std::string tgt_frame, ros::Time stamp, tf::TransformListener *tf)
00528 {
00529 pcl::PointXYZ temp;
00530 temp.x = temp.y = 0;
00531 temp.z = val;
00532 tf::Stamped<pcl::PointXYZ> temp_stamped (temp, stamp, src_frame);
00533 transformPoint (tf, tgt_frame, temp_stamped, temp_stamped);
00534 return (temp_stamped.z);
00535 }
00536
00537 };
00538
00539
00540 int
00541 main (int argc, char** argv)
00542 {
00543 ros::init (argc, argv, "sac_ground_removal");
00544
00545 ros::NodeHandle ros_node ("~");
00546
00547
00548 IncGroundRemoval p (ros_node);
00549 ros::spin ();
00550
00551 return (0);
00552 }
00553
00554