fast_region_plane_detector.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 "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>
#include <fast_plane_detection/SlavePoint.h>
//#include <haptic_fb_controller_plane/MsgDataPr2.h>

namespace fast_plane_detection {

class FastRegionPlaneDetector {

private:

        bool maskDisparityImage(
                        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_;

        ros::Subscriber camera_info_sub_;
        std::string camera_info_topic_;

        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;

        sensor_msgs::CameraInfo::ConstPtr camera_info_;

        ros::Publisher plane_marker_pub_;

        ros::Publisher pos_marker_pub_;

        ros::Publisher plane_msg_pub_;

        ros::Publisher disparity_image_pub_;

        tf::TransformListener listener_;
        // From source to camera
        tf::TransformListener listener_check_;
        tf::StampedTransform transform_check_;
        // From camera to source
        tf::TransformListener listener_check2_;
        tf::StampedTransform transform_check2_;

        PlaneDetection* plane_detect_;

        bool find_table_;

        PlaneTransform* plane_transform_;

        double up_direction_;

        int inlier_threshold_;
        int number_inliers_;
        double percentage_inliers_;
        float mean_error_;

        // Service to set Position
        ros::ServiceServer srv_position_;
        double X_;
        double Y_;
        double Z_;
        bool manual_;
        bool in_contact_;
        // Subscriber to PR2 Position
        ros::Subscriber sub_position_;

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

        int marker_id_;
        int marker2_id_;

        // What is the source and the target frame
        std::string source_frame_;
        std::string camera_frame_;

        // Colors
        std_msgs::ColorRGBA green_;
        std_msgs::ColorRGBA red_;

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

        //----------------- Subscribers -------------------
        void Pr2Callback(const fast_plane_detection::SlavePoint msg_pose);

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

        Plane plane_;

};

FastRegionPlaneDetector::FastRegionPlaneDetector(int buffer_size) 
        : root_nh_("")
        , priv_nh_("~")
        , X_(0)
        , Y_(0)
        , Z_(1)
        , number_inliers_(0)
        , percentage_inliers_(0)
        , buffer_size_(buffer_size)
        , region_width_(10)
        , region_height_(40)
        , manual_(false)
        , marker_id_(354345)//arbitrary
        , marker2_id_(154345)//arbitrary
        , source_frame_("/torso_lift_link")
        , camera_frame_("/narrow_stereo_optical_frame")
{
  
        green_.r = 0, green_.g = 0.8, green_.b = 0, green_.a = 1;
        red_.r = 1, red_.g = 0, red_.b = 0, red_.a = 1;

        // initialise operational flags
        priv_nh_.param<std::string> ("camera_info_topic", camera_info_topic_,
                        "/narrow_stereo_textured/left/camera_info");
        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);

        // retrieve camera info just once and one sample disparity
        // for parameter extraction
        while (!camera_info_ || !disparity_image_) {
                if (!camera_info_) {
                        ROS_INFO_STREAM("  Waiting for message on topic "
                                        << camera_info_topic_);
                        camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(
                                        camera_info_topic_, root_nh_, ros::Duration(0.5));
                }

                if (!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_ = new PlaneDetection((int) drange, w, h, find_table_,
                        disparity_image_->image.header);

        plane_transform_ = new PlaneTransform(*camera_info_, 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 slave location
        pos_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker> (
                        "slave_pos_marker", 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);

        // advertise a service to set Position
        srv_position_ = root_nh_.advertiseService("set_position", &FastRegionPlaneDetector::setPosition, this);

        // subscriber to Slave Position
        sub_position_ = root_nh_.subscribe("/bilateral_teleop/slave_point", 1, &FastRegionPlaneDetector::Pr2Callback,this);

}

FastRegionPlaneDetector::~FastRegionPlaneDetector() {
        delete plane_detect_;
        delete plane_transform_;
}

void FastRegionPlaneDetector::planeCallback(
                const stereo_msgs::DisparityImage::ConstPtr& disparity_image) {
        ROS_INFO("-----------------");
        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, camera_info_, 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_->Update(disparity_masked_ptr);

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

        sensor_msgs::Image labels;
        plane_detect_->GetLabels(labels);


        // Check number of inliers
        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;
                }
        number_inliers_ = sum;

    // Number of inliers
    //plane_detect_->GetNInliers(number_inliers_);
    // Mean squared error
    plane_detect_->GetMeanSquaredError(mean_error_);
    
    // make inlier threshold dependent on region size
    inlier_threshold_ = (region_width_ * region_height_) / 3;
    percentage_inliers_ = 
      100*number_inliers_/(region_width_ * region_height_);
    
    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;
      plane_msg_pub_.publish(plane_);
      return;
    } else {
      
      // What is the current transform  from camera to source
      try {
        listener_check2_.lookupTransform(source_frame_,
                                         camera_frame_, 
                                         ros::Time(0), 
                                         transform_check2_);
      } catch (tf::TransformException ex) {
        ROS_ERROR("%s", ex.what());
      }
      //  Plane Point (transformed to source frame)
      tf::Vector3 point1(plane_.pose.pose.position.x, 
                       plane_.pose.pose.position.y, 
                       plane_.pose.pose.position.z);
      point1 = transform_check2_.getBasis() * point1 + 
        transform_check2_.getOrigin();
      geometry_msgs::PointStamped plane_point;
      plane_point.header.frame_id = "torso_lift_link";
      plane_point.header.stamp = ros::Time::now();
      plane_point.point.x = point1.x();
      plane_point.point.y = point1.y();
      plane_point.point.z = point1.z();
      plane_.plane_point = plane_point;
      // Plane normal (transformed to source frame)
      tf::Vector3 point2(plane_.normal.vector.x, 
                       plane_.normal.vector.y, 
                       plane_.normal.vector.z);
      point2 = transform_check2_.getBasis() * point2;
      ROS_INFO("Normal of plane:  %f %f %f", 
               plane_.normal.vector.x, plane_.normal.vector.y, 
               plane_.normal.vector.z);
      geometry_msgs::Vector3Stamped plane_normal;
      plane_normal.header.frame_id = "torso_lift_link";
      plane_normal.header.stamp = ros::Time::now();
      plane_normal.vector.x = point2.x();
      plane_normal.vector.y = point2.y();
      plane_normal.vector.z = point2.z();
      plane_.normal = plane_normal;
      // Slave point (defined in source frame)
      geometry_msgs::PointStamped slave_point;
      slave_point.header.frame_id = "torso_lift_link";
      slave_point.header.stamp = ros::Time::now();
      slave_point.point.x = X_;
      slave_point.point.y = Y_;
      slave_point.point.z = Z_;
      plane_.slave_point = slave_point;
      // Confidence parameter
      if(number_inliers_<inlier_threshold_){
        ROS_WARN("Percentage of inliers is only %f", percentage_inliers_);
        plane_.result = Plane::FEW_INLIERS;
      }
      else{
        ROS_INFO("Percentage of inliers is %f", percentage_inliers_);
        plane_.result = Plane::SUCCESS;
      }
      ROS_INFO("Mean squared error is %f", mean_error_);
      plane_.percentage_inliers = percentage_inliers_;
      plane_.error = mean_error_;
      plane_msg_pub_.publish(plane_);
      
      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_;
      if(in_contact_)
        planeMarker.color = red_;
      else
        planeMarker.color = green_;
      planeMarker.scale.x = 0.002;
      plane_marker_pub_.publish(planeMarker);
      
    }
    
    
    //ROS_INFO_STREAM("Time: " << timer.read() << " ms");
    
}

bool FastRegionPlaneDetector::maskDisparityImage(
                const stereo_msgs::DisparityImage::ConstPtr& disparity,
                const sensor_msgs::CameraInfo::ConstPtr& camera_info,
                const stereo_msgs::DisparityImage::Ptr &masked_disparity)

{
        // What is the current transform  from source to camera
        try {
                listener_check_.lookupTransform(camera_frame_, source_frame_, ros::Time(0), transform_check_);
        } catch (tf::TransformException ex) {
                ROS_ERROR("%s", ex.what());
        }

        tf::Vector3 point(X_,Y_,Z_);

        point = transform_check_ * point;

        visualization_msgs::Marker delete_posMarker;
        delete_posMarker.header.stamp = ros::Time::now();
        delete_posMarker.header.frame_id = camera_frame_;
        delete_posMarker.id = marker2_id_;
        delete_posMarker.action = visualization_msgs::Marker::DELETE;
        delete_posMarker.ns = "tabletop_node";
        pos_marker_pub_.publish(delete_posMarker);

        visualization_msgs::Marker posMarker;
        posMarker.header.frame_id = camera_frame_;
        posMarker.header.stamp = ros::Time::now();
        posMarker.ns = "tabletop_node";
        posMarker.id = marker2_id_;
        posMarker.type = visualization_msgs::Marker::SPHERE;
        //posMarker.action = visualization_msgs::Marker::ADD;
        // Set full 6DOF pose relative to the frame/time specified in the header
        posMarker.pose.position.x = point.x();
        posMarker.pose.position.y = point.y();
        posMarker.pose.position.z = point.z();
        // Set the scale of the posMarker -- 1x1x1 here means 1m on a side
        posMarker.scale.x = 0.01;
        posMarker.scale.y = 0.01;
        posMarker.scale.z = 0.01;
        // Set the color -- be sure to set alpha to something non-zero!
        if(in_contact_)
                posMarker.color = red_;
        else
                posMarker.color = green_;
        pos_marker_pub_.publish(posMarker);


        // Start processing
        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];

        // project point to image
        int u = point.x() / point.z() * fx + cx;
        int v = point.y() / point.z() * fy + cy;

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

        ROS_INFO("3D point: (%f, %f, %f) -> Pixel position (%d, %d) ",X_, Y_,Z_, 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) {
        if (req.transform){
                X_ = req.x;
                Y_ = req.y;
                Z_ = req.z;
        }
        region_width_ = req.width;
        region_height_ = req.height;
        manual_ = req.transform;
        ROS_INFO("Fast Plane Detector: Position changed");

        resp.x = X_;
        resp.y = Y_;
        resp.z = Z_;
        resp.transform = manual_;
        resp.width = region_width_;
        resp.height = region_height_;
        return true;
}

void FastRegionPlaneDetector::Pr2Callback(const fast_plane_detection::SlavePoint msg_pose)
{
        if (!manual_){
                X_ = msg_pose.end_effector.x;
                Y_ = msg_pose.end_effector.y;
                Z_ = msg_pose.end_effector.z;
        }
        in_contact_ = msg_pose.contact;


}

}

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