fast_region_plane_detector_fixedCam.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 <visualization_msgs/Marker.h>

#include <tf/transform_listener.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

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

#include "fast_plane_detector/timercpu.h"
#include "fast_plane_detector/utils.h"

#include <fast_plane_detection/SetPosition.h>

namespace enc = sensor_msgs::image_encodings;

#define PI 3.1415926535897932384626433832795

namespace fast_plane_detection {
  
  class FastRegionPlaneDetector
  {
    
  private:
    
    bool maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity,
                             const stereo_msgs::DisparityImage::Ptr &masked_disparity);
    ros::NodeHandle root_nh_;
    ros::NodeHandle priv_nh_;
    
    ros::Subscriber disparity_image_sub_;
    std::string disparity_image_topic_;

    int buffer_size_;
    
    stereo_msgs::DisparityImage::ConstPtr disparity_image_;

    stereo_msgs::DisparityImage disparity_masked_;
    stereo_msgs::DisparityImage::Ptr disparity_masked_ptr;

    ros::Publisher plane_marker_pub_;

    ros::Publisher plane_msg_pub_;

    ros::Publisher disparity_image_pub_;

    ros::Publisher hist_image_pub_;
    ros::Publisher hist_image_x_pub_;

    ros::Publisher labels_pub_;

    tf::TransformListener listener_;

    PlaneDetection* plane_detect_y_;
    
    PlaneDetection* plane_detect_x_;

    bool find_table_;
  
    PlaneTransform* plane_transform_;

    double up_direction_;
    
    int inlier_threshold_;

    // Service to set Position
    ros::ServiceServer srv_position_;
    double X;
    double Y;
    double Z;
    bool transform_;


    geometry_msgs::Vector3Stamped point_;
    int region_width_;
    int region_height_;

    int marker_id_;

      float fx_;
      float fy_;
      float cx_;
      float cy_;

  public:
    
    FastRegionPlaneDetector( int buffer_size );
    ~FastRegionPlaneDetector();
    
    //------------------ Callbacks -------------------

    void planeCallback(const stereo_msgs::DisparityImage::ConstPtr& 
                       disparity_image);

    //------------------- Services --------------------
    bool setPosition(fast_plane_detection::SetPosition::Request& req,
                     fast_plane_detection::SetPosition::Response& resp);

    //------------------ The Data --------------------

    Plane plane_;
    
  };
  
  
  FastRegionPlaneDetector::FastRegionPlaneDetector( int buffer_size)
    : root_nh_("")
    , priv_nh_("~")
    , buffer_size_(buffer_size)
    , X(-0.11f)
    , Y(0.01f)
      //, X(-0.1)
      //, X(-0.04)
      // , Y(-0.04)
      //, Y(0)
    , Z(1)
    , transform_(false)
    , region_width_(20)
    , region_height_(20)
    , marker_id_(354345)//arbitrary
    , fx_(937.91)
    , fy_(937.91)
    , cx_(298.41)
    , cy_(256.18)
  {
    
    // initialise operational flags
    priv_nh_.param<std::string>("disparity_image_topic", 
                                disparity_image_topic_, 
                                "/narrow_stereo_textured/disparity");
    priv_nh_.param<bool>("find_table", 
                         find_table_, 
                         false);  
    priv_nh_.param<double>("up_direction", 
                           up_direction_, 
                           -1.0f);  
    priv_nh_.param<int>("inlier_threshold", 
                        inlier_threshold_, 
                        300);  
    
    // THIS SHOULD BE INSTEAD OF BEING INITIALISED IN THE CONSTRUCTOR 
    // BE CONSTANTLY FILLED BY A POSITION PUBLISHER THAT THIS NODE SUBSCRIBES TO 
    
    if (transform_)
      point_.header.frame_id = "torso_lift_link";
    else
      point_.header.frame_id = "narrow_stereo_optical_frame";
    point_.header.stamp = ros::Time::now();
    point_.vector.x = 0.0;
    point_.vector.y = 0.0;
    point_.vector.z = 1.0;
    

    // retrieve camera info just once and one sample disparity 
    // for parameter extraction
    while (! disparity_image_){
        ROS_INFO_STREAM("  Waiting for message on topic " 
                        << disparity_image_topic_);
        disparity_image_ = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
            (disparity_image_topic_, root_nh_, ros::Duration(1.0));
    }

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

    
    // get parameters from disparity image
    float drange = disparity_image_->max_disparity - 
      disparity_image_->min_disparity;
    int w = disparity_image_->image.width;
    int h = disparity_image_->image.height;
    
    plane_detect_y_ = new PlaneDetection((int)drange, w, h, find_table_, 
                                       disparity_image_->image.header);

    plane_detect_x_ = new PlaneDetection((int)drange, w, h, find_table_, 
                                         disparity_image_->image.header, 
                                         false);

    plane_transform_ = new PlaneTransform( fx_, fy_, cx_, cy_,
                                           disparity_image_->T, 
                                           up_direction_);

    // subscribe to filtered disparity images
    std::string topic_disp = root_nh_.resolveName("disparity_in");

    disparity_image_sub_ = root_nh_.subscribe(topic_disp, 
                                              buffer_size_, 
                                              &FastRegionPlaneDetector::planeCallback,
                                              this);
    
    // publish plane markers
    plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("region_plane_markers", 1); 
    
    // publish plane msg
    plane_msg_pub_ = root_nh_.advertise<Plane>("region_plane_msg", 1);

    // publish masked disparity images
    std::string topic = root_nh_.resolveName("/plane_detection/masked_disparity");
    disparity_image_pub_ 
      = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);

    // publish masked disparity images
    topic = root_nh_.resolveName("/plane_detection/hist_y");
    hist_image_pub_ 
      = root_nh_.advertise<sensor_msgs::Image>(topic, 1);

    topic = root_nh_.resolveName("/plane_detection/hist_x");
    hist_image_x_pub_ 
      = root_nh_.advertise<sensor_msgs::Image>(topic, 1);

    topic = root_nh_.resolveName("/plane_detection/labels");
    labels_pub_ 
      = root_nh_.advertise<sensor_msgs::Image>(topic, 1);
    
    // advertise a service to set Position
    srv_position_ = root_nh_.advertiseService("set_position", &FastRegionPlaneDetector::setPosition, this);
    
  }

  FastRegionPlaneDetector::~FastRegionPlaneDetector()
  {
    delete plane_detect_y_;
    delete plane_detect_x_;
    delete plane_transform_;
  }


  void FastRegionPlaneDetector::planeCallback(const stereo_msgs::DisparityImage::ConstPtr& disparity_image) 
  {
    TimerCPU timer(2800);
    
    stereo_msgs::DisparityImage::Ptr disparity_masked_ptr 
      = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked_);

    // masks the incoming disparity image based on 3D point and region size
    if(!maskDisparityImage( disparity_image, 
                            disparity_masked_ptr)) {    
      ROS_WARN("Could not project input point to disparity image");
      plane_.result = Plane::OTHER_ERROR;
      return;
    }

    disparity_image_pub_.publish(*disparity_masked_ptr);
    plane_detect_y_->Update(disparity_masked_ptr);
    
    float spread_y = plane_detect_y_->GetSpreadY();

    float alpha_y, beta_y, d_y;
    int min_x_y, max_x_y, min_y_y, max_y_y;
    plane_detect_y_->GetPlaneParameters( alpha_y, beta_y, d_y, 
                                         min_x_y, max_x_y, 
                                         min_y_y, max_y_y);
 
    float alpha_x, beta_x, d_x;
    int min_x_x, max_x_x, min_y_x, max_y_x;
    plane_detect_x_->Update(disparity_masked_ptr);
    float spread_x = plane_detect_x_->GetSpreadX();
    plane_detect_x_->GetPlaneParameters( alpha_x, beta_x, d_x, 
                                         min_x_x, max_x_x, 
                                         min_y_x, max_y_x);
    
    ROS_INFO("Error values - Y: %f - X: %f", spread_y, spread_x);
    float alpha, beta, d;
    int min_x, max_x, min_y, max_y;
    float spread;
    
    int n_inliers;
    int n_disps;
    float mean_error;
    if(spread_y>=spread_x) {
    //     if(true) {
      alpha = alpha_x;
      beta = beta_x;
      d = d_x;
      min_x = min_x_x;
      min_y = min_y_x;
      max_x = max_x_x;
      max_y = max_y_x;
      plane_detect_x_->GetNInliers(n_inliers);
      plane_detect_x_->GetNDisp(n_disps);
      mean_error = spread_x;
    } else {
      alpha = alpha_y;
      beta = beta_y;
      d = d_y;
      min_x = min_x_y;
      min_y = min_y_y;
      max_x = max_x_y;
      max_y = max_y_y;
      plane_detect_y_->GetNInliers(n_inliers);
      plane_detect_y_->GetNDisp(n_disps);
      mean_error = spread_y;
    }

    /*
    sensor_msgs::Image hist_y;
    float tmp_beta;
    float tmp_disp;
    plane_detect_y_->GetHistY(hist_y, tmp_beta, tmp_disp);

    cv_bridge::CvImagePtr cv_line;
    sensor_msgs::Image::ConstPtr hist_ptr 
      = boost::make_shared<sensor_msgs::Image>(hist_y);
    
    try {
      
      cv_line = cv_bridge::toCvCopy(hist_ptr, enc::MONO8);
      
    } catch (cv_bridge::Exception& e) {
      
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    
    float drange = disparity_image_->max_disparity - 
      disparity_image_->min_disparity;
    
    sensor_msgs::Image hist_rgb;
    hist_rgb.header = disparity_image->image.header;
    hist_rgb.width  = drange;
    hist_rgb.height = disparity_image->image.height;
    hist_rgb.encoding = enc::RGB8;
    hist_rgb.is_bigendian = false;
    hist_rgb.step = hist_rgb.width * 3;
    size_t size = hist_rgb.step * hist_rgb.height;
    hist_rgb.data.resize(size, 0);
    cv_bridge::CvImagePtr cv_line_rgb;
    sensor_msgs::Image::ConstPtr hist_rgb_ptr 
      = boost::make_shared<sensor_msgs::Image>(hist_rgb);
    
    try {
      
      cv_line_rgb = cv_bridge::toCvCopy(hist_rgb_ptr, enc::RGB8);
      
    } catch (cv_bridge::Exception& e) {
      
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cvtColor(cv_line->image,cv_line_rgb->image,CV_GRAY2RGB);

    int x = hist_rgb.height;
    int y = tmp_beta*x + tmp_disp;
    ROS_WARN("Y: tmp_beta %f, tmp_disp %f, y %d, drange %f", 
             tmp_beta, tmp_disp, y, drange);
    
    cv::line( cv_line_rgb->image, 
              cv::Point(y, x), cv::Point(tmp_disp, 0),CV_RGB(0,0,255), 1);
    
    hist_image_pub_.publish(cv_line_rgb->toImageMsg());


    sensor_msgs::Image hist_x;
    plane_detect_y_->GetHistX(hist_x, tmp_beta, tmp_disp);
    sensor_msgs::Image::ConstPtr hist_ptr_x 
      = boost::make_shared<sensor_msgs::Image>(hist_x);
    try {
      
      cv_line = cv_bridge::toCvCopy(hist_ptr_x, enc::MONO8);
      
    } catch (cv_bridge::Exception& e) {
      
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    
    sensor_msgs::Image hist_rgb_x;
    hist_rgb_x.header = disparity_image->image.header;
    hist_rgb_x.width  = drange;
    hist_rgb_x.height = disparity_image->image.width;
    hist_rgb_x.encoding = enc::RGB8;
    hist_rgb_x.is_bigendian = false;
    hist_rgb_x.step = hist_rgb_x.width * 3;
    size = hist_rgb_x.step * hist_rgb_x.height;
    hist_rgb_x.data.resize(size, 0);
    sensor_msgs::Image::ConstPtr hist_rgb_x_ptr 
      = boost::make_shared<sensor_msgs::Image>(hist_rgb_x);
    
    try {
      
      cv_line_rgb = cv_bridge::toCvCopy(hist_rgb_x_ptr, enc::RGB8);
      
    } catch (cv_bridge::Exception& e) {
      
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cvtColor(cv_line->image,cv_line_rgb->image,CV_GRAY2RGB);

    x = hist_rgb_x.height;
    y = tmp_beta*x + tmp_disp;
    ROS_WARN("X: tmp_beta %f, tmp_disp %f, y %d, drange %f", 
             tmp_beta, tmp_disp, y, drange);
    
    cv::line( cv_line_rgb->image, 
              cv::Point(y, x), cv::Point(tmp_disp, 0),CV_RGB(0,0,255), 1);
    
    hist_image_x_pub_.publish(cv_line_rgb->toImageMsg());
    */
    
    
    sensor_msgs::Image labels;
    plane_detect_y_->GetLabels(labels);
    labels_pub_.publish(labels);
    
    int n_labels = 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;
        if(labels.data[adr]==255)
          n_labels++;
      }
    
    
    int percentage_disp_inliers = 100 * (float)n_inliers/(float)n_disps;
    ROS_INFO("Number of inliers %d, Number of valid disparities %d, Ratio %d ", n_inliers, n_disps, percentage_disp_inliers);

    float percentage_inliers = 
      100.0f *n_inliers/(float)(region_width_ * region_height_);

    ROS_INFO("Number of inliers %d, Region Size %d, Ratio %f ", 
             n_inliers, region_width_ * region_height_, 
             percentage_inliers);

    // make inlier threshold dependent on region size
    inlier_threshold_ = (region_width_ * region_height_)/3;

    if(n_inliers<inlier_threshold_){
      ROS_WARN("Plane has only %d inliers", n_inliers);
      plane_.result = Plane::FEW_INLIERS;
      return; 
    } else {
      plane_.result = Plane::SUCCESS;
    }

    ROS_INFO("Mean Squared error of detected Plane: %f", mean_error );

    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");
      plane_.result = Plane::NO_PLANE;
      return; 
    } else {

      float a = plane_.normal.vector.x;
      float b = plane_.normal.vector.y;
      ROS_INFO("Angle %f", acos( -plane_.normal.vector.z)*PI/180.0f );

      // add confidence measures to plane message
      plane_.error = mean_error;
      plane_.percentage_inliers = percentage_inliers;
      plane_.percentage_disp_inliers = percentage_disp_inliers;
      plane_.percentage_valid_disp = n_disps;

      visualization_msgs::Marker delete_marker;
      delete_marker.header.stamp = ros::Time::now();
      delete_marker.header.frame_id = disparity_image->header.frame_id;
      delete_marker.id = marker_id_;
      delete_marker.action = visualization_msgs::Marker::DELETE;
      delete_marker.ns = "tabletop_node";
      plane_marker_pub_.publish(delete_marker);
      
      visualization_msgs::Marker planeMarker 
        = getPlaneMarker( plane_.top_left, 
                          plane_.top_right,
                          plane_.bottom_left, 
                          plane_.bottom_right);
      planeMarker.header = disparity_image->header;
      planeMarker.pose = plane_.pose.pose;
      planeMarker.ns = "tabletop_node";
      planeMarker.id = marker_id_; 
      plane_marker_pub_.publish(planeMarker);
      
    }
    
    ROS_INFO_STREAM("Time: "  << timer.read() << " ms");
    
    plane_msg_pub_.publish(plane_);

  }
  
  
  bool 
  FastRegionPlaneDetector::maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity, const stereo_msgs::DisparityImage::Ptr &masked_disparity)
    
  {
    // THIS SHOULD BE INSTEAD OF BEING INITIALISED IN THE CONSTRUCTOR
    // BE CONSTANTLY FILLED BY A POSITION PUBLISHER THAT THIS NODE SUBSCRIBES TO
    if (transform_)
      point_.header.frame_id = "torso_lift_link";
    else
      point_.header.frame_id = "narrow_stereo_optical_frame";
    
    point_.header.stamp = disparity->header.stamp;
    point_.vector.x = X;
    point_.vector.y = Y;
    point_.vector.z = Z;
    
    float f = disparity->f;
    float T = disparity->T;
    
    if(point_.header.frame_id!=disparity->header.frame_id){
      // transform point into same frame as disparity 
      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", 
                  point_.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-region_width_/2 && x<u+region_width_/2 &&
             y>v-region_height_/2 && y<v+region_height_/2){
          dimd_out[adr] = dimd_in[adr];
        } else dimd_out[adr] = -1.0f;
      }
    }
    
    return true;
  }
  
  bool FastRegionPlaneDetector::setPosition(fast_plane_detection::SetPosition::Request& req,
                                            fast_plane_detection::SetPosition::Response& resp)
  {
    X = req.x;
    Y = req.y;
    Z = req.z;
    transform_ = req.transform;
    ROS_INFO("Fast Plane Detector: Position changed");
    
    resp.x = X;
    resp.y = Y;
    resp.z = Z;
    resp.transform = transform_;
    return true;
  }
  
}




int main(int argc, char **argv)
{
  ros::init(argc, argv, "fast_plane_detection_node");
  
  int buffer_size_ = 1;
  fast_plane_detection::FastRegionPlaneDetector node_(buffer_size_);
  ros::spin();
  
  return 0;
}