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
00031
00032
00033
00034
00035
00036
00037
00046 #include <boost/thread/mutex.hpp>
00047
00048
00049 #include <ros/node_handle.h>
00050
00051 #include <sensor_msgs/PointCloud.h>
00052 #include <geometry_msgs/PoseStamped.h>
00053 #include <geometry_msgs/Vector3Stamped.h>
00054
00055 #include <visualization_msgs/Marker.h>
00056
00057
00058 #include <stereo_wall_detection/geometry/angles.h>
00059 #include <stereo_wall_detection/geometry/point.h>
00060 #include <stereo_wall_detection/geometry/areas.h>
00061 #include <stereo_wall_detection/geometry/nearest.h>
00062 #include <stereo_wall_detection/geometry/intersections.h>
00063 #include <Eigen/Geometry>
00064
00065
00066 #include <stereo_wall_detection/sample_consensus/sac.h>
00067 #include <stereo_wall_detection/sample_consensus/msac.h>
00068 #include <stereo_wall_detection/sample_consensus/sac_model_normal_plane.h>
00069
00070 #include <stereo_wall_detection/DetectWall.h>
00071 using namespace std;
00072
00073 class PlanarFit
00074 {
00075 protected:
00076 ros::NodeHandle nh_;
00077
00078 public:
00079
00080 sensor_msgs::PointCloud cloud_msg_;
00081 boost::mutex cloud_msg_mutex_;
00082 ros::ServiceServer serv_;
00083 ros::Publisher plane_normal_pub_;
00084 ros::Publisher vis_pub_;
00085
00086
00087 double radius_;
00088 int k_;
00089
00090
00091 geometry_msgs::Point leaf_width_;
00092
00093
00094 int normals_fidelity_;
00095
00096 double sac_distance_threshold_;
00097 int sac_use_normals_;
00098 double sac_normal_distance_weight_;
00099 double sac_normal_inlier_distance_weight_;
00100
00101 double max_dist_;
00102 string cloud_topic_;
00103 bool subscriber_enabled_, received_cloud_;
00104
00106 PlanarFit () : nh_ ("~"), cloud_topic_ ("narrow_stereo_textured/points"), subscriber_enabled_(false)
00107 {
00108 nh_.param ("search_radius", radius_, 0.03);
00109
00110 nh_.param ("downsample_leaf_width_x", leaf_width_.x, 0.015);
00111 nh_.param ("downsample_leaf_width_y", leaf_width_.y, 0.015);
00112 nh_.param ("downsample_leaf_width_z", leaf_width_.z, 0.015);
00113 nh_.param ("downsample_max_dist", max_dist_, 3.0);
00114
00115 nh_.param ("normals_high_fidelity", normals_fidelity_, 1);
00116
00117 nh_.param ("sac_distance_threshold", sac_distance_threshold_, 0.03);
00118 nh_.param ("sac_use_normals", sac_use_normals_, 0);
00119 nh_.param ("sac_normal_distance_weight", sac_normal_distance_weight_, 0.05);
00120 nh_.param ("sac_normal_inlier_distance_weight", sac_normal_inlier_distance_weight_, 0.05);
00121
00122 ros::NodeHandle nh_toplevel_;
00123 serv_ = nh_.advertiseService ("detect_wall", &PlanarFit::detect_wall, this);
00124
00125
00126 vis_pub_ = nh_.advertise<visualization_msgs::Marker> ("visualization_marker", 0);
00127 }
00128
00130 virtual ~PlanarFit () { }
00131
00133
00134 void
00135 cloud_cb (const sensor_msgs::PointCloud::ConstPtr& cloud)
00136 {
00137 if(!subscriber_enabled_)
00138 return;
00139 boost::mutex::scoped_lock(cloud_msg_mutex_);
00140 cloud_msg_ = *cloud;
00141 received_cloud_ = true;
00142 }
00143
00145
00146 bool
00147 detect_wall (stereo_wall_detection::DetectWall::Request &req, stereo_wall_detection::DetectWall::Response &resp)
00148 {
00149 received_cloud_ = false;
00150
00151
00152 ros::NodeHandle nh_toplevel;
00153 ros::Subscriber cloud_sub = nh_toplevel.subscribe<sensor_msgs::PointCloud> (cloud_topic_, 1, &PlanarFit::cloud_cb, this);
00154 ros::Time start = ros::Time::now();
00155 bool have_cloud = false;
00156 subscriber_enabled_ = true;
00157 sensor_msgs::PointCloud cloud_msg_local;
00158 while (!have_cloud){
00159 ros::Duration(0.1).sleep();
00160 boost::mutex::scoped_lock(cloud_msg_mutex_);
00161 if (received_cloud_)
00162 {
00163 if(cloud_msg_.points.size() >= 50000){
00164
00165 cloud_msg_local = cloud_msg_;
00166 have_cloud = true;
00167 }
00168 }
00169 else
00170 {
00171 if (ros::Time::now() > start + ros::Duration(10.0)){
00172 if (!received_cloud_)
00173 ROS_ERROR("Timed out waiting for point cloud");
00174 else
00175 ROS_ERROR("Only received a point cloud of size %d", (int)cloud_msg_local.points.size());
00176 subscriber_enabled_ = false;
00177 return false;
00178 }
00179 }
00180 }
00181
00182 subscriber_enabled_ = false;
00183
00184 ROS_INFO("point cloud of size %u", (unsigned int)(cloud_msg_local.points.size()));
00185 if (cloud_msg_local.points.size () == 0){
00186 ROS_ERROR("Received point cloud of size zero");
00187 return (false);
00188 }
00189
00190 sensor_msgs::PointCloud cloud_down;
00191 cloud_down.header = cloud_msg_local.header;
00192
00193 ros::Time ts = ros::Time::now ();
00194
00195 geometry_msgs::PointStamped viewpoint_cloud;
00196 viewpoint_cloud.point.x = 0; viewpoint_cloud.point.y = 0; viewpoint_cloud.point.z = 0;
00197
00198
00199 int nrp = 0;
00200 vector<int> indices_down (cloud_msg_local.points.size ());
00201 for (size_t i = 0; i < indices_down.size (); ++i)
00202 if (cloud_msg_local.points[i].z < max_dist_)
00203 indices_down[nrp++] = i;
00204 indices_down.resize (nrp);
00205
00206 if (indices_down.size () == 0){
00207 ROS_ERROR("No points found in plane");
00208 return (false);
00209 }
00210 try
00211 {
00212
00213 vector<cloud_geometry::Leaf> leaves;
00214 cloud_geometry::downsamplePointCloud (cloud_msg_local, indices_down, cloud_down, leaf_width_, leaves, -1);
00215 }
00216 catch (std::bad_alloc)
00217 {
00218 ROS_ERROR ("Not enough memory to create a simple downsampled representation. Change the downsample_leaf_width parameters.");
00219 return (false);
00220 }
00221
00222 if (normals_fidelity_)
00223 cloud_geometry::nearest::computePointCloudNormals (cloud_down, cloud_msg_local, radius_, viewpoint_cloud);
00224 else
00225 cloud_geometry::nearest::computePointCloudNormals (cloud_down, radius_, viewpoint_cloud);
00226
00227
00228
00229 vector<int> inliers;
00230 vector<double> coeff;
00231 if (!fitSACPlane (&cloud_down, inliers, coeff, viewpoint_cloud, sac_distance_threshold_)){
00232 ROS_ERROR("Failed to fit planar model");
00233 return (false);
00234 }
00235
00236
00237 coeff[0] = -coeff[0]; coeff[1] = -coeff[1]; coeff[2] = -coeff[2];
00238
00239 resp.wall_norm.header = cloud_msg_local.header;
00240 resp.wall_norm.vector.x = coeff[0];
00241 resp.wall_norm.vector.y = coeff[1];
00242 resp.wall_norm.vector.z = coeff[2];
00243
00244 geometry_msgs::Point32 centroid;
00245 cloud_geometry::nearest::computeCentroid (cloud_down, inliers, centroid);
00246
00247
00248
00249
00250 resp.wall_point.header = cloud_msg_local.header;
00251 resp.wall_point.point.x = centroid.x;
00252 resp.wall_point.point.y = centroid.y;
00253 resp.wall_point.point.z = centroid.z;
00254
00255 Eigen::AngleAxisf aa (acos (coeff[0]), Eigen::Vector3f (0, -coeff[2], coeff[1]));
00256 Eigen::Quaternionf q (aa);
00257
00258 publishNormal (centroid, q, cloud_msg_local.header, 0.1);
00259 ROS_INFO("Found wall at %f %f %f with normal: %f %f %f in frame %s",
00260 centroid.x, centroid.y, centroid.z, coeff[0], coeff[1], coeff[2], cloud_msg_local.header.frame_id.c_str());
00261
00262
00263 return (true);
00264 }
00265
00267
00275 bool
00276 fitSACPlane (sensor_msgs::PointCloud *points, vector<int> &inliers, vector<double> &coeff,
00277 const geometry_msgs::PointStamped &viewpoint_cloud, double dist_thresh)
00278 {
00279
00280 sample_consensus::SACModelPlane *model;
00281 sample_consensus::SACModelNormalPlane *normal_model = new sample_consensus::SACModelNormalPlane ();
00282
00283 if (sac_use_normals_)
00284 {
00285 normal_model->setNormalDistanceWeight(sac_normal_distance_weight_);
00286 model = (sample_consensus::SACModelPlane *) normal_model;
00287 }
00288 else
00289 model = new sample_consensus::SACModelPlane ();
00290
00291 sample_consensus::SAC *sac = new sample_consensus::MSAC (model, dist_thresh);
00292 model->setDataSet (points);
00293
00294
00295 if (sac->computeModel (0))
00296 {
00297 sac->computeCoefficients (coeff);
00298 sac->refineCoefficients (coeff);
00299
00300 normal_model->setNormalDistanceWeight (sac_normal_inlier_distance_weight_);
00301 model->selectWithinDistance (coeff, dist_thresh, inliers);
00302
00303 cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points->points.at (inliers[0]), viewpoint_cloud);
00304 }
00305 else
00306 {
00307 ROS_ERROR ("Could not compute a plane model.");
00308 return (false);
00309 }
00310
00311 delete sac;
00312 delete model;
00313 return (true);
00314 }
00315
00317
00327 void
00328 publishNormal (const geometry_msgs::Point32 &position, Eigen::Quaternionf orientation, const std_msgs::Header &header, double length)
00329 {
00330 visualization_msgs::Marker marker;
00331 marker.header.frame_id = header.frame_id;
00332
00333 marker.header.stamp = header.stamp;
00334 marker.ns = "plane_normal";
00335 marker.id = 0;
00336 marker.type = visualization_msgs::Marker::ARROW;
00337 marker.action = visualization_msgs::Marker::ADD;
00338 marker.pose.position.x = position.x;
00339 marker.pose.position.y = position.y;
00340 marker.pose.position.z = position.z;
00341 marker.pose.orientation.x = orientation.x ();
00342 marker.pose.orientation.y = orientation.y ();
00343 marker.pose.orientation.z = orientation.z ();
00344 marker.pose.orientation.w = orientation.w ();
00345 marker.scale.x = length;
00346 marker.scale.y = length;
00347 marker.scale.z = length;
00348 marker.color.a = 1.0;
00349 marker.color.r = 0.0;
00350 marker.color.g = 0.0;
00351 marker.color.b = 1.0;
00352 vis_pub_.publish (marker);
00353 }
00354 };
00355
00356
00357 int
00358 main (int argc, char** argv)
00359 {
00360 ros::init (argc, argv, "wall_extractor");
00361
00362 PlanarFit p;
00363 ros::MultiThreadedSpinner spinner(2);
00364 spinner.spin();
00365
00366 return (0);
00367 }
00368
00369