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 <sensor_msgs/PointCloud.h>
00046 #include <Eigen/QR>
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 using namespace std;
00061
00062 class IncGroundRemoval
00063 {
00064 protected:
00065 ros::NodeHandle& node_;
00066
00067 public:
00068
00069
00070 sensor_msgs::PointCloud laser_cloud_, cloud_, cloud_noground_;
00071
00072 tf::TransformListener tf_;
00073 geometry_msgs::PointStamped viewpoint_cloud_;
00074 tf::MessageFilter<sensor_msgs::PointCloud>* cloud_notifier_;
00075 message_filters::Subscriber<sensor_msgs::PointCloud>* cloud_subscriber_;
00076
00077
00078 double z_threshold_, ground_slope_threshold_;
00079 int sac_min_points_per_model_, sac_max_iterations_;
00080 double sac_distance_threshold_;
00081 double sac_fitting_distance_threshold_;
00082 int planar_refine_;
00083 std::string robot_footprint_frame_, laser_tilt_mount_frame_;
00084
00085 ros::Publisher cloud_publisher_;
00086
00088 IncGroundRemoval (ros::NodeHandle& anode) : node_ (anode)
00089 {
00090 node_.param ("z_threshold", z_threshold_, 0.1);
00091 node_.param ("ground_slope_threshold", ground_slope_threshold_, 0.0);
00092 node_.param ("sac_distance_threshold", sac_distance_threshold_, 0.03);
00093 node_.param ("sac_fitting_distance_threshold", sac_fitting_distance_threshold_, 0.015);
00094
00095 node_.param ("planar_refine", planar_refine_, 1);
00096 node_.param ("sac_min_points_per_model", sac_min_points_per_model_, 6);
00097 node_.param ("sac_max_iterations", sac_max_iterations_, 200);
00098 node_.param ("robot_footprint_frame", robot_footprint_frame_, std::string("base_footprint"));
00099 node_.param ("laser_tilt_mount_frame", laser_tilt_mount_frame_, std::string("laser_tilt_mount_link"));
00100
00101 string cloud_topic ("tilt_laser_cloud_filtered");
00102
00103 bool topic_found = false;
00104 std::vector<ros::master::TopicInfo> t_list;
00105 ros::master::getTopics (t_list);
00106 for (vector<ros::master::TopicInfo>::iterator it = t_list.begin (); it != t_list.end (); it++)
00107 {
00108 if (it->name == cloud_topic)
00109 {
00110 topic_found = true;
00111 break;
00112 }
00113 }
00114 if (!topic_found)
00115 ROS_WARN ("Trying to subscribe to %s, but the topic doesn't exist!", cloud_topic.c_str ());
00116
00117 ros::NodeHandle public_node;
00118
00119
00120 cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud>(public_node,cloud_topic,50);
00121 cloud_notifier_ = new tf::MessageFilter<sensor_msgs::PointCloud>(*cloud_subscriber_,tf_,"odom_combined",50);
00122 cloud_notifier_->registerCallback(boost::bind(&IncGroundRemoval::cloud_cb,this,_1));
00123
00124
00125
00126
00127 cloud_publisher_ = public_node.advertise<sensor_msgs::PointCloud> ("cloud_ground_filtered", 1);
00128 }
00129
00131 virtual ~IncGroundRemoval () { delete cloud_notifier_; }
00132
00134 void
00135 updateParametersFromServer ()
00136 {
00137 node_.getParam ("z_threshold", z_threshold_);
00138 node_.getParam ("ground_slope_threshold", ground_slope_threshold_);
00139 node_.getParam ("sac_fitting_distance_threshold", sac_fitting_distance_threshold_);
00140 node_.getParam ("sac_distance_threshold", sac_distance_threshold_);
00141 }
00142
00144
00150 void
00151 splitPointsBasedOnLaserScanIndex (sensor_msgs::PointCloud *points, vector<int> *indices, vector<vector<int> > &clusters, int idx)
00152 {
00153 vector<int> seed_queue;
00154 int prev_idx = -1;
00155
00156 for (unsigned int i = 0; i < indices->size (); i++)
00157 {
00158
00159 int cur_idx = points->channels[idx].values.at (indices->at (i));
00160
00161 if (cur_idx > prev_idx)
00162 {
00163 seed_queue.push_back (indices->at (i));
00164 prev_idx = cur_idx;
00165 }
00166 else
00167 {
00168 prev_idx = -1;
00169 vector<int> r;
00170 r.resize (seed_queue.size ());
00171 for (unsigned int j = 0; j < r.size (); j++)
00172 r[j] = seed_queue[j];
00173 clusters.push_back (r);
00174 seed_queue.resize (0);
00175 }
00176 }
00177
00178 if (seed_queue.size () > 0)
00179 {
00180 vector<int> r;
00181 r.resize (seed_queue.size ());
00182 for (unsigned int j = 0; j < r.size (); j++)
00183 r[j] = seed_queue[j];
00184 clusters.push_back (r);
00185 }
00186 }
00187
00189
00194 bool
00195 fitSACLine (sensor_msgs::PointCloud *points, vector<int> *indices, vector<int> &inliers)
00196 {
00197 if ((int)indices->size () < sac_min_points_per_model_)
00198 return (false);
00199
00200
00201 sample_consensus::SACModelLine *model = new sample_consensus::SACModelLine ();
00202 sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, sac_fitting_distance_threshold_);
00203 sac->setMaxIterations (sac_max_iterations_);
00204 sac->setProbability (0.99);
00205
00206 model->setDataSet (points, *indices);
00207
00208 vector<double> line_coeff;
00209
00210 if (sac->computeModel (0))
00211 {
00212
00213 if ((int)sac->getInliers ().size () < sac_min_points_per_model_)
00214 return (false);
00215
00216
00217 sac->computeCoefficients (line_coeff);
00218 sac->refineCoefficients (line_coeff);
00219 model->selectWithinDistance (line_coeff, sac_distance_threshold_, inliers);
00220
00221
00222
00223 }
00224 else
00225 return (false);
00226
00227 delete sac;
00228 delete model;
00229 return (true);
00230 }
00231
00233
00238 inline void
00239 flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
00240 {
00241
00242 float vp_m[3];
00243 vp_m[0] = viewpoint.point.x - point.x;
00244 vp_m[1] = viewpoint.point.y - point.y;
00245 vp_m[2] = viewpoint.point.z - point.z;
00246
00247
00248 double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));
00249
00250
00251 if (cos_theta < 0)
00252 {
00253 for (int d = 0; d < 3; d++)
00254 normal (d) *= -1;
00255
00256 normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
00257 }
00258 }
00259
00261
00266 inline void
00267 computeCentroid (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 ¢roid)
00268 {
00269 centroid.x = centroid.y = centroid.z = 0;
00270
00271 for (unsigned int i = 0; i < indices.size (); i++)
00272 {
00273 centroid.x += points.points.at (indices.at (i)).x;
00274 centroid.y += points.points.at (indices.at (i)).y;
00275 centroid.z += points.points.at (indices.at (i)).z;
00276 }
00277
00278 centroid.x /= indices.size ();
00279 centroid.y /= indices.size ();
00280 centroid.z /= indices.size ();
00281 }
00282
00284
00292 inline void
00293 computeCovarianceMatrix (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 ¢roid)
00294 {
00295 computeCentroid (points, indices, centroid);
00296
00297
00298 covariance_matrix = Eigen::Matrix3d::Zero ();
00299
00300 for (unsigned int j = 0; j < indices.size (); j++)
00301 {
00302 covariance_matrix (0, 0) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].x - centroid.x);
00303 covariance_matrix (0, 1) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].y - centroid.y);
00304 covariance_matrix (0, 2) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].z - centroid.z);
00305
00306 covariance_matrix (1, 0) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].x - centroid.x);
00307 covariance_matrix (1, 1) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].y - centroid.y);
00308 covariance_matrix (1, 2) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].z - centroid.z);
00309
00310 covariance_matrix (2, 0) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].x - centroid.x);
00311 covariance_matrix (2, 1) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].y - centroid.y);
00312 covariance_matrix (2, 2) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].z - centroid.z);
00313 }
00314 }
00315
00317
00327 void
00328 computePointNormal (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature)
00329 {
00330 geometry_msgs::Point32 centroid;
00331
00332 Eigen::Matrix3d covariance_matrix;
00333 computeCovarianceMatrix (points, indices, covariance_matrix, centroid);
00334
00335
00336 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
00337 Eigen::Vector3d eigen_values = ei_symm.eigenvalues ();
00338 Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();
00339
00340
00341
00342 double norm = sqrt ( eigen_vectors (0, 0) * eigen_vectors (0, 0) +
00343 eigen_vectors (1, 0) * eigen_vectors (1, 0) +
00344 eigen_vectors (2, 0) * eigen_vectors (2, 0));
00345 plane_parameters (0) = eigen_vectors (0, 0) / norm;
00346 plane_parameters (1) = eigen_vectors (1, 0) / norm;
00347 plane_parameters (2) = eigen_vectors (2, 0) / norm;
00348
00349
00350 plane_parameters (3) = -1 * (plane_parameters (0) * centroid.x + plane_parameters (1) * centroid.y + plane_parameters (2) * centroid.z);
00351
00352
00353 curvature = fabs ( eigen_values (0) / (eigen_values (0) + eigen_values (1) + eigen_values (2)) );
00354 }
00355
00357
00361 int
00362 getChannelIndex (const sensor_msgs::PointCloud &points, std::string channel_name)
00363 {
00364 for (unsigned int d = 0; d < points.channels.size (); d++)
00365 if (points.channels[d].name == channel_name)
00366 return (d);
00367 return (-1);
00368 }
00369
00371
00374 std::string
00375 getAvailableChannels (const sensor_msgs::PointCloud &cloud)
00376 {
00377 std::string result;
00378 if (cloud.channels.size () == 0)
00379 return (result);
00380 unsigned int i;
00381 for (i = 0; i < cloud.channels.size () - 1; i++)
00382 {
00383 std::string index = cloud.channels[i].name + " ";
00384 result += index;
00385 }
00386 std::string index = cloud.channels[i].name;
00387 result += index;
00388 return (result);
00389 }
00390
00392
00393 void cloud_cb (const sensor_msgs::PointCloudConstPtr& msg)
00394 {
00395 laser_cloud_ = *msg;
00396
00397 if(laser_cloud_.points.empty()){
00398 ROS_DEBUG("Received an empty point cloud");
00399 cloud_publisher_.publish(msg);
00400 return;
00401 }
00402
00403 try{
00404 tf_.transformPointCloud("odom_combined", laser_cloud_, cloud_);
00405 }
00406 catch(tf::TransformException &ex){
00407 ROS_ERROR("Can't transform cloud for ground plane detection");
00408 return;
00409 }
00410
00411 ROS_DEBUG ("Received %d data points with %d channels (%s).", (int)cloud_.points.size (), (int)cloud_.channels.size (), getAvailableChannels (cloud_).c_str ());
00412 int idx_idx = getChannelIndex (cloud_, "index");
00413 if (idx_idx == -1)
00414 {
00415 ROS_ERROR ("Channel 'index' missing in input PointCloud message!");
00416 return;
00417 }
00418 if (cloud_.points.size () == 0)
00419 return;
00420
00421
00422
00423 cloud_noground_.header = cloud_.header;
00424
00425 timeval t1, t2;
00426 gettimeofday (&t1, NULL);
00427
00428
00429 getCloudViewPoint (cloud_.header.frame_id, viewpoint_cloud_, &tf_);
00430
00431
00432 double z_threshold_cloud = transformDoubleValueTF (z_threshold_, robot_footprint_frame_, cloud_.header.frame_id, cloud_.header.stamp, &tf_);
00433
00434
00435 vector<int> possible_ground_indices (cloud_.points.size ());
00436 vector<int> all_indices (cloud_.points.size ());
00437 int nr_p = 0;
00438 for (unsigned int cp = 0; cp < cloud_.points.size (); cp++)
00439 {
00440 all_indices[cp] = cp;
00441 if (fabs (cloud_.points[cp].z) < z_threshold_cloud ||
00442 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))
00443 {
00444 possible_ground_indices[nr_p] = cp;
00445 nr_p++;
00446 }
00447 }
00448 possible_ground_indices.resize (nr_p);
00449
00450 ROS_DEBUG ("Number of possible ground indices: %d.", (int)possible_ground_indices.size ());
00451
00452 vector<int> ground_inliers;
00453
00454
00455 fitSACLine (&cloud_, &possible_ground_indices, ground_inliers);
00456
00457
00458 if (ground_inliers.size () == 0){
00459 ROS_DEBUG ("Couldn't fit a model to the scan.");
00460
00461 ground_inliers = possible_ground_indices;
00462 }
00463
00464 ROS_DEBUG ("Total number of ground inliers before refinement: %d.", (int)ground_inliers.size ());
00465
00466
00467 if (planar_refine_ > 0)
00468 {
00469
00470 vector<int> remaining_possible_ground_indices;
00471 sort (all_indices.begin (), all_indices.end ());
00472 sort (ground_inliers.begin (), ground_inliers.end ());
00473 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00474 inserter (remaining_possible_ground_indices, remaining_possible_ground_indices.begin ()));
00475
00476
00477 Eigen::Vector4d plane_parameters;
00478 double curvature;
00479 computePointNormal (cloud_, ground_inliers, plane_parameters, curvature);
00480
00481
00482 if (!ground_inliers.empty ())
00483 {
00484 flipNormalTowardsViewpoint (plane_parameters, cloud_.points.at (ground_inliers[0]), viewpoint_cloud_);
00485
00486
00487 for (unsigned int i = 0; i < remaining_possible_ground_indices.size (); i++)
00488 {
00489 double distance_to_ground = pointToPlaneDistanceSigned (cloud_.points.at (remaining_possible_ground_indices[i]), plane_parameters);
00490 if (distance_to_ground >= 1e-6){
00491 continue;
00492 }
00493 ground_inliers.push_back (remaining_possible_ground_indices[i]);
00494 }
00495 }
00496 }
00497 ROS_DEBUG ("Total number of ground inliers after refinement: %d.", (int)ground_inliers.size ());
00498
00499 #if DEBUG
00500
00501 cloud_noground_.points.resize (possible_ground_indices.size ());
00502 cloud_noground_.channels.resize (1);
00503 cloud_noground_.channels[0].name = "rgb";
00504 cloud_noground_.channels[0].values.resize (possible_ground_indices.size ());
00505
00506 cloud_noground_.points.resize (ground_inliers.size ());
00507 cloud_noground_.channels[0].values.resize (ground_inliers.size ());
00508 float r = rand () / (RAND_MAX + 1.0);
00509 float g = rand () / (RAND_MAX + 1.0);
00510 float b = rand () / (RAND_MAX + 1.0);
00511
00512 for (unsigned int i = 0; i < ground_inliers.size (); i++)
00513 {
00514 cloud_noground_.points[i].x = cloud_.points.at (ground_inliers[i]).x;
00515 cloud_noground_.points[i].y = cloud_.points.at (ground_inliers[i]).y;
00516 cloud_noground_.points[i].z = cloud_.points.at (ground_inliers[i]).z;
00517 cloud_noground_.channels[0].values[i] = getRGB (r, g, b);
00518 }
00519 cloud_publisher_.publish (cloud_noground_);
00520
00521 return;
00522 #endif
00523
00524
00525 vector<int> remaining_indices;
00526 sort (ground_inliers.begin (), ground_inliers.end ());
00527 sort (all_indices.begin(), all_indices.end());
00528 set_difference (all_indices.begin (), all_indices.end (), ground_inliers.begin (), ground_inliers.end (),
00529 inserter (remaining_indices, remaining_indices.begin ()));
00530
00531
00532 int nr_remaining_pts = remaining_indices.size ();
00533 cloud_noground_.points.resize (nr_remaining_pts);
00534 cloud_noground_.channels.resize (cloud_.channels.size ());
00535 for (unsigned int d = 0; d < cloud_.channels.size (); d++)
00536 {
00537 cloud_noground_.channels[d].name = cloud_.channels[d].name;
00538 cloud_noground_.channels[d].values.resize (nr_remaining_pts);
00539 }
00540
00541 for (unsigned int i = 0; i < remaining_indices.size (); i++)
00542 {
00543 cloud_noground_.points[i].x = cloud_.points.at (remaining_indices[i]).x;
00544 cloud_noground_.points[i].y = cloud_.points.at (remaining_indices[i]).y;
00545 cloud_noground_.points[i].z = cloud_.points.at (remaining_indices[i]).z;
00546 for (unsigned int d = 0; d < cloud_.channels.size (); d++)
00547 cloud_noground_.channels[d].values[i] = cloud_.channels[d].values.at (remaining_indices[i]);
00548 }
00549
00550 gettimeofday (&t2, NULL);
00551 double time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
00552 ROS_DEBUG ("Number of points found on ground plane: %d ; remaining: %d (%g seconds).", (int)ground_inliers.size (),
00553 (int)remaining_indices.size (), time_spent);
00554 cloud_publisher_.publish (cloud_noground_);
00555 }
00556
00557
00559
00563 inline double
00564 pointToPlaneDistanceSigned (const geometry_msgs::Point32 &p, const Eigen::Vector4d &plane_coefficients)
00565 {
00566 return ( plane_coefficients (0) * p.x + plane_coefficients (1) * p.y + plane_coefficients (2) * p.z + plane_coefficients (3) );
00567 }
00568
00570
00575 void
00576 getCloudViewPoint (string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, tf::TransformListener *tf)
00577 {
00578
00579 geometry_msgs::PointStamped viewpoint_laser;
00580 viewpoint_laser.header.frame_id = laser_tilt_mount_frame_;
00581
00582 viewpoint_laser.point.x = viewpoint_laser.point.y = viewpoint_laser.point.z = 0.0;
00583
00584 try
00585 {
00586 tf->transformPoint (cloud_frame, viewpoint_laser, viewpoint_cloud);
00587 ROS_DEBUG ("Cloud view point in frame %s is: %g, %g, %g.", cloud_frame.c_str (),
00588 viewpoint_cloud.point.x, viewpoint_cloud.point.y, viewpoint_cloud.point.z);
00589 }
00590 catch (tf::ConnectivityException)
00591 {
00592 ROS_WARN ("Could not transform a point from frame %s to frame %s!", viewpoint_laser.header.frame_id.c_str (), cloud_frame.c_str ());
00593
00594 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00595 }
00596 catch(tf::TransformException &ex){
00597 ROS_ERROR("Can't transform viewpoint for ground plan detection");
00598 viewpoint_cloud.point.x = 0.05; viewpoint_cloud.point.y = 0.0; viewpoint_cloud.point.z = 0.942768;
00599 }
00600 }
00601
00603
00609 inline void
00610 transformPoint (tf::TransformListener *tf, const std::string &target_frame,
00611 const tf::Stamped< geometry_msgs::Point32 > &stamped_in, tf::Stamped< geometry_msgs::Point32 > &stamped_out)
00612 {
00613 tf::Stamped<tf::Point> tmp;
00614 tmp.stamp_ = stamped_in.stamp_;
00615 tmp.frame_id_ = stamped_in.frame_id_;
00616 tmp[0] = stamped_in.x;
00617 tmp[1] = stamped_in.y;
00618 tmp[2] = stamped_in.z;
00619
00620 try{
00621 tf->transformPoint (target_frame, tmp, tmp);
00622 }
00623 catch(tf::TransformException &ex){
00624 ROS_ERROR("Can't transform cloud for ground plane detection");
00625 return;
00626 }
00627
00628 stamped_out.stamp_ = tmp.stamp_;
00629 stamped_out.frame_id_ = tmp.frame_id_;
00630 stamped_out.x = tmp[0];
00631 stamped_out.y = tmp[1];
00632 stamped_out.z = tmp[2];
00633 }
00634
00636
00643 inline double
00644 transformDoubleValueTF (double val, std::string src_frame, std::string tgt_frame, ros::Time stamp, tf::TransformListener *tf)
00645 {
00646 geometry_msgs::Point32 temp;
00647 temp.x = temp.y = 0;
00648 temp.z = val;
00649 tf::Stamped<geometry_msgs::Point32> temp_stamped (temp, stamp, src_frame);
00650 transformPoint (tf, tgt_frame, temp_stamped, temp_stamped);
00651 return (temp_stamped.z);
00652 }
00653
00654 };
00655
00656
00657 int
00658 main (int argc, char** argv)
00659 {
00660 ros::init (argc, argv, "sac_ground_removal");
00661
00662 ros::NodeHandle ros_node ("~");
00663
00664
00665 IncGroundRemoval p (ros_node);
00666 ros::spin ();
00667
00668 return (0);
00669 }
00670
00671