fast_plane_detector_server.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
 * ({bohg,celle}@csc.kth.se) 
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *  1.Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  2.Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.  
 *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
 *    endorse or promote products derived from this software without 
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <fast_plane_detection/Plane.h>
#include <fast_plane_detection/PlaneInRegion.h>

#include <fast_plane_detector/msg_saver.h>

#include <visualization_msgs/Marker.h>

#include "fast_plane_detector/plane_detection.h"
#include "fast_plane_detector/plane_transform.h"


#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>

#include "fast_plane_detector/timercpu.h"

namespace fast_plane_detection 
{
  
  class FastRegionPlaneService
  {
    
  private:
    
    bool 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);

    ros::NodeHandle root_nh_;
    ros::NodeHandle priv_nh_;
        
    PlaneDetection* plane_detect_;

    PlaneTransform* plane_transform_;

    double up_direction_;
    
    int inlier_threshold_;
    
    ros::Publisher disparity_image_pub_;

  public:
    
    FastRegionPlaneService();
    ~FastRegionPlaneService();
    
    //------------------ Callbacks -------------------

    bool planeCallback(fast_plane_detection::PlaneInRegion::Request &req,
                       fast_plane_detection::PlaneInRegion::Response &res );
    //------------------ The Data --------------------
    
    Plane plane_;
    
  };
  
  
  FastRegionPlaneService::FastRegionPlaneService()
    : root_nh_("")
    , priv_nh_("~")
  {
    
    priv_nh_.param<double>("up_direction", 
                           up_direction_, 
                           -1.0f);  
    priv_nh_.param<int>("inlier_threshold", 
                        inlier_threshold_, 
                        300);  

    // publish masked disparity images (for debug only)
    std::string topic = root_nh_.resolveName("/plane_detection/masked_disparity");
    disparity_image_pub_ 
      = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
  }
    
  FastRegionPlaneService::~FastRegionPlaneService()
  {}
  
    
  bool 
  FastRegionPlaneService::planeCallback(fast_plane_detection::PlaneInRegion::Request &req,
                                         fast_plane_detection::PlaneInRegion::Response &res )
    
  {

    ROS_INFO("Calling fast plane detection service");
        
    // get camera info and disparity from generic topics that have 
    // to be remapped in the launch file
    std::string topic_disp = root_nh_.resolveName("disparity_in");
    MsgSaver< stereo_msgs::DisparityImage > 
      recent_disp(topic_disp);
    std::string topic_cam = root_nh_.resolveName("cam_info_in");
    MsgSaver< sensor_msgs::CameraInfo > 
      recent_cam_info(topic_cam);

    ros::Time start_time = ros::Time::now();
    ros::Duration time_out  = ros::Duration(40.0);
    
    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());
    
    while ((!recent_disp.hasMsg() || 
            !recent_cam_info.hasMsg()) && 
           priv_nh_.ok()) {
     
      ROS_INFO("Entering the sensor assembly");
 
      ros::spinOnce();
      
      ros::Time current_time = ros::Time::now();
      if (time_out >= ros::Duration(0) && current_time - start_time >= time_out)
      {
        ROS_INFO("Timed out while waiting for sensor data");
        return false;
      }
      ros::Duration(1.0).sleep();
    }

    
    if (!priv_nh_.ok()) return false;;
    
    
    stereo_msgs::DisparityImage disparity = *recent_disp.getMsg();
    sensor_msgs::CameraInfo cam_info = *recent_cam_info.getMsg();

    TimerCPU timer_1(2800);

    // get parameters from disparity image
    float drange = disparity.max_disparity - 
      disparity.min_disparity;
    int w = disparity.image.width;
    int h = disparity.image.height;
    
    plane_detect_ = new PlaneDetection((int)drange, w, h, false, 
                                       disparity.image.header);
    
    plane_transform_ = new PlaneTransform( cam_info, 
                                           disparity.T, 
                                           up_direction_);

    // project 3D point to disparity image and mask it accordingly
    stereo_msgs::DisparityImage::ConstPtr disparity_ptr 
      = boost::make_shared<const stereo_msgs::DisparityImage> (disparity);
    sensor_msgs::CameraInfo::ConstPtr cam_info_ptr 
      = boost::make_shared<const sensor_msgs::CameraInfo> (cam_info);

    stereo_msgs::DisparityImage disparity_masked;
    disparity_masked.header = disparity.header;
    disparity_masked.image.header = disparity.image.header;
    disparity_masked.image.height = disparity.image.height;
    disparity_masked.image.width = disparity.image.width;
    disparity_masked.image.encoding = disparity.image.encoding;
    disparity_masked.image.is_bigendian = disparity.image.is_bigendian;
    //    int step = disparity.image.step/ disparity.image.width;
    //    ROS_INFO("Stepsize for new reduced size disparity image %d", step);
    disparity_masked.image.step = disparity.image.step;
    size_t size 
      = disparity_masked.image.step * disparity_masked.image.height;
    disparity_masked.image.data.resize(size, 0);
    
    disparity_masked.f = disparity.f;
    disparity_masked.T = disparity.T;
    disparity_masked.min_disparity = disparity.min_disparity;
    disparity_masked.max_disparity = disparity.max_disparity;
    disparity_masked.delta_d = disparity.delta_d;
    
    stereo_msgs::DisparityImage::Ptr disparity_masked_ptr 
      = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked);

    ROS_INFO_STREAM("Init Time: "  << timer_1.read() << " ms");

    TimerCPU timer_5(2800);
    if(!maskDisparityImage(req, disparity_ptr, cam_info_ptr, 
                           disparity_masked_ptr)) {     
      ROS_WARN("Could not project input point to disparity image");
      return false;
    }
    
    ROS_INFO_STREAM("Mask Time: "  << timer_5.read() << " ms");
    
    TimerCPU timer_2(2800);
    disparity_image_pub_.publish(*disparity_masked_ptr);
    ROS_INFO_STREAM("Publish Time: "  << timer_2.read() << " ms");
    
    TimerCPU timer_3(2800);
    plane_detect_->Update(disparity_masked_ptr);
    ROS_INFO_STREAM("Detect Time: "  << timer_3.read() << " ms");

    TimerCPU timer_6(2800);
    float alpha, beta, d;
    int min_x, max_x, min_y, max_y;
    plane_detect_->GetPlaneParameters( alpha, beta, d, 
                                       min_x, max_x, min_y, max_y);
    
    ROS_INFO("Plane parameters %f %f %f", alpha, beta, d);

    sensor_msgs::Image labels;
    plane_detect_->GetLabels(labels);
    
    int sum = 0;
    for(unsigned int i=0; i<labels.height; ++i)
      for(unsigned int j=0; j<labels.width; ++j){
        int adr = i*labels.width+j; 
        sum+=(labels.data[adr])/255;
      }
      

    // make inlier threshold dependent on region size
    int region_width  = req.width;
    int region_height = req.height;
    inlier_threshold_ = (region_width * region_height)/3;

    if(sum<inlier_threshold_){
      ROS_WARN("Plane has only %d inliers", sum);
      return false;
    }
    ROS_INFO_STREAM("Init Convert Time: "  << timer_6.read() << " ms");
          
    TimerCPU timer_4(2800);
    plane_transform_->setParameters( alpha, 
                                     beta, 
                                     d,
                                     min_x, max_x, 
                                     min_y, max_y, 
                                     disparity.image.header);
    
    if(!plane_transform_->getPlane(plane_)) {
      ROS_WARN("No Plane found");
      return false; 
    } else res.plane = plane_;

    ROS_INFO_STREAM("Convert Time: "  << timer_4.read() << " ms");
      
    return true;
  }
  
  bool 
  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)
  {
    // extract data from request
    geometry_msgs::Vector3Stamped point = req.point;
    int width  = req.width;
    int height = req.height;

    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);

    float f = disparity->f;
    float T = disparity->T;

    float fx = camera_info->P[0*4+0]; 
    float fy = camera_info->P[1*4+1]; 
    float cx = camera_info->P[0*4+2]; 
    float cy = camera_info->P[1*4+2]; 

    if(point.header.frame_id!=disparity->header.frame_id){
      // transform point into same frame as disparity 
      tf::TransformListener listener;
      std::string error_msg;
    
      if (!listener.canTransform(point.header.frame_id, 
                                 disparity->header.frame_id, 
                                 disparity->header.stamp, &error_msg)) {
        ROS_ERROR("Cannot transform point cloud from frame %s to %s; error %s", 
                  point.header.frame_id.c_str(), 
                  disparity->header.frame_id.c_str(), error_msg.c_str());
        return false;
      } try {
        listener.transformVector(disparity->header.frame_id, 
                                 point, point);
      } catch (tf::TransformException ex) {
        ROS_ERROR("Failed to transform point cloud from frame %s into %s; error %s", 
                  disparity->header.frame_id.c_str(), 
                  disparity->header.frame_id.c_str(), ex.what());
        return false;
      }
    }
    point.header.frame_id = disparity->header.frame_id;
    
    // project point to image
    int u = point.vector.x/point.vector.z * fx + cx;
    int v = point.vector.y/point.vector.z * fy + cy;

    int d = (f*T)/point.vector.z;


    ROS_INFO("Pixel position of 3D point %d %d ", u, v);
     
    float *dimd_in  = (float*)&(disparity->image.data[0]);
    float *dimd_out = (float*)&(masked_disparity->image.data[0]);
    
    
    for (int y=0;y<(int)masked_disparity->image.height; y++) {
      for (int x=0;x< (int)masked_disparity->image.width; x++) {
        int adr = y*masked_disparity->image.width+x;
        if(x>u-width/2 && x<u+width/2 &&
           y>v-height/2 && y<v+height/2){
          dimd_out[adr] = dimd_in[adr];
        } else dimd_out[adr] = -1.0f;
      }
    }
    
    return true;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "fast_plane_detection_server");
  ros::NodeHandle nh;
  
  fast_plane_detection::FastRegionPlaneService fast_region_plane_detector;

  ros::ServiceServer service 
    = nh.advertiseService("fast_plane_detection", 
                          &fast_plane_detection::FastRegionPlaneService::planeCallback, &fast_region_plane_detector);
  ROS_INFO("Ready to detect planes.");
  ros::spin();

  return 0;
}