fast_plane_detector_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
00003  * ({bohg,celle}@csc.kth.se) 
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are
00008  * met:
00009  *
00010  *  1.Redistributions of source code must retain the above copyright
00011  *    notice, this list of conditions and the following disclaimer.
00012  *  2.Redistributions in binary form must reproduce the above
00013  *    copyright notice, this list of conditions and the following
00014  *    disclaimer in the documentation and/or other materials provided
00015  *    with the distribution.  
00016  *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
00017  *    endorse or promote products derived from this software without 
00018  *    specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00023  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00024  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00025  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00026  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00027  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00028  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00029  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00030  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <ros/ros.h>
00034 #include <std_msgs/String.h>
00035 
00036 #include <sensor_msgs/CameraInfo.h>
00037 #include <stereo_msgs/DisparityImage.h>
00038 
00039 #include <fast_plane_detection/Plane.h>
00040 
00041 #include <visualization_msgs/Marker.h>
00042 
00043 
00044 #include "fast_plane_detector/plane_detection.h"
00045 #include "fast_plane_detector/plane_transform.h"
00046 
00047 #include <image_transport/image_transport.h>
00048 
00049 #include "fast_plane_detector/timercpu.h"
00050 #include "fast_plane_detector/utils.h"
00051 
00052 namespace fast_plane_detection {
00053   
00054   class FastPlaneDetector
00055   {
00056     
00057   private:
00058     
00059     void clearOldMarkers(std::string frame_id);
00060     
00061     void maskDisparityImage(const stereo_msgs::DisparityImage::ConstPtr&
00062                             disparity_image,
00063                             const sensor_msgs::Image &labels,
00064                             stereo_msgs::DisparityImage &masked_disparity);
00065 
00067     ros::NodeHandle root_nh_;
00069     ros::NodeHandle priv_nh_;
00070     
00072     ros::Subscriber camera_info_sub_;
00073     std::string camera_info_topic_;
00074 
00076     ros::Subscriber disparity_image_sub_;
00077     std::string disparity_image_topic_;
00078 
00080     int buffer_size_;
00081     
00083     stereo_msgs::DisparityImage::ConstPtr disparity_image_;
00084 
00086     sensor_msgs::CameraInfo::ConstPtr camera_info_;
00087         
00089     ros::Publisher plane_marker_pub_;
00090 
00092     ros::Publisher plane_msg_pub_;
00093 
00095     ros::Publisher disparity_image_pub_;
00096 
00098     image_transport::Publisher image_pub_;
00099 
00100     
00101 
00103     PlaneDetection* plane_detect_;
00104 
00106     bool find_table_;
00107   
00109     PlaneTransform* plane_transform_;
00110 
00112     double up_direction_;
00113     
00115     int inlier_threshold_;
00116 
00118     int num_markers_published_;
00120     int current_marker_id_;
00121 
00122   public:
00123     
00124     FastPlaneDetector( int buffer_size );
00125     ~FastPlaneDetector();
00126     
00127     //------------------ Callbacks -------------------
00128 
00130     void planeCallback(const stereo_msgs::DisparityImage::ConstPtr& 
00131                        disparity_image);
00132 
00133     //------------------ The Data --------------------
00134 
00136     Plane plane_;
00137     
00138   };
00139   
00140   
00141   FastPlaneDetector::FastPlaneDetector( int buffer_size)
00142     : root_nh_("")
00143     , priv_nh_("~")
00144     , buffer_size_(buffer_size)
00145     , num_markers_published_(0)
00146     , current_marker_id_(0)
00147   {
00148     
00149     // initialise operational flags
00150     priv_nh_.param<std::string>("camera_info_topic", 
00151                                 camera_info_topic_, 
00152                                 "/narrow_stereo_textured/left/camera_info");
00153     priv_nh_.param<std::string>("disparity_image_topic", 
00154                                 disparity_image_topic_, 
00155                                 "/narrow_stereo_textured/disparity");
00156     priv_nh_.param<bool>("find_table", 
00157                          find_table_, 
00158                          true);  
00159     priv_nh_.param<double>("up_direction", 
00160                            up_direction_, 
00161                            -1.0f);  
00162     priv_nh_.param<int>("inlier_threshold", 
00163                         inlier_threshold_, 
00164                         300);  
00165 
00166     // retrieve camera info just once and one sample disparity 
00167     // for parameter extraction
00168     while (!camera_info_ || ! disparity_image_){
00169       if( !camera_info_ ){
00170         ROS_INFO_STREAM("  Waiting for message on topic " 
00171                         << camera_info_topic_);
00172         camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00173           (camera_info_topic_, root_nh_, ros::Duration(0.5));
00174       }
00175 
00176       if( !disparity_image_){
00177         ROS_INFO_STREAM("  Waiting for message on topic " 
00178                         << disparity_image_topic_);
00179         disparity_image_ = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00180           (disparity_image_topic_, root_nh_, ros::Duration(1.0));
00181       }
00182     }
00183     
00184     // get parameters from disparity image
00185     float drange = disparity_image_->max_disparity - 
00186       disparity_image_->min_disparity;
00187     int w = disparity_image_->image.width;
00188     int h = disparity_image_->image.height;
00189     
00190     plane_detect_ = new PlaneDetection((int)drange, w, h, find_table_, 
00191                                        disparity_image_->image.header);
00192 
00193     plane_transform_ = new PlaneTransform( *camera_info_, 
00194                                            disparity_image_->T, 
00195                                            up_direction_);
00196     // subscribe to disparity images
00197     disparity_image_sub_ = root_nh_.subscribe(disparity_image_topic_, 
00198                                               buffer_size_, 
00199                                               &FastPlaneDetector::planeCallback,
00200                                               this);
00201     
00202     // publish plane markers
00203     plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("plane_markers", 1); 
00204 
00205     // publish plane msg
00206     plane_msg_pub_ = root_nh_.advertise<Plane>("plane_msg", 1);
00207 
00208     // publish masked disparity images
00209     std::string topic = root_nh_.resolveName("/plane_detection/disparity");
00210     disparity_image_pub_ 
00211       = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
00212     
00214     image_transport::ImageTransport it_(root_nh_);
00215     image_pub_ = it_.advertise("plane_out", 1);
00216     
00217   }
00218 
00219   FastPlaneDetector::~FastPlaneDetector()
00220   {}
00221 
00222 
00223   void FastPlaneDetector::planeCallback(const stereo_msgs::DisparityImage::ConstPtr& disparity_image) 
00224   {
00225     TimerCPU timer(2800);
00226     
00227     plane_detect_->Update(disparity_image);
00228 
00229     float alpha, beta, d;
00230     int min_x, max_x, min_y, max_y;
00231     plane_detect_->GetPlaneParameters( alpha, beta, d, 
00232                                        min_x, max_x, min_y, max_y);
00233     
00234     sensor_msgs::Image labels;
00235     plane_detect_->GetLabels(labels);
00236 
00237     stereo_msgs::DisparityImage disparity_masked;
00238     maskDisparityImage(disparity_image, labels, disparity_masked);
00239     
00240     int sum = 0;
00241     for(unsigned int i=0; i<labels.height; ++i)
00242       for(unsigned int j=0; j<labels.width; ++j){
00243         int adr = i*labels.width+j; 
00244         sum+=(labels.data[adr])/255;
00245       }
00246       
00247     
00248     image_pub_.publish(labels);
00249     disparity_image_pub_.publish(disparity_masked);
00250     
00251 
00252     if(sum<inlier_threshold_){
00253       ROS_WARN("Plane has only %d inliers", sum);
00254       return;
00255     }
00256     
00257     plane_transform_->setParameters( alpha, 
00258                                      beta, 
00259                                      d,
00260                                      min_x, max_x, 
00261                                      min_y, max_y, 
00262                                      disparity_image->header);
00263     
00264     if(!plane_transform_->getPlane(plane_)) {
00265       ROS_WARN("No Plane found");
00266       return; 
00267     } else {
00268       
00269       visualization_msgs::Marker planeMarker 
00270         = getPlaneMarker( plane_.top_left, 
00271                           plane_.top_right,
00272                           plane_.bottom_left, 
00273                           plane_.bottom_right);
00274       planeMarker.header = disparity_image->header;
00275       planeMarker.pose = plane_.pose.pose;
00276       planeMarker.ns = "tabletop_node";
00277       planeMarker.id = current_marker_id_++;
00278       plane_marker_pub_.publish(planeMarker);
00279       
00280       /*      ROS_INFO("Before clearing old");
00281       ROS_INFO("current_marker_id %d", current_marker_id_);
00282       ROS_INFO("num_markers_published %d", num_markers_published_);
00283       */
00284       clearOldMarkers(disparity_image->header.frame_id);
00285 
00286       /*
00287       ROS_INFO("After clearing old");
00288       ROS_INFO("current_marker_id %d", current_marker_id_);
00289       ROS_INFO("num_markers_published %d", num_markers_published_);
00290       */
00291     }
00292 
00293     ROS_INFO_STREAM("Time: "  << timer.read() << " ms");
00294     
00295   }
00296   
00297   void FastPlaneDetector::clearOldMarkers(std::string frame_id)
00298   {
00299     for (int id=current_marker_id_; id < num_markers_published_; id++)
00300       {
00301 
00302         ROS_WARN("Cleared old markers");
00303 
00304         visualization_msgs::Marker delete_marker;
00305         delete_marker.header.stamp = ros::Time::now();
00306         delete_marker.header.frame_id = frame_id;
00307         delete_marker.id = id;
00308         delete_marker.action = visualization_msgs::Marker::DELETE;
00309         delete_marker.ns = "tabletop_node";
00310         plane_marker_pub_.publish(delete_marker);
00311       }
00312     
00313     num_markers_published_ = current_marker_id_;
00314     current_marker_id_ = 0;
00315   }
00316  
00317 
00318   void 
00319   FastPlaneDetector::maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr
00320                                          &disparity_image,
00321                                          const sensor_msgs::Image &labels,
00322                                          stereo_msgs::DisparityImage &masked_disparity)
00323   {
00324     int height = labels.height;
00325     int width  = labels.width;
00326     
00327     masked_disparity.header = disparity_image->header;
00328     masked_disparity.image.header = disparity_image->image.header;
00329     masked_disparity.image.height = height;
00330     masked_disparity.image.width = width;
00331     masked_disparity.image.encoding = disparity_image->image.encoding;
00332     masked_disparity.image.is_bigendian = disparity_image->image.is_bigendian;
00333     masked_disparity.image.step = disparity_image->image.step;
00334     size_t size = masked_disparity.image.step * masked_disparity.image.height;
00335     masked_disparity.image.data.resize(size, 0);
00336 
00337     masked_disparity.f = disparity_image->f;
00338     masked_disparity.T = disparity_image->T;
00339     masked_disparity.min_disparity = disparity_image->min_disparity;
00340     masked_disparity.max_disparity = disparity_image->max_disparity;
00341     masked_disparity.delta_d = disparity_image->delta_d;
00342 
00343     float *dimd_in  = (float*)&(disparity_image->image.data[0]);
00344     uint8_t *labelsd = (uint8_t*)&(labels.data[0]);
00345     
00346     float *dimd_out = (float*)&(masked_disparity.image.data[0]);
00347     
00348     for (int y=0;y<height; y++) {
00349       for (int x=0;x<width; x++) {
00350         int adr = y*width+x;
00351         if(labelsd[adr]==0)
00352           dimd_out[adr] = dimd_in[adr];
00353         else dimd_out[adr] = -1.0f;
00354       }
00355     }
00356   }
00357 }
00358 
00359 int main(int argc, char **argv)
00360 {
00361   ros::init(argc, argv, "fast_plane_detection_node");
00362   
00364   int buffer_size_ = 1;
00365   fast_plane_detection::FastPlaneDetector node_(buffer_size_);
00366   ros::spin();
00367   
00368   return 0;
00369 }


fast_plane_detection
Author(s): Jeannette Bohg
autogenerated on Fri Jan 3 2014 12:11:20