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 "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
00053 namespace fast_plane_detection {
00054
00055 class FastRegionPlaneDetector
00056 {
00057
00058 private:
00059
00060 bool maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity,
00061 const sensor_msgs::CameraInfo::ConstPtr& camera_info,
00062 const stereo_msgs::DisparityImage::Ptr &masked_disparity);
00064 ros::NodeHandle root_nh_;
00066 ros::NodeHandle priv_nh_;
00067
00069 ros::Subscriber camera_info_sub_;
00070 std::string camera_info_topic_;
00071
00073 ros::Subscriber disparity_image_sub_;
00074 std::string disparity_image_topic_;
00075
00077 int buffer_size_;
00078
00080 stereo_msgs::DisparityImage::ConstPtr disparity_image_;
00081
00083 stereo_msgs::DisparityImage disparity_masked_;
00084 stereo_msgs::DisparityImage::Ptr disparity_masked_ptr;
00085
00087 sensor_msgs::CameraInfo::ConstPtr camera_info_;
00088
00090 ros::Publisher plane_marker_pub_;
00091
00093 ros::Publisher plane_msg_pub_;
00094
00096 ros::Publisher disparity_image_pub_;
00097
00098 tf::TransformListener listener_;
00099
00101 PlaneDetection* plane_detect_;
00102
00104 bool find_table_;
00105
00107 PlaneTransform* plane_transform_;
00108
00110 double up_direction_;
00111
00113 int inlier_threshold_;
00114
00115
00116 ros::ServiceServer srv_position_;
00117 double X;
00118 double Y;
00119 double Z;
00120 bool transform_;
00121
00122
00124 geometry_msgs::Vector3Stamped point_;
00125 int region_width_;
00126 int region_height_;
00127
00128 int marker_id_;
00129
00130 public:
00131
00132 FastRegionPlaneDetector( int buffer_size );
00133 ~FastRegionPlaneDetector();
00134
00135
00136
00138 void planeCallback(const stereo_msgs::DisparityImage::ConstPtr&
00139 disparity_image);
00140
00141
00142 bool setPosition(fast_plane_detection::SetPosition::Request& req,
00143 fast_plane_detection::SetPosition::Response& resp);
00144
00145
00146
00148 Plane plane_;
00149
00150 };
00151
00152
00153 FastRegionPlaneDetector::FastRegionPlaneDetector( int buffer_size)
00154 : root_nh_("")
00155 , priv_nh_("~"), X(0), Y(0), Z(1), transform_(false)
00156 , buffer_size_(buffer_size)
00157 , region_width_(10)
00158 , region_height_(40)
00159 , marker_id_(354345)
00160 {
00161
00162
00163 priv_nh_.param<std::string>("camera_info_topic",
00164 camera_info_topic_,
00165 "/narrow_stereo_textured/left/camera_info");
00166 priv_nh_.param<std::string>("disparity_image_topic",
00167 disparity_image_topic_,
00168 "/narrow_stereo_textured/disparity");
00169 priv_nh_.param<bool>("find_table",
00170 find_table_,
00171 false);
00172 priv_nh_.param<double>("up_direction",
00173 up_direction_,
00174 -1.0f);
00175 priv_nh_.param<int>("inlier_threshold",
00176 inlier_threshold_,
00177 300);
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192 while (!camera_info_ || ! disparity_image_){
00193 if( !camera_info_ ){
00194 ROS_INFO_STREAM(" Waiting for message on topic "
00195 << camera_info_topic_);
00196 camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
00197 (camera_info_topic_, root_nh_, ros::Duration(0.5));
00198 }
00199
00200 if( !disparity_image_){
00201 ROS_INFO_STREAM(" Waiting for message on topic "
00202 << disparity_image_topic_);
00203 disparity_image_ = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
00204 (disparity_image_topic_, root_nh_, ros::Duration(1.0));
00205 }
00206 }
00207
00208
00209 disparity_masked_.header = disparity_image_->header;
00210 disparity_masked_.image.header = disparity_image_->image.header;
00211 disparity_masked_.image.height = disparity_image_->image.height;
00212 disparity_masked_.image.width = disparity_image_->image.width;
00213 disparity_masked_.image.encoding = disparity_image_->image.encoding;
00214 disparity_masked_.image.is_bigendian = disparity_image_->image.is_bigendian;
00215
00216
00217 disparity_masked_.image.step = disparity_image_->image.step;
00218 size_t size
00219 = disparity_masked_.image.step * disparity_masked_.image.height;
00220 disparity_masked_.image.data.resize(size, 0);
00221
00222 disparity_masked_.f = disparity_image_->f;
00223 disparity_masked_.T = disparity_image_->T;
00224 disparity_masked_.min_disparity = disparity_image_->min_disparity;
00225 disparity_masked_.max_disparity = disparity_image_->max_disparity;
00226 disparity_masked_.delta_d = disparity_image_->delta_d;
00227
00228
00229
00230
00231
00232
00233 float drange = disparity_image_->max_disparity -
00234 disparity_image_->min_disparity;
00235 int w = disparity_image_->image.width;
00236 int h = disparity_image_->image.height;
00237
00238 plane_detect_ = new PlaneDetection((int)drange, w, h, find_table_,
00239 disparity_image_->image.header);
00240
00241 plane_transform_ = new PlaneTransform( *camera_info_,
00242 disparity_image_->T,
00243 up_direction_);
00244
00245
00246 std::string topic_disp = root_nh_.resolveName("disparity_in");
00247 disparity_image_sub_ = root_nh_.subscribe(topic_disp,
00248 buffer_size_,
00249 &FastRegionPlaneDetector::planeCallback,
00250 this);
00251
00252
00253 plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("region_plane_markers", 1);
00254
00255
00256 plane_msg_pub_ = root_nh_.advertise<Plane>("region_plane_msg", 1);
00257
00258
00259 std::string topic = root_nh_.resolveName("/plane_detection/masked_disparity");
00260 disparity_image_pub_
00261 = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
00262
00263
00264 srv_position_ = root_nh_.advertiseService("set_position", &FastRegionPlaneDetector::setPosition, this);
00265
00266 }
00267
00268 FastRegionPlaneDetector::~FastRegionPlaneDetector()
00269 {
00270 delete plane_detect_;
00271 delete plane_transform_;
00272 }
00273
00274
00275 void FastRegionPlaneDetector::planeCallback(const stereo_msgs::DisparityImage::ConstPtr& disparity_image)
00276 {
00277 TimerCPU timer(2800);
00278
00279 stereo_msgs::DisparityImage::Ptr disparity_masked_ptr
00280 = boost::make_shared<stereo_msgs::DisparityImage> (disparity_masked_);
00281
00282
00283 if(!maskDisparityImage( disparity_image, camera_info_,
00284 disparity_masked_ptr)) {
00285 ROS_WARN("Could not project input point to disparity image");
00286 plane_.result = Plane::OTHER_ERROR;
00287 return;
00288 }
00289
00290 disparity_image_pub_.publish(*disparity_masked_ptr);
00291 plane_detect_->Update(disparity_masked_ptr);
00292
00293 float alpha, beta, d;
00294 int min_x, max_x, min_y, max_y;
00295 plane_detect_->GetPlaneParameters( alpha, beta, d,
00296 min_x, max_x, min_y, max_y);
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 int n_inliers;
00312 plane_detect_->GetNInliers(n_inliers);
00313
00314
00315 inlier_threshold_ = (region_width_ * region_height_)/3;
00316
00317 if(n_inliers<inlier_threshold_){
00318 ROS_WARN("Plane has only %d inliers", n_inliers);
00319 plane_.result = Plane::NO_PLANE;
00320 return;
00321 }
00322
00323 float mean_error;
00324 plane_detect_->GetMeanSquaredError(mean_error);
00325
00326 ROS_INFO("Mean Squared error of detected Plane: %f", mean_error );
00327
00328 plane_transform_->setParameters( alpha,
00329 beta,
00330 d,
00331 min_x, max_x,
00332 min_y, max_y,
00333 disparity_image->header);
00334
00335 if(!plane_transform_->getPlane(plane_)) {
00336 ROS_WARN("No Plane found");
00337 plane_.result = Plane::NO_PLANE;
00338 return;
00339 } else {
00340
00341
00342 plane_.error = mean_error;
00343
00344 visualization_msgs::Marker delete_marker;
00345 delete_marker.header.stamp = ros::Time::now();
00346 delete_marker.header.frame_id = disparity_image->header.frame_id;
00347 delete_marker.id = marker_id_;
00348 delete_marker.action = visualization_msgs::Marker::DELETE;
00349 delete_marker.ns = "tabletop_node";
00350 plane_marker_pub_.publish(delete_marker);
00351
00352 visualization_msgs::Marker planeMarker
00353 = getPlaneMarker( plane_.top_left,
00354 plane_.top_right,
00355 plane_.bottom_left,
00356 plane_.bottom_right);
00357 planeMarker.header = disparity_image->header;
00358 planeMarker.pose = plane_.pose.pose;
00359 planeMarker.ns = "tabletop_node";
00360 planeMarker.id = marker_id_;
00361 plane_marker_pub_.publish(planeMarker);
00362
00363 }
00364
00365 ROS_INFO_STREAM("Time: " << timer.read() << " ms");
00366
00367 }
00368
00369
00370 bool
00371 FastRegionPlaneDetector::maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr& disparity, const sensor_msgs::CameraInfo::ConstPtr& camera_info, const stereo_msgs::DisparityImage::Ptr &masked_disparity)
00372
00373 {
00374
00375
00376 if (transform_)
00377 point_.header.frame_id = "torso_lift_link";
00378 else
00379 point_.header.frame_id = "narrow_stereo_optical_frame";
00380 point_.header.stamp = disparity->header.stamp;
00381 point_.vector.x = 0.7 + X;
00382 point_.vector.y = Y;
00383 point_.vector.z = -0.1 + Z;
00384
00385 float f = disparity->f;
00386 float T = disparity->T;
00387
00388 float fx = camera_info->P[0*4+0];
00389 float fy = camera_info->P[1*4+1];
00390 float cx = camera_info->P[0*4+2];
00391 float cy = camera_info->P[1*4+2];
00392
00393 if(point_.header.frame_id!=disparity->header.frame_id){
00394
00395 std::string error_msg;
00396
00397 if (!listener_.canTransform(point_.header.frame_id,
00398 disparity->header.frame_id,
00399 disparity->header.stamp, &error_msg)) {
00400 ROS_ERROR("Cannot transform point cloud from frame %s to %s; error %s",
00401 point_.header.frame_id.c_str(),
00402 disparity->header.frame_id.c_str(), error_msg.c_str());
00403 return false;
00404 } try {
00405 listener_.transformVector(disparity->header.frame_id,
00406 point_, point_);
00407 } catch (tf::TransformException ex) {
00408 ROS_ERROR("Failed to transform point cloud from frame %s into %s; error %s",
00409 point_.header.frame_id.c_str(),
00410 disparity->header.frame_id.c_str(), ex.what());
00411 return false;
00412 }
00413 }
00414 point_.header.frame_id = disparity->header.frame_id;
00415
00416
00417 int u = point_.vector.x/point_.vector.z * fx + cx;
00418 int v = point_.vector.y/point_.vector.z * fy + cy;
00419
00420 int d = (f*T)/point_.vector.z;
00421
00422
00423 ROS_INFO("Pixel position of 3D point %d %d ", u, v);
00424
00425 float *dimd_in = (float*)&(disparity->image.data[0]);
00426 float *dimd_out = (float*)&(masked_disparity->image.data[0]);
00427
00428
00429 for (int y=0;y<(int)masked_disparity->image.height; y++) {
00430 for (int x=0;x< (int)masked_disparity->image.width; x++) {
00431 int adr = y*masked_disparity->image.width+x;
00432 if(x>u-region_width_/2 && x<u+region_width_/2 &&
00433 y>v-region_height_/2 && y<v+region_height_/2){
00434 dimd_out[adr] = dimd_in[adr];
00435 } else dimd_out[adr] = -1.0f;
00436 }
00437 }
00438
00439 return true;
00440 }
00441
00443 bool FastRegionPlaneDetector::setPosition(fast_plane_detection::SetPosition::Request& req,
00444 fast_plane_detection::SetPosition::Response& resp)
00445 {
00446 X = req.x;
00447 Y = req.y;
00448 Z = req.z;
00449 transform_ = req.transform;
00450 ROS_INFO("Fast Plane Detector: Position changed");
00451
00452 resp.x = X;
00453 resp.y = Y;
00454 resp.z = Z;
00455 resp.transform = transform_;
00456 return true;
00457 }
00458
00459 }
00460
00461
00462
00463
00464 int main(int argc, char **argv)
00465 {
00466 ros::init(argc, argv, "fast_plane_detection_node");
00467
00469 int buffer_size_ = 1;
00470 fast_plane_detection::FastRegionPlaneDetector node_(buffer_size_);
00471 ros::spin();
00472
00473 return 0;
00474 }