$search
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 <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 // Service to set Position 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 //------------------ Callbacks ------------------- 00155 00157 void planeCallback(const stereo_msgs::DisparityImage::ConstPtr& 00158 disparity_image); 00159 00160 //------------------- Services -------------------- 00161 bool setPosition(fast_plane_detection::SetPosition::Request& req, 00162 fast_plane_detection::SetPosition::Response& resp); 00163 00164 //------------------ The Data -------------------- 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 //, X(-0.04) 00178 , Y(-0.04) 00179 //, Y(0) 00180 , Z(1) 00181 , transform_(false) 00182 , region_width_(20) 00183 , region_height_(20) 00184 , marker_id_(354345)//arbitrary 00185 { 00186 00187 // initialise operational flags 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 // THIS SHOULD BE INSTEAD OF BEING INITIALISED IN THE CONSTRUCTOR 00205 // BE CONSTANTLY FILLED BY A POSITION PUBLISHER THAT THIS NODE SUBSCRIBES TO 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 // retrieve camera info just once and one sample disparity 00218 // for parameter extraction 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 // Initialise the masked disparity_image 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 // int step = disparity_image_->image.step/ disparity_image_->image.width; 00243 // ROS_INFO("Stepsize for new reduced size disparity image %d", step); 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 // stereo_msgs::DisparityImage::Ptr disparity_masked_ptr 00256 // = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked_); 00257 00258 00259 // get parameters from disparity image 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 // subscribe to filtered disparity images 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 // publish plane markers 00285 plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("region_plane_markers", 1); 00286 00287 // publish plane msg 00288 plane_msg_pub_ = root_nh_.advertise<Plane>("region_plane_msg", 1); 00289 00290 // publish masked disparity images 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 // publish masked disparity images 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 // advertise a service to set Position 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 // masks the incoming disparity image based on 3D point and region size 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 // if(true) { 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 sensor_msgs::Image hist_y; 00391 float tmp_beta; 00392 float tmp_disp; 00393 plane_detect_y_->GetHistY(hist_y, tmp_beta, tmp_disp); 00394 00395 cv_bridge::CvImagePtr cv_line; 00396 sensor_msgs::Image::ConstPtr hist_ptr 00397 = boost::make_shared<sensor_msgs::Image>(hist_y); 00398 00399 try { 00400 00401 cv_line = cv_bridge::toCvCopy(hist_ptr, enc::MONO8); 00402 00403 } catch (cv_bridge::Exception& e) { 00404 00405 ROS_ERROR("cv_bridge exception: %s", e.what()); 00406 return; 00407 } 00408 00409 00410 float drange = disparity_image_->max_disparity - 00411 disparity_image_->min_disparity; 00412 00413 sensor_msgs::Image hist_rgb; 00414 hist_rgb.header = disparity_image->image.header; 00415 hist_rgb.width = drange; 00416 hist_rgb.height = disparity_image->image.height; 00417 hist_rgb.encoding = enc::RGB8; 00418 hist_rgb.is_bigendian = false; 00419 hist_rgb.step = hist_rgb.width * 3; 00420 size_t size = hist_rgb.step * hist_rgb.height; 00421 hist_rgb.data.resize(size, 0); 00422 cv_bridge::CvImagePtr cv_line_rgb; 00423 sensor_msgs::Image::ConstPtr hist_rgb_ptr 00424 = boost::make_shared<sensor_msgs::Image>(hist_rgb); 00425 00426 try { 00427 00428 cv_line_rgb = cv_bridge::toCvCopy(hist_rgb_ptr, enc::RGB8); 00429 00430 } catch (cv_bridge::Exception& e) { 00431 00432 ROS_ERROR("cv_bridge exception: %s", e.what()); 00433 return; 00434 } 00435 cvtColor(cv_line->image,cv_line_rgb->image,CV_GRAY2RGB); 00436 00437 int x = hist_rgb.height; 00438 int y = tmp_beta*x + tmp_disp; 00439 ROS_WARN("Y: tmp_beta %f, tmp_disp %f, y %d, drange %f", 00440 tmp_beta, tmp_disp, y, drange); 00441 00442 cv::line( cv_line_rgb->image, 00443 cv::Point(y, x), cv::Point(tmp_disp, 0),CV_RGB(0,0,255), 1); 00444 00445 hist_image_pub_.publish(cv_line_rgb->toImageMsg()); 00446 00447 00448 sensor_msgs::Image hist_x; 00449 plane_detect_y_->GetHistX(hist_x, tmp_beta, tmp_disp); 00450 sensor_msgs::Image::ConstPtr hist_ptr_x 00451 = boost::make_shared<sensor_msgs::Image>(hist_x); 00452 try { 00453 00454 cv_line = cv_bridge::toCvCopy(hist_ptr_x, enc::MONO8); 00455 00456 } catch (cv_bridge::Exception& e) { 00457 00458 ROS_ERROR("cv_bridge exception: %s", e.what()); 00459 return; 00460 } 00461 00462 00463 sensor_msgs::Image hist_rgb_x; 00464 hist_rgb_x.header = disparity_image->image.header; 00465 hist_rgb_x.width = drange; 00466 hist_rgb_x.height = disparity_image->image.width; 00467 hist_rgb_x.encoding = enc::RGB8; 00468 hist_rgb_x.is_bigendian = false; 00469 hist_rgb_x.step = hist_rgb_x.width * 3; 00470 size = hist_rgb_x.step * hist_rgb_x.height; 00471 hist_rgb_x.data.resize(size, 0); 00472 sensor_msgs::Image::ConstPtr hist_rgb_x_ptr 00473 = boost::make_shared<sensor_msgs::Image>(hist_rgb_x); 00474 00475 try { 00476 00477 cv_line_rgb = cv_bridge::toCvCopy(hist_rgb_x_ptr, enc::RGB8); 00478 00479 } catch (cv_bridge::Exception& e) { 00480 00481 ROS_ERROR("cv_bridge exception: %s", e.what()); 00482 return; 00483 } 00484 cvtColor(cv_line->image,cv_line_rgb->image,CV_GRAY2RGB); 00485 00486 x = hist_rgb_x.height; 00487 y = tmp_beta*x + tmp_disp; 00488 ROS_WARN("X: tmp_beta %f, tmp_disp %f, y %d, drange %f", 00489 tmp_beta, tmp_disp, y, drange); 00490 00491 cv::line( cv_line_rgb->image, 00492 cv::Point(y, x), cv::Point(tmp_disp, 0),CV_RGB(0,0,255), 1); 00493 00494 hist_image_x_pub_.publish(cv_line_rgb->toImageMsg()); 00495 */ 00496 00497 /* 00498 sensor_msgs::Image labels; 00499 plane_detect_y_->GetLabels(labels); 00500 labels_pub_.publish(labels); 00501 00502 int n_labels = 0; 00503 for(unsigned int i=0; i<labels.height; ++i) 00504 for(unsigned int j=0; j<labels.width; ++j){ 00505 int adr = i*labels.width+j; 00506 if(labels.data[adr]==255) 00507 n_labels++; 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 // make inlier threshold dependent on region size 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 // add confidence measures to plane message 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 // THIS SHOULD BE INSTEAD OF BEING INITIALISED IN THE CONSTRUCTOR 00590 // BE CONSTANTLY FILLED BY A POSITION PUBLISHER THAT THIS NODE SUBSCRIBES TO 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 // transform point into same frame as disparity 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 // project point to image 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 }