fast_plane_detection: fast_plane_detection: fast_region_plane

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 #include <tf/transform_listener.h>
00044 
00045 #include "fast_plane_detector/plane_detection.h"
00046 #include "fast_plane_detector/plane_transform.h"
00047 
00048 #include "fast_plane_detector/timercpu.h"
00049 #include "fast_plane_detector/utils.h"
00050 
00051 #include <fast_plane_detection/SetPosition.h>
00052 #include <fast_plane_detection/SlavePoint.h>
00053 //#include <haptic_fb_controller_plane/MsgDataPr2.h>
00054 
00055 namespace fast_plane_detection {
00056 
00057 class FastRegionPlaneDetector {
00058 
00059 private:
00060 
00061         bool maskDisparityImage(
00062                         const stereo_msgs::DisparityImage::ConstPtr& disparity,
00063                         const sensor_msgs::CameraInfo::ConstPtr& camera_info,
00064                         const stereo_msgs::DisparityImage::Ptr &masked_disparity);
00066         ros::NodeHandle root_nh_;
00068         ros::NodeHandle priv_nh_;
00069 
00071         ros::Subscriber camera_info_sub_;
00072         std::string camera_info_topic_;
00073 
00075         ros::Subscriber disparity_image_sub_;
00076         std::string disparity_image_topic_;
00077 
00079         int buffer_size_;
00080 
00082         stereo_msgs::DisparityImage::ConstPtr disparity_image_;
00083 
00085         stereo_msgs::DisparityImage disparity_masked_;
00086         stereo_msgs::DisparityImage::Ptr disparity_masked_ptr;
00087 
00089         sensor_msgs::CameraInfo::ConstPtr camera_info_;
00090 
00092         ros::Publisher plane_marker_pub_;
00093 
00095         ros::Publisher pos_marker_pub_;
00096 
00098         ros::Publisher plane_msg_pub_;
00099 
00101         ros::Publisher disparity_image_pub_;
00102 
00103         tf::TransformListener listener_;
00104         // From source to camera
00105         tf::TransformListener listener_check_;
00106         tf::StampedTransform transform_check_;
00107         // From camera to source
00108         tf::TransformListener listener_check2_;
00109         tf::StampedTransform transform_check2_;
00110 
00112         PlaneDetection* plane_detect_;
00113 
00115         bool find_table_;
00116 
00118         PlaneTransform* plane_transform_;
00119 
00121         double up_direction_;
00122 
00124         int inlier_threshold_;
00125         int number_inliers_;
00126         double percentage_inliers_;
00127         float mean_error_;
00128 
00129         // Service to set Position
00130         ros::ServiceServer srv_position_;
00131         double X_;
00132         double Y_;
00133         double Z_;
00134         bool manual_;
00135         bool in_contact_;
00136         // Subscriber to PR2 Position
00137         ros::Subscriber sub_position_;
00138 
00140         geometry_msgs::Vector3Stamped point_;
00141         int region_width_;
00142         int region_height_;
00143 
00144         int marker_id_;
00145         int marker2_id_;
00146 
00147         // What is the source and the target frame
00148         std::string source_frame_;
00149         std::string camera_frame_;
00150 
00151         // Colors
00152         std_msgs::ColorRGBA green_;
00153         std_msgs::ColorRGBA red_;
00154 
00155 public:
00156 
00157         FastRegionPlaneDetector(int buffer_size);
00158         ~FastRegionPlaneDetector();
00159 
00160         //------------------ Callbacks -------------------
00161 
00163         void planeCallback(
00164                         const stereo_msgs::DisparityImage::ConstPtr& disparity_image);
00165 
00166         //------------------- Services --------------------
00167         bool setPosition(fast_plane_detection::SetPosition::Request& req,
00168                         fast_plane_detection::SetPosition::Response& resp);
00169 
00170         //----------------- Subscribers -------------------
00171         void Pr2Callback(const fast_plane_detection::SlavePoint msg_pose);
00172 
00173         //------------------ The Data --------------------
00174 
00176         Plane plane_;
00177 
00178 };
00179 
00180 FastRegionPlaneDetector::FastRegionPlaneDetector(int buffer_size) 
00181         : root_nh_("")
00182         , priv_nh_("~")
00183         , X_(0)
00184         , Y_(0)
00185         , Z_(1)
00186         , number_inliers_(0)
00187         , percentage_inliers_(0)
00188         , buffer_size_(buffer_size)
00189         , region_width_(10)
00190         , region_height_(40)
00191         , manual_(false)
00192         , marker_id_(354345)//arbitrary
00193         , marker2_id_(154345)//arbitrary
00194         , source_frame_("/torso_lift_link")
00195         , camera_frame_("/narrow_stereo_optical_frame")
00196 {
00197   
00198         green_.r = 0, green_.g = 0.8, green_.b = 0, green_.a = 1;
00199         red_.r = 1, red_.g = 0, red_.b = 0, red_.a = 1;
00200 
00201         // initialise operational flags
00202         priv_nh_.param<std::string> ("camera_info_topic", camera_info_topic_,
00203                         "/narrow_stereo_textured/left/camera_info");
00204         priv_nh_.param<std::string> ("disparity_image_topic",
00205                         disparity_image_topic_, "/narrow_stereo_textured/disparity");
00206         priv_nh_.param<bool> ("find_table", find_table_, false);
00207         priv_nh_.param<double> ("up_direction", up_direction_, -1.0f);
00208         priv_nh_.param<int> ("inlier_threshold", inlier_threshold_, 300);
00209 
00210         // retrieve camera info just once and one sample disparity
00211         // for parameter extraction
00212         while (!camera_info_ || !disparity_image_) {
00213                 if (!camera_info_) {
00214                         ROS_INFO_STREAM("  Waiting for message on topic "
00215                                         << camera_info_topic_);
00216                         camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(
00217                                         camera_info_topic_, root_nh_, ros::Duration(0.5));
00218                 }
00219 
00220                 if (!disparity_image_) {
00221                         ROS_INFO_STREAM("  Waiting for message on topic "
00222                                         << disparity_image_topic_);
00223                         disparity_image_ = ros::topic::waitForMessage<
00224                                         stereo_msgs::DisparityImage>(disparity_image_topic_,
00225                                         root_nh_, ros::Duration(1.0));
00226                 }
00227         }
00228 
00229         // Initialise the masked disparity_image
00230         disparity_masked_.header = disparity_image_->header;
00231         disparity_masked_.image.header = disparity_image_->image.header;
00232         disparity_masked_.image.height = disparity_image_->image.height;
00233         disparity_masked_.image.width = disparity_image_->image.width;
00234         disparity_masked_.image.encoding = disparity_image_->image.encoding;
00235         disparity_masked_.image.is_bigendian = disparity_image_->image.is_bigendian;
00236         //    int step = disparity_image_->image.step/ disparity_image_->image.width;
00237         //    ROS_INFO("Stepsize for new reduced size disparity image %d", step);
00238         disparity_masked_.image.step = disparity_image_->image.step;
00239         size_t size = disparity_masked_.image.step * disparity_masked_.image.height;
00240         disparity_masked_.image.data.resize(size, 0);
00241 
00242         disparity_masked_.f = disparity_image_->f;
00243         disparity_masked_.T = disparity_image_->T;
00244         disparity_masked_.min_disparity = disparity_image_->min_disparity;
00245         disparity_masked_.max_disparity = disparity_image_->max_disparity;
00246         disparity_masked_.delta_d = disparity_image_->delta_d;
00247 
00248         // stereo_msgs::DisparityImage::Ptr disparity_masked_ptr
00249         //   = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked_);
00250 
00251 
00252         // get parameters from disparity image
00253         float drange = disparity_image_->max_disparity
00254                         - disparity_image_->min_disparity;
00255         int w = disparity_image_->image.width;
00256         int h = disparity_image_->image.height;
00257 
00258         plane_detect_ = new PlaneDetection((int) drange, w, h, find_table_,
00259                         disparity_image_->image.header);
00260 
00261         plane_transform_ = new PlaneTransform(*camera_info_, disparity_image_->T,
00262                         up_direction_);
00263 
00264         // subscribe to filtered disparity images
00265         std::string topic_disp = root_nh_.resolveName("disparity_in");
00266         disparity_image_sub_ = root_nh_.subscribe(topic_disp, buffer_size_,
00267                         &FastRegionPlaneDetector::planeCallback, this);
00268 
00269         // publish plane markers
00270         plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker> (
00271                         "region_plane_markers", 1);
00272 
00273         // publish slave location
00274         pos_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker> (
00275                         "slave_pos_marker", 1);
00276 
00277         // publish plane msg
00278         plane_msg_pub_ = root_nh_.advertise<Plane> ("region_plane_msg", 1);
00279 
00280         // publish masked disparity images
00281         std::string topic = root_nh_.resolveName(
00282                         "/plane_detection/masked_disparity");
00283         disparity_image_pub_ = root_nh_.advertise<stereo_msgs::DisparityImage> (
00284                         topic, 1);
00285 
00286         // advertise a service to set Position
00287         srv_position_ = root_nh_.advertiseService("set_position", &FastRegionPlaneDetector::setPosition, this);
00288 
00289         // subscriber to Slave Position
00290         sub_position_ = root_nh_.subscribe("/bilateral_teleop/slave_point", 1, &FastRegionPlaneDetector::Pr2Callback,this);
00291 
00292 }
00293 
00294 FastRegionPlaneDetector::~FastRegionPlaneDetector() {
00295         delete plane_detect_;
00296         delete plane_transform_;
00297 }
00298 
00299 void FastRegionPlaneDetector::planeCallback(
00300                 const stereo_msgs::DisparityImage::ConstPtr& disparity_image) {
00301         ROS_INFO("-----------------");
00302         TimerCPU timer(2800);
00303 
00304         stereo_msgs::DisparityImage::Ptr disparity_masked_ptr = boost::make_shared<
00305                         stereo_msgs::DisparityImage>(disparity_masked_);
00306 
00307         // masks the incoming disparity image based on 3D point and region size
00308         if (!maskDisparityImage(disparity_image, camera_info_, disparity_masked_ptr)) {
00309                 ROS_WARN("Could not project input point to disparity image");
00310                 plane_.result = Plane::OTHER_ERROR;
00311                 return;
00312         }
00313 
00314         disparity_image_pub_.publish(*disparity_masked_ptr);
00315         plane_detect_->Update(disparity_masked_ptr);
00316 
00317         float alpha, beta, d;
00318         int min_x, max_x, min_y, max_y;
00319         plane_detect_->GetPlaneParameters(alpha, beta, d,
00320                                           min_x, max_x, min_y,
00321                                           max_y);
00322 
00323         sensor_msgs::Image labels;
00324         plane_detect_->GetLabels(labels);
00325 
00326 
00327         // Check number of inliers
00328         int sum = 0;
00329         for (unsigned int i = 0; i < labels.height; ++i)
00330                 for (unsigned int j = 0; j < labels.width; ++j) {
00331                         int adr = i * labels.width + j;
00332                         sum += (labels.data[adr]) / 255;
00333                 }
00334         number_inliers_ = sum;
00335 
00336     // Number of inliers
00337     //plane_detect_->GetNInliers(number_inliers_);
00338     // Mean squared error
00339     plane_detect_->GetMeanSquaredError(mean_error_);
00340     
00341     // make inlier threshold dependent on region size
00342     inlier_threshold_ = (region_width_ * region_height_) / 3;
00343     percentage_inliers_ = 
00344       100*number_inliers_/(region_width_ * region_height_);
00345     
00346     plane_transform_->setParameters(alpha, beta, d, 
00347                                     min_x, max_x, min_y, max_y,
00348                                     disparity_image->header);
00349     
00350     if (!plane_transform_->getPlane(plane_)) {
00351       ROS_WARN("No Plane found");
00352       plane_.result = Plane::NO_PLANE;
00353       plane_msg_pub_.publish(plane_);
00354       return;
00355     } else {
00356       
00357       // What is the current transform  from camera to source
00358       try {
00359         listener_check2_.lookupTransform(source_frame_,
00360                                          camera_frame_, 
00361                                          ros::Time(0), 
00362                                          transform_check2_);
00363       } catch (tf::TransformException ex) {
00364         ROS_ERROR("%s", ex.what());
00365       }
00366       //  Plane Point (transformed to source frame)
00367       btVector3 point1(plane_.pose.pose.position.x, 
00368                        plane_.pose.pose.position.y, 
00369                        plane_.pose.pose.position.z);
00370       point1 = transform_check2_.getBasis() * point1 + 
00371         transform_check2_.getOrigin();
00372       geometry_msgs::PointStamped plane_point;
00373       plane_point.header.frame_id = "torso_lift_link";
00374       plane_point.header.stamp = ros::Time::now();
00375       plane_point.point.x = point1.x();
00376       plane_point.point.y = point1.y();
00377       plane_point.point.z = point1.z();
00378       plane_.plane_point = plane_point;
00379       // Plane normal (transformed to source frame)
00380       btVector3 point2(plane_.normal.vector.x, 
00381                        plane_.normal.vector.y, 
00382                        plane_.normal.vector.z);
00383       point2 = transform_check2_.getBasis() * point2;
00384       ROS_INFO("Normal of plane:  %f %f %f", 
00385                plane_.normal.vector.x, plane_.normal.vector.y, 
00386                plane_.normal.vector.z);
00387       geometry_msgs::Vector3Stamped plane_normal;
00388       plane_normal.header.frame_id = "torso_lift_link";
00389       plane_normal.header.stamp = ros::Time::now();
00390       plane_normal.vector.x = point2.x();
00391       plane_normal.vector.y = point2.y();
00392       plane_normal.vector.z = point2.z();
00393       plane_.normal = plane_normal;
00394       // Slave point (defined in source frame)
00395       geometry_msgs::PointStamped slave_point;
00396       slave_point.header.frame_id = "torso_lift_link";
00397       slave_point.header.stamp = ros::Time::now();
00398       slave_point.point.x = X_;
00399       slave_point.point.y = Y_;
00400       slave_point.point.z = Z_;
00401       plane_.slave_point = slave_point;
00402       // Confidence parameter
00403       if(number_inliers_<inlier_threshold_){
00404         ROS_WARN("Percentage of inliers is only %f", percentage_inliers_);
00405         plane_.result = Plane::FEW_INLIERS;
00406       }
00407       else{
00408         ROS_INFO("Percentage of inliers is %f", percentage_inliers_);
00409         plane_.result = Plane::SUCCESS;
00410       }
00411       ROS_INFO("Mean squared error is %f", mean_error_);
00412       plane_.percentage_inliers = percentage_inliers_;
00413       plane_.error = mean_error_;
00414       plane_msg_pub_.publish(plane_);
00415       
00416       visualization_msgs::Marker delete_marker;
00417       delete_marker.header.stamp = ros::Time::now();
00418       delete_marker.header.frame_id = disparity_image->header.frame_id;
00419       delete_marker.id = marker_id_;
00420       delete_marker.action = visualization_msgs::Marker::DELETE;
00421       delete_marker.ns = "tabletop_node";
00422       plane_marker_pub_.publish(delete_marker);
00423       
00424       visualization_msgs::Marker planeMarker 
00425         = getPlaneMarker(
00426                          plane_.top_left, plane_.top_right, 
00427                          plane_.bottom_left,
00428                          plane_.bottom_right);
00429       planeMarker.header = disparity_image->header;
00430       planeMarker.pose = plane_.pose.pose;
00431       planeMarker.ns = "tabletop_node";
00432       planeMarker.id = marker_id_;
00433       if(in_contact_)
00434         planeMarker.color = red_;
00435       else
00436         planeMarker.color = green_;
00437       planeMarker.scale.x = 0.002;
00438       plane_marker_pub_.publish(planeMarker);
00439       
00440     }
00441     
00442     
00443     //ROS_INFO_STREAM("Time: " << timer.read() << " ms");
00444     
00445 }
00446 
00447 bool FastRegionPlaneDetector::maskDisparityImage(
00448                 const stereo_msgs::DisparityImage::ConstPtr& disparity,
00449                 const sensor_msgs::CameraInfo::ConstPtr& camera_info,
00450                 const stereo_msgs::DisparityImage::Ptr &masked_disparity)
00451 
00452 {
00453         // What is the current transform  from source to camera
00454         try {
00455                 listener_check_.lookupTransform(camera_frame_, source_frame_, ros::Time(0), transform_check_);
00456         } catch (tf::TransformException ex) {
00457                 ROS_ERROR("%s", ex.what());
00458         }
00459 
00460         btVector3 point(X_,Y_,Z_);
00461 
00462         point = transform_check_ * point;
00463 
00465         visualization_msgs::Marker delete_posMarker;
00466         delete_posMarker.header.stamp = ros::Time::now();
00467         delete_posMarker.header.frame_id = camera_frame_;
00468         delete_posMarker.id = marker2_id_;
00469         delete_posMarker.action = visualization_msgs::Marker::DELETE;
00470         delete_posMarker.ns = "tabletop_node";
00471         pos_marker_pub_.publish(delete_posMarker);
00472 
00473         visualization_msgs::Marker posMarker;
00474         posMarker.header.frame_id = camera_frame_;
00475         posMarker.header.stamp = ros::Time::now();
00476         posMarker.ns = "tabletop_node";
00477         posMarker.id = marker2_id_;
00478         posMarker.type = visualization_msgs::Marker::SPHERE;
00479         //posMarker.action = visualization_msgs::Marker::ADD;
00480         // Set full 6DOF pose relative to the frame/time specified in the header
00481         posMarker.pose.position.x = point.x();
00482         posMarker.pose.position.y = point.y();
00483         posMarker.pose.position.z = point.z();
00484         // Set the scale of the posMarker -- 1x1x1 here means 1m on a side
00485         posMarker.scale.x = 0.01;
00486         posMarker.scale.y = 0.01;
00487         posMarker.scale.z = 0.01;
00488         // Set the color -- be sure to set alpha to something non-zero!
00489         if(in_contact_)
00490                 posMarker.color = red_;
00491         else
00492                 posMarker.color = green_;
00493         pos_marker_pub_.publish(posMarker);
00494 
00495 
00496         // Start processing
00497         float f = disparity->f;
00498         float T = disparity->T;
00499 
00500         float fx = camera_info->P[0 * 4 + 0];
00501         float fy = camera_info->P[1 * 4 + 1];
00502         float cx = camera_info->P[0 * 4 + 2];
00503         float cy = camera_info->P[1 * 4 + 2];
00504 
00505         // project point to image
00506         int u = point.x() / point.z() * fx + cx;
00507         int v = point.y() / point.z() * fy + cy;
00508 
00509         int d = (f * T) / point.z();
00510 
00511         ROS_INFO("3D point: (%f, %f, %f) -> Pixel position (%d, %d) ",X_, Y_,Z_, u, v);
00512 
00513         float *dimd_in = (float*) &(disparity->image.data[0]);
00514         float *dimd_out = (float*) &(masked_disparity->image.data[0]);
00515 
00516         for (int y = 0; y < (int) masked_disparity->image.height; y++) {
00517                 for (int x = 0; x < (int) masked_disparity->image.width; x++) {
00518                         int adr = y * masked_disparity->image.width + x;
00519                         if (x > u - region_width_ / 2 && x < u + region_width_ / 2 && y > v
00520                                         - region_height_ / 2 && y < v + region_height_ / 2) {
00521                                 dimd_out[adr] = dimd_in[adr];
00522                         } else
00523                                 dimd_out[adr] = -1.0f;
00524                 }
00525         }
00526 
00527         return true;
00528 }
00529 
00531 bool FastRegionPlaneDetector::setPosition(
00532                 fast_plane_detection::SetPosition::Request& req,
00533                 fast_plane_detection::SetPosition::Response& resp) {
00534         if (req.transform){
00535                 X_ = req.x;
00536                 Y_ = req.y;
00537                 Z_ = req.z;
00538         }
00539         region_width_ = req.width;
00540         region_height_ = req.height;
00541         manual_ = req.transform;
00542         ROS_INFO("Fast Plane Detector: Position changed");
00543 
00544         resp.x = X_;
00545         resp.y = Y_;
00546         resp.z = Z_;
00547         resp.transform = manual_;
00548         resp.width = region_width_;
00549         resp.height = region_height_;
00550         return true;
00551 }
00552 
00554 void FastRegionPlaneDetector::Pr2Callback(const fast_plane_detection::SlavePoint msg_pose)
00555 {
00556         if (!manual_){
00557                 X_ = msg_pose.end_effector.x;
00558                 Y_ = msg_pose.end_effector.y;
00559                 Z_ = msg_pose.end_effector.z;
00560         }
00561         in_contact_ = msg_pose.contact;
00562 
00563 
00564 }
00565 
00566 }
00567 
00568 int main(int argc, char **argv) {
00569         ros::init(argc, argv, "fast_plane_detection_node");
00570 
00572         int buffer_size_ = 1;
00573         fast_plane_detection::FastRegionPlaneDetector node_(buffer_size_);
00574         ros::spin();
00575 
00576         return 0;
00577 }
fast_region_plane_detector.cpp
