fast_plane_detector_server.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 #include <fast_plane_detection/PlaneInRegion.h>
00041 
00042 #include <fast_plane_detector/msg_saver.h>
00043 
00044 #include <visualization_msgs/Marker.h>
00045 
00046 #include "fast_plane_detector/plane_detection.h"
00047 #include "fast_plane_detector/plane_transform.h"
00048 
00049 
00050 #include <tf/transform_listener.h>
00051 #include <image_transport/image_transport.h>
00052 
00053 #include "fast_plane_detector/timercpu.h"
00054 
00055 namespace fast_plane_detection 
00056 {
00057   
00058   class FastRegionPlaneService
00059   {
00060     
00061   private:
00062     
00063     bool maskDisparityImage( fast_plane_detection::PlaneInRegion::Request &req,
00064                              const stereo_msgs::DisparityImage::ConstPtr& disparity,
00065                              const sensor_msgs::CameraInfo::ConstPtr& camera_info,
00066                              const stereo_msgs::DisparityImage::Ptr &masked_disparity);
00067 
00069     ros::NodeHandle root_nh_;
00071     ros::NodeHandle priv_nh_;
00072         
00074     PlaneDetection* plane_detect_;
00075 
00077     PlaneTransform* plane_transform_;
00078 
00081     double up_direction_;
00082     
00084     int inlier_threshold_;
00085     
00087     ros::Publisher disparity_image_pub_;
00088 
00089   public:
00090     
00091     FastRegionPlaneService();
00092     ~FastRegionPlaneService();
00093     
00094     //------------------ Callbacks -------------------
00095 
00097     bool planeCallback(fast_plane_detection::PlaneInRegion::Request &req,
00098                        fast_plane_detection::PlaneInRegion::Response &res );
00099     //------------------ The Data --------------------
00100     
00102     Plane plane_;
00103     
00104   };
00105   
00106   
00107   FastRegionPlaneService::FastRegionPlaneService()
00108     : root_nh_("")
00109     , priv_nh_("~")
00110   {
00111     
00112     priv_nh_.param<double>("up_direction", 
00113                            up_direction_, 
00114                            -1.0f);  
00115     priv_nh_.param<int>("inlier_threshold", 
00116                         inlier_threshold_, 
00117                         300);  
00118 
00119     // publish masked disparity images (for debug only)
00120     std::string topic = root_nh_.resolveName("/plane_detection/masked_disparity");
00121     disparity_image_pub_ 
00122       = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
00123   }
00124     
00125   FastRegionPlaneService::~FastRegionPlaneService()
00126   {}
00127   
00128     
00129   bool 
00130   FastRegionPlaneService::planeCallback(fast_plane_detection::PlaneInRegion::Request &req,
00131                                          fast_plane_detection::PlaneInRegion::Response &res )
00132     
00133   {
00134 
00135     ROS_INFO("Calling fast plane detection service");
00136         
00137     // get camera info and disparity from generic topics that have 
00138     // to be remapped in the launch file
00139     std::string topic_disp = root_nh_.resolveName("disparity_in");
00140     MsgSaver< stereo_msgs::DisparityImage > 
00141       recent_disp(topic_disp);
00142     std::string topic_cam = root_nh_.resolveName("cam_info_in");
00143     MsgSaver< sensor_msgs::CameraInfo > 
00144       recent_cam_info(topic_cam);
00145 
00146     ros::Time start_time = ros::Time::now();
00147     ros::Duration time_out  = ros::Duration(40.0);
00148     
00149     ROS_INFO("plane detection service called; waiting for a disparity image on topic %s and for camera info on topic %s", topic_disp.c_str(), topic_cam.c_str());
00150     
00151     while ((!recent_disp.hasMsg() || 
00152             !recent_cam_info.hasMsg()) && 
00153            priv_nh_.ok()) {
00154      
00155       ROS_INFO("Entering the sensor assembly");
00156  
00157       ros::spinOnce();
00158       
00159       ros::Time current_time = ros::Time::now();
00160       if (time_out >= ros::Duration(0) && current_time - start_time >= time_out)
00161       {
00162         ROS_INFO("Timed out while waiting for sensor data");
00163         return false;
00164       }
00165       ros::Duration(1.0).sleep();
00166     }
00167 
00168     
00169     if (!priv_nh_.ok()) return false;;
00170     
00171     
00172     stereo_msgs::DisparityImage disparity = *recent_disp.getMsg();
00173     sensor_msgs::CameraInfo cam_info = *recent_cam_info.getMsg();
00174 
00175     TimerCPU timer_1(2800);
00176 
00177     // get parameters from disparity image
00178     float drange = disparity.max_disparity - 
00179       disparity.min_disparity;
00180     int w = disparity.image.width;
00181     int h = disparity.image.height;
00182     
00183     plane_detect_ = new PlaneDetection((int)drange, w, h, false, 
00184                                        disparity.image.header);
00185     
00186     plane_transform_ = new PlaneTransform( cam_info, 
00187                                            disparity.T, 
00188                                            up_direction_);
00189 
00190     // project 3D point to disparity image and mask it accordingly
00191     stereo_msgs::DisparityImage::ConstPtr disparity_ptr 
00192       = boost::make_shared<const stereo_msgs::DisparityImage> (disparity);
00193     sensor_msgs::CameraInfo::ConstPtr cam_info_ptr 
00194       = boost::make_shared<const sensor_msgs::CameraInfo> (cam_info);
00195 
00196     stereo_msgs::DisparityImage disparity_masked;
00197     disparity_masked.header = disparity.header;
00198     disparity_masked.image.header = disparity.image.header;
00199     disparity_masked.image.height = disparity.image.height;
00200     disparity_masked.image.width = disparity.image.width;
00201     disparity_masked.image.encoding = disparity.image.encoding;
00202     disparity_masked.image.is_bigendian = disparity.image.is_bigendian;
00203     //    int step = disparity.image.step/ disparity.image.width;
00204     //    ROS_INFO("Stepsize for new reduced size disparity image %d", step);
00205     disparity_masked.image.step = disparity.image.step;
00206     size_t size 
00207       = disparity_masked.image.step * disparity_masked.image.height;
00208     disparity_masked.image.data.resize(size, 0);
00209     
00210     disparity_masked.f = disparity.f;
00211     disparity_masked.T = disparity.T;
00212     disparity_masked.min_disparity = disparity.min_disparity;
00213     disparity_masked.max_disparity = disparity.max_disparity;
00214     disparity_masked.delta_d = disparity.delta_d;
00215     
00216     stereo_msgs::DisparityImage::Ptr disparity_masked_ptr 
00217       = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked);
00218 
00219     ROS_INFO_STREAM("Init Time: "  << timer_1.read() << " ms");
00220 
00221     TimerCPU timer_5(2800);
00222     if(!maskDisparityImage(req, disparity_ptr, cam_info_ptr, 
00223                            disparity_masked_ptr)) {     
00224       ROS_WARN("Could not project input point to disparity image");
00225       return false;
00226     }
00227     
00228     ROS_INFO_STREAM("Mask Time: "  << timer_5.read() << " ms");
00229     
00230     TimerCPU timer_2(2800);
00231     disparity_image_pub_.publish(*disparity_masked_ptr);
00232     ROS_INFO_STREAM("Publish Time: "  << timer_2.read() << " ms");
00233     
00234     TimerCPU timer_3(2800);
00235     plane_detect_->Update(disparity_masked_ptr);
00236     ROS_INFO_STREAM("Detect Time: "  << timer_3.read() << " ms");
00237 
00238     TimerCPU timer_6(2800);
00239     float alpha, beta, d;
00240     int min_x, max_x, min_y, max_y;
00241     plane_detect_->GetPlaneParameters( alpha, beta, d, 
00242                                        min_x, max_x, min_y, max_y);
00243     
00244     ROS_INFO("Plane parameters %f %f %f", alpha, beta, d);
00245 
00246     sensor_msgs::Image labels;
00247     plane_detect_->GetLabels(labels);
00248     
00249     int sum = 0;
00250     for(unsigned int i=0; i<labels.height; ++i)
00251       for(unsigned int j=0; j<labels.width; ++j){
00252         int adr = i*labels.width+j; 
00253         sum+=(labels.data[adr])/255;
00254       }
00255       
00256 
00257     // make inlier threshold dependent on region size
00258     int region_width  = req.width;
00259     int region_height = req.height;
00260     inlier_threshold_ = (region_width * region_height)/3;
00261 
00262     if(sum<inlier_threshold_){
00263       ROS_WARN("Plane has only %d inliers", sum);
00264       return false;
00265     }
00266     ROS_INFO_STREAM("Init Convert Time: "  << timer_6.read() << " ms");
00267           
00268     TimerCPU timer_4(2800);
00269     plane_transform_->setParameters( alpha, 
00270                                      beta, 
00271                                      d,
00272                                      min_x, max_x, 
00273                                      min_y, max_y, 
00274                                      disparity.image.header);
00275     
00276     if(!plane_transform_->getPlane(plane_)) {
00277       ROS_WARN("No Plane found");
00278       return false; 
00279     } else res.plane = plane_;
00280 
00281     ROS_INFO_STREAM("Convert Time: "  << timer_4.read() << " ms");
00282       
00283     return true;
00284   }
00285   
00286   bool 
00287   FastRegionPlaneService::maskDisparityImage( fast_plane_detection::PlaneInRegion::Request &req, const stereo_msgs::DisparityImage::ConstPtr& disparity, const sensor_msgs::CameraInfo::ConstPtr& camera_info, const stereo_msgs::DisparityImage::Ptr &masked_disparity)
00288   {
00289     // extract data from request
00290     geometry_msgs::Vector3Stamped point = req.point;
00291     int width  = req.width;
00292     int height = req.height;
00293 
00294     ROS_INFO("Searching for best plane around point (%f %f %f) in region of size %d %d", point.vector.x, point.vector.y, point.vector.z, width, height);
00295 
00296     float f = disparity->f;
00297     float T = disparity->T;
00298 
00299     float fx = camera_info->P[0*4+0]; 
00300     float fy = camera_info->P[1*4+1]; 
00301     float cx = camera_info->P[0*4+2]; 
00302     float cy = camera_info->P[1*4+2]; 
00303 
00304     if(point.header.frame_id!=disparity->header.frame_id){
00305       // transform point into same frame as disparity 
00306       tf::TransformListener listener;
00307       std::string error_msg;
00308     
00309       if (!listener.canTransform(point.header.frame_id, 
00310                                  disparity->header.frame_id, 
00311                                  disparity->header.stamp, &error_msg)) {
00312         ROS_ERROR("Cannot transform point cloud from frame %s to %s; error %s", 
00313                   point.header.frame_id.c_str(), 
00314                   disparity->header.frame_id.c_str(), error_msg.c_str());
00315         return false;
00316       } try {
00317         listener.transformVector(disparity->header.frame_id, 
00318                                  point, point);
00319       } catch (tf::TransformException ex) {
00320         ROS_ERROR("Failed to transform point cloud from frame %s into %s; error %s", 
00321                   disparity->header.frame_id.c_str(), 
00322                   disparity->header.frame_id.c_str(), ex.what());
00323         return false;
00324       }
00325     }
00326     point.header.frame_id = disparity->header.frame_id;
00327     
00328     // project point to image
00329     int u = point.vector.x/point.vector.z * fx + cx;
00330     int v = point.vector.y/point.vector.z * fy + cy;
00331 
00332     int d = (f*T)/point.vector.z;
00333 
00334 
00335     ROS_INFO("Pixel position of 3D point %d %d ", u, v);
00336      
00337     float *dimd_in  = (float*)&(disparity->image.data[0]);
00338     float *dimd_out = (float*)&(masked_disparity->image.data[0]);
00339     
00340     
00341     for (int y=0;y<(int)masked_disparity->image.height; y++) {
00342       for (int x=0;x< (int)masked_disparity->image.width; x++) {
00343         int adr = y*masked_disparity->image.width+x;
00344         if(x>u-width/2 && x<u+width/2 &&
00345            y>v-height/2 && y<v+height/2){
00346           dimd_out[adr] = dimd_in[adr];
00347         } else dimd_out[adr] = -1.0f;
00348       }
00349     }
00350     
00351     return true;
00352   }
00353 }
00354 
00355 int main(int argc, char **argv)
00356 {
00357   ros::init(argc, argv, "fast_plane_detection_server");
00358   ros::NodeHandle nh;
00359   
00360   fast_plane_detection::FastRegionPlaneService fast_region_plane_detector;
00361 
00362   ros::ServiceServer service 
00363     = nh.advertiseService("fast_plane_detection", 
00364                           &fast_plane_detection::FastRegionPlaneService::planeCallback, &fast_region_plane_detector);
00365   ROS_INFO("Ready to detect planes.");
00366   ros::spin();
00367 
00368   return 0;
00369 }


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