00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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 <cv_bridge/cv_bridge.h>
00046 #include <sensor_msgs/image_encodings.h>
00047 #include <opencv2/imgproc/imgproc.hpp>
00048 #include <opencv2/highgui/highgui.hpp>
00049
00050 #include "fast_plane_detector/plane_detection.h"
00051 #include "fast_plane_detector/plane_transform.h"
00052
00053 #include "fast_plane_detector/timercpu.h"
00054 #include "fast_plane_detector/utils.h"
00055
00056 #include <fast_plane_detection/SetPosition.h>
00057
00058 namespace enc = sensor_msgs::image_encodings;
00059
00060 #define PI 3.1415926535897932384626433832795
00061
00062 namespace fast_plane_detection {
00063
00064 class FastRegionPlaneDetector
00065 {
00066
00067 private:
00068
00069 bool maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity,
00070 const sensor_msgs::CameraInfo::ConstPtr& camera_info,
00071 const stereo_msgs::DisparityImage::Ptr &masked_disparity);
00073 ros::NodeHandle root_nh_;
00075 ros::NodeHandle priv_nh_;
00076
00078 ros::Subscriber camera_info_sub_;
00079 std::string camera_info_topic_;
00080
00082 ros::Subscriber disparity_image_sub_;
00083 std::string disparity_image_topic_;
00084
00086 int buffer_size_;
00087
00089 stereo_msgs::DisparityImage::ConstPtr disparity_image_;
00090
00092 stereo_msgs::DisparityImage disparity_masked_;
00093 stereo_msgs::DisparityImage::Ptr disparity_masked_ptr;
00094
00096 sensor_msgs::CameraInfo::ConstPtr camera_info_;
00097
00099 ros::Publisher plane_marker_pub_;
00100
00102 ros::Publisher plane_msg_pub_;
00103
00105 ros::Publisher disparity_image_pub_;
00106
00108 ros::Publisher hist_image_pub_;
00109 ros::Publisher hist_image_x_pub_;
00110
00112 ros::Publisher labels_pub_;
00113
00114 tf::TransformListener listener_;
00115
00117 PlaneDetection* plane_detect_y_;
00118
00120 PlaneDetection* plane_detect_x_;
00121
00123 bool find_table_;
00124
00126 PlaneTransform* plane_transform_;
00127
00129 double up_direction_;
00130
00132 int inlier_threshold_;
00133
00134
00135 ros::ServiceServer srv_position_;
00136 double X;
00137 double Y;
00138 double Z;
00139 bool transform_;
00140
00141
00143 geometry_msgs::Vector3Stamped point_;
00144 int region_width_;
00145 int region_height_;
00146
00147 int marker_id_;
00148
00149 public:
00150
00151 FastRegionPlaneDetector( int buffer_size );
00152 ~FastRegionPlaneDetector();
00153
00154
00155
00157 void planeCallback(const stereo_msgs::DisparityImage::ConstPtr&
00158 disparity_image);
00159
00160
00161 bool setPosition(fast_plane_detection::SetPosition::Request& req,
00162 fast_plane_detection::SetPosition::Response& resp);
00163
00164
00165
00167 Plane plane_;
00168
00169 };
00170
00171
00172 FastRegionPlaneDetector::FastRegionPlaneDetector( int buffer_size)
00173 : root_nh_("")
00174 , priv_nh_("~")
00175 , buffer_size_(buffer_size)
00176 , X(-0.1)
00177
00178 , Y(-0.04)
00179
00180 , Z(1)
00181 , transform_(false)
00182 , region_width_(20)
00183 , region_height_(20)
00184 , marker_id_(354345)
00185 {
00186
00187
00188 priv_nh_.param<std::string>("camera_info_topic",
00189 camera_info_topic_,
00190 "/narrow_stereo_textured/left/camera_info");
00191 priv_nh_.param<std::string>("disparity_image_topic",
00192 disparity_image_topic_,
00193 "/narrow_stereo_textured/disparity");
00194 priv_nh_.param<bool>("find_table",
00195 find_table_,
00196 false);
00197 priv_nh_.param<double>("up_direction",
00198 up_direction_,
00199 -1.0f);
00200 priv_nh_.param<int>("inlier_threshold",
00201 inlier_threshold_,
00202 300);
00203
00204
00205
00206
00207 if (transform_)
00208 point_.header.frame_id = "torso_lift_link";
00209 else
00210 point_.header.frame_id = "narrow_stereo_optical_frame";
00211 point_.header.stamp = ros::Time::now();
00212 point_.vector.x = 0.0;
00213 point_.vector.y = 0.0;
00214 point_.vector.z = 1.0;
00215
00216
00217
00218
00219 while (!camera_info_ || ! disparity_image_){
00220 if( !camera_info_ ){
00221 ROS_INFO_STREAM(" Waiting for message on topic "
00222 << camera_info_topic_);
00223 camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00224 (camera_info_topic_, root_nh_, ros::Duration(0.5));
00225 }
00226
00227 if( !disparity_image_){
00228 ROS_INFO_STREAM(" Waiting for message on topic "
00229 << disparity_image_topic_);
00230 disparity_image_ = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00231 (disparity_image_topic_, root_nh_, ros::Duration(1.0));
00232 }
00233 }
00234
00235
00236 disparity_masked_.header = disparity_image_->header;
00237 disparity_masked_.image.header = disparity_image_->image.header;
00238 disparity_masked_.image.height = disparity_image_->image.height;
00239 disparity_masked_.image.width = disparity_image_->image.width;
00240 disparity_masked_.image.encoding = disparity_image_->image.encoding;
00241 disparity_masked_.image.is_bigendian = disparity_image_->image.is_bigendian;
00242
00243
00244 disparity_masked_.image.step = disparity_image_->image.step;
00245 size_t size
00246 = disparity_masked_.image.step * disparity_masked_.image.height;
00247 disparity_masked_.image.data.resize(size, 0);
00248
00249 disparity_masked_.f = disparity_image_->f;
00250 disparity_masked_.T = disparity_image_->T;
00251 disparity_masked_.min_disparity = disparity_image_->min_disparity;
00252 disparity_masked_.max_disparity = disparity_image_->max_disparity;
00253 disparity_masked_.delta_d = disparity_image_->delta_d;
00254
00255
00256
00257
00258
00259
00260 float drange = disparity_image_->max_disparity -
00261 disparity_image_->min_disparity;
00262 int w = disparity_image_->image.width;
00263 int h = disparity_image_->image.height;
00264
00265 plane_detect_y_ = new PlaneDetection((int)drange, w, h, find_table_,
00266 disparity_image_->image.header);
00267
00268 plane_detect_x_ = new PlaneDetection((int)drange, w, h, find_table_,
00269 disparity_image_->image.header,
00270 false);
00271
00272 plane_transform_ = new PlaneTransform( *camera_info_,
00273 disparity_image_->T,
00274 up_direction_);
00275
00276
00277 std::string topic_disp = root_nh_.resolveName("disparity_in");
00278
00279 disparity_image_sub_ = root_nh_.subscribe(topic_disp,
00280 buffer_size_,
00281 &FastRegionPlaneDetector::planeCallback,
00282 this);
00283
00284
00285 plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("region_plane_markers", 1);
00286
00287
00288 plane_msg_pub_ = root_nh_.advertise<Plane>("region_plane_msg", 1);
00289
00290
00291 std::string topic = root_nh_.resolveName("/plane_detection/masked_disparity");
00292 disparity_image_pub_
00293 = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
00294
00295
00296 topic = root_nh_.resolveName("/plane_detection/hist_y");
00297 hist_image_pub_
00298 = root_nh_.advertise<sensor_msgs::Image>(topic, 1);
00299
00300 topic = root_nh_.resolveName("/plane_detection/hist_x");
00301 hist_image_x_pub_
00302 = root_nh_.advertise<sensor_msgs::Image>(topic, 1);
00303
00304 topic = root_nh_.resolveName("/plane_detection/labels");
00305 labels_pub_
00306 = root_nh_.advertise<sensor_msgs::Image>(topic, 1);
00307
00308
00309 srv_position_ = root_nh_.advertiseService("set_position", &FastRegionPlaneDetector::setPosition, this);
00310
00311 }
00312
00313 FastRegionPlaneDetector::~FastRegionPlaneDetector()
00314 {
00315 delete plane_detect_y_;
00316 delete plane_detect_x_;
00317 delete plane_transform_;
00318 }
00319
00320
00321 void FastRegionPlaneDetector::planeCallback(const stereo_msgs::DisparityImage::ConstPtr& disparity_image)
00322 {
00323 TimerCPU timer(2800);
00324
00325 stereo_msgs::DisparityImage::Ptr disparity_masked_ptr
00326 = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked_);
00327
00328
00329 if(!maskDisparityImage( disparity_image, camera_info_,
00330 disparity_masked_ptr)) {
00331 ROS_WARN("Could not project input point to disparity image");
00332 plane_.result = Plane::OTHER_ERROR;
00333 return;
00334 }
00335
00336 disparity_image_pub_.publish(*disparity_masked_ptr);
00337 plane_detect_y_->Update(disparity_masked_ptr);
00338
00339 float spread_y = plane_detect_y_->GetSpreadY();
00340
00341 float alpha_y, beta_y, d_y;
00342 int min_x_y, max_x_y, min_y_y, max_y_y;
00343 plane_detect_y_->GetPlaneParameters( alpha_y, beta_y, d_y,
00344 min_x_y, max_x_y,
00345 min_y_y, max_y_y);
00346
00347 float alpha_x, beta_x, d_x;
00348 int min_x_x, max_x_x, min_y_x, max_y_x;
00349 plane_detect_x_->Update(disparity_masked_ptr);
00350 float spread_x = plane_detect_x_->GetSpreadX();
00351 plane_detect_x_->GetPlaneParameters( alpha_x, beta_x, d_x,
00352 min_x_x, max_x_x,
00353 min_y_x, max_y_x);
00354
00355 ROS_INFO("Error values - Y: %f - X: %f", spread_y, spread_x);
00356 float alpha, beta, d;
00357 int min_x, max_x, min_y, max_y;
00358 float spread;
00359
00360 int n_inliers;
00361 int n_disps;
00362 float mean_error;
00363 if(spread_y>=spread_x) {
00364
00365 alpha = alpha_x;
00366 beta = beta_x;
00367 d = d_x;
00368 min_x = min_x_x;
00369 min_y = min_y_x;
00370 max_x = max_x_x;
00371 max_y = max_y_x;
00372 plane_detect_x_->GetNInliers(n_inliers);
00373 plane_detect_x_->GetNDisp(n_disps);
00374 mean_error = spread_x;
00375 } else {
00376 alpha = alpha_y;
00377 beta = beta_y;
00378 d = d_y;
00379 min_x = min_x_y;
00380 min_y = min_y_y;
00381 max_x = max_x_y;
00382 max_y = max_y_y;
00383 plane_detect_y_->GetNInliers(n_inliers);
00384 plane_detect_y_->GetNDisp(n_disps);
00385 mean_error = spread_y;
00386 }
00387
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511 int percentage_disp_inliers = 100 * (float)n_inliers/(float)n_disps;
00512 ROS_INFO("Number of inliers %d, Number of valid disparities %d, Ratio %d ", n_inliers, n_disps, percentage_disp_inliers);
00513
00514 float percentage_inliers =
00515 100.0f *n_inliers/(float)(region_width_ * region_height_);
00516
00517 ROS_INFO("Number of inliers %d, Region Size %d, Ratio %f ",
00518 n_inliers, region_width_ * region_height_,
00519 percentage_inliers);
00520
00521
00522 inlier_threshold_ = (region_width_ * region_height_)/3;
00523
00524 if(n_inliers<inlier_threshold_){
00525 ROS_WARN("Plane has only %d inliers", n_inliers);
00526 plane_.result = Plane::FEW_INLIERS;
00527 return;
00528 } else {
00529 plane_.result = Plane::SUCCESS;
00530 }
00531
00532 ROS_INFO("Mean Squared error of detected Plane: %f", mean_error );
00533
00534 plane_transform_->setParameters( alpha,
00535 beta,
00536 d,
00537 min_x, max_x,
00538 min_y, max_y,
00539 disparity_image->header);
00540
00541 if(!plane_transform_->getPlane(plane_)) {
00542 ROS_WARN("No Plane found");
00543 plane_.result = Plane::NO_PLANE;
00544 return;
00545 } else {
00546
00547 float a = plane_.normal.vector.x;
00548 float b = plane_.normal.vector.y;
00549 ROS_INFO("Angle %f", acos( -plane_.normal.vector.z)*PI/180.0f );
00550
00551
00552 plane_.error = mean_error;
00553 plane_.percentage_inliers = percentage_inliers;
00554 plane_.percentage_disp_inliers = percentage_disp_inliers;
00555 plane_.percentage_valid_disp = n_disps;
00556
00557 visualization_msgs::Marker delete_marker;
00558 delete_marker.header.stamp = ros::Time::now();
00559 delete_marker.header.frame_id = disparity_image->header.frame_id;
00560 delete_marker.id = marker_id_;
00561 delete_marker.action = visualization_msgs::Marker::DELETE;
00562 delete_marker.ns = "tabletop_node";
00563 plane_marker_pub_.publish(delete_marker);
00564
00565 visualization_msgs::Marker planeMarker
00566 = getPlaneMarker( plane_.top_left,
00567 plane_.top_right,
00568 plane_.bottom_left,
00569 plane_.bottom_right);
00570 planeMarker.header = disparity_image->header;
00571 planeMarker.pose = plane_.pose.pose;
00572 planeMarker.ns = "tabletop_node";
00573 planeMarker.id = marker_id_;
00574 plane_marker_pub_.publish(planeMarker);
00575
00576 }
00577
00578 ROS_INFO_STREAM("Time: " << timer.read() << " ms");
00579
00580 plane_msg_pub_.publish(plane_);
00581
00582 }
00583
00584
00585 bool
00586 FastRegionPlaneDetector::maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity, const sensor_msgs::CameraInfo::ConstPtr& camera_info, const stereo_msgs::DisparityImage::Ptr &masked_disparity)
00587
00588 {
00589
00590
00591 if (transform_)
00592 point_.header.frame_id = "torso_lift_link";
00593 else
00594 point_.header.frame_id = "narrow_stereo_optical_frame";
00595
00596 point_.header.stamp = disparity->header.stamp;
00597 point_.vector.x = X;
00598 point_.vector.y = Y;
00599 point_.vector.z = Z;
00600
00601 float f = disparity->f;
00602 float T = disparity->T;
00603
00604 float fx = camera_info->P[0*4+0];
00605 float fy = camera_info->P[1*4+1];
00606 float cx = camera_info->P[0*4+2];
00607 float cy = camera_info->P[1*4+2];
00608
00609 if(point_.header.frame_id!=disparity->header.frame_id){
00610
00611 std::string error_msg;
00612
00613 if (!listener_.canTransform(point_.header.frame_id,
00614 disparity->header.frame_id,
00615 disparity->header.stamp, &error_msg)) {
00616 ROS_ERROR("Cannot transform point cloud from frame %s to %s; error %s",
00617 point_.header.frame_id.c_str(),
00618 disparity->header.frame_id.c_str(), error_msg.c_str());
00619 return false;
00620 } try {
00621 listener_.transformVector(disparity->header.frame_id,
00622 point_, point_);
00623 } catch (tf::TransformException ex) {
00624 ROS_ERROR("Failed to transform point cloud from frame %s into %s; error %s",
00625 point_.header.frame_id.c_str(),
00626 disparity->header.frame_id.c_str(), ex.what());
00627 return false;
00628 }
00629 }
00630 point_.header.frame_id = disparity->header.frame_id;
00631
00632
00633 int u = point_.vector.x/point_.vector.z * fx + cx;
00634 int v = point_.vector.y/point_.vector.z * fy + cy;
00635
00636 int d = (f*T)/point_.vector.z;
00637
00638 ROS_INFO("Pixel position of 3D point %d %d ", u, v);
00639
00640 float *dimd_in = (float*)&(disparity->image.data[0]);
00641 float *dimd_out = (float*)&(masked_disparity->image.data[0]);
00642
00643 for (int y=0;y<(int)masked_disparity->image.height; y++) {
00644 for (int x=0;x< (int)masked_disparity->image.width; x++) {
00645 int adr = y*masked_disparity->image.width+x;
00646 if(x>u-region_width_/2 && x<u+region_width_/2 &&
00647 y>v-region_height_/2 && y<v+region_height_/2){
00648 dimd_out[adr] = dimd_in[adr];
00649 } else dimd_out[adr] = -1.0f;
00650 }
00651 }
00652
00653 return true;
00654 }
00655
00657 bool FastRegionPlaneDetector::setPosition(fast_plane_detection::SetPosition::Request& req,
00658 fast_plane_detection::SetPosition::Response& resp)
00659 {
00660 X = req.x;
00661 Y = req.y;
00662 Z = req.z;
00663 transform_ = req.transform;
00664 ROS_INFO("Fast Plane Detector: Position changed");
00665
00666 resp.x = X;
00667 resp.y = Y;
00668 resp.z = Z;
00669 resp.transform = transform_;
00670 return true;
00671 }
00672
00673 }
00674
00675
00676
00677
00678 int main(int argc, char **argv)
00679 {
00680 ros::init(argc, argv, "fast_plane_detection_node");
00681
00683 int buffer_size_ = 1;
00684 fast_plane_detection::FastRegionPlaneDetector node_(buffer_size_);
00685 ros::spin();
00686
00687 return 0;
00688 }