$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: bb_estimator_client.cpp 742 2012-04-25 15:18:28Z spanel $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Tomas Hodan (xhodan04@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 25.4.2012 (version 6.0) 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00071 #include <srs_env_model_percp/bb_estimator/funcs.h> 00072 #include <srs_env_model_percp/services_list.h> 00073 #include <srs_env_model_percp/topics_list.h> 00074 #include <srs_env_model_percp/parameters_list.h> 00075 00076 // Definition of the service for BB estimation 00077 #include "srs_env_model_percp/EstimateBB.h" 00078 #include "srs_env_model_percp/EstimateBBAlt.h" 00079 #include "srs_env_model_percp/EstimateRect.h" 00080 #include "srs_env_model_percp/EstimateRectAlt.h" 00081 #include "srs_env_model_percp/Estimate2DHullMesh.h" 00082 00083 #include <srs_interaction_primitives/AddBoundingBox.h> 00084 #include <srs_interaction_primitives/ChangePose.h> 00085 #include <srs_interaction_primitives/ChangeScale.h> 00086 #include <srs_interaction_primitives/services_list.h> 00087 #include <srs_interaction_primitives/PoseType.h> 00088 00089 #include <algorithm> 00090 #include <cstdlib> 00091 00092 #include <message_filters/subscriber.h> 00093 #include <message_filters/synchronizer.h> 00094 #include <message_filters/sync_policies/approximate_time.h> 00095 #include <message_filters/cache.h> 00096 00097 #include <sensor_msgs/Image.h> 00098 #include <sensor_msgs/PointCloud2.h> 00099 #include <sensor_msgs/CameraInfo.h> 00100 #include <sensor_msgs/image_encodings.h> 00101 00102 #include <arm_navigation_msgs/Shape.h> 00103 00104 #include <geometry_msgs/Pose.h> 00105 #include <geometry_msgs/Vector3.h> 00106 00107 #include <cv_bridge/cv_bridge.h> 00108 #include <opencv2/highgui/highgui.hpp> 00109 #include <opencv2/imgproc/imgproc.hpp> 00110 00111 #include <pcl/io/pcd_io.h> 00112 #include <pcl/point_types.h> 00113 00114 #include <tf/transform_listener.h> 00115 00116 using namespace cv; 00117 using namespace sensor_msgs; 00118 using namespace message_filters; 00119 00120 00121 namespace srs_env_model_percp 00122 { 00123 00124 // Cache 00125 message_filters::Cache<Image> depthCache; // Cache of Image messages 00126 message_filters::Cache<PointCloud2> pointCloudCache; // Cache of PointCloud2 messages 00127 message_filters::Cache<CameraInfo> camInfoCache; // Cache of CameraInfo messages 00128 00129 // Frame IDs 00130 std::string camFrameId; // Camera frame id (will be obtained automatically) 00131 std::string sceneFrameId; // Scene (world) frame id 00132 00133 // Subscription variants 00134 int subVariant = SV_NONE; 00135 00136 // TF listener 00137 tf::TransformListener *tfListener; 00138 00139 // Percentage of farthest points from mean considered as outliers when 00140 // calculating statistics of ROI 00141 int outliersPercent = OUTLIERS_PERCENT_DEFAULT; 00142 00143 // The required maximum ratio of sides length (the longer side is at maximum 00144 // sidesRatio times longer than the shorter one) 00145 double sidesRatio = SIDES_RATIO_DEFAULT; 00146 00147 // Modes of bounding box estimation 00148 int estimationMode = 1; 00149 00150 bool isWaitingForResponse = false; // Client is waiting for response from server 00151 bool isBBCalculated = false; // Indicates, if there is any BB already calculated 00152 bool isBBPrimitiveCreated = false; // Indicates, if there is any BB primitive already created 00153 // (for visualization using interactive markers) 00154 Point2i mouseDown; // Position of mouse click 00155 Point2i roiP1, roiP2; // Two diagonally opposite corners of ROI 00156 vector<Point3f> bbVertices(8); // Corners of the estimated BB 00157 geometry_msgs::Pose bbPose; // Pose of the estimated BB 00158 geometry_msgs::Vector3 bbScale; // Scale of the estimated BB 00159 vector<Point2i> rectPoints(2); // Estimated rectangle after backward projection 00160 00161 // Names of windows 00162 string inputVideoWinName("Input Video (use your mouse to specify a ROI)"); 00163 string bbWinName("Bounding Box"); 00164 string hullWinName("Convex Hull"); 00165 00166 // Bounding box colors 00167 Scalar bbColorFront(0, 0, 255); // Color of the front side edges of the box 00168 Scalar bbColor(255, 0, 0); // Color of the rest of the edges 00169 Scalar rectColor(0, 255, 255); // Color of the rectangle 00170 00171 // Transformation from world to camera coordinates 00172 tf::StampedTransform worldToSensorTf; 00173 00174 // Display mode enumeration 00175 enum displayModesEnum {RGB, DEPTH}; 00176 int displayMode = RGB; 00177 00178 // Clients for communication with bb_estimator_server 00179 ros::ServiceClient bbEstimateClient, rectEstimateClient, Hull2DMeshEstimateClient; 00180 00181 // Client for the add_bounding_box, change_pose and change_scale service 00182 ros::ServiceClient bbAddClient, bbChangePoseClient, bbChangeScaleClient; 00183 00184 // Current and request images 00185 // currentX = the last received image of type X 00186 // requestX = the image of type X, which was current in the time of request 00187 // (currentImage and requestImage are references to corresponding Rgb or Depth 00188 // images - according to the selected display mode) 00189 Mat currentImage, requestImage; 00190 Mat currentRgb, requestRgb, currentDepth, requestDepth; 00191 Mat currentCamK, requestCamK; 00192 00193 00194 /*============================================================================== 00195 * Visualization of bounding box in a special window. 00196 */ 00197 void visualizeInImage() 00198 { 00199 // If the image does not have 3 channels => convert it (we want to visualize 00200 // the bounding box in color, thus we need 3 channels) 00201 Mat img3ch; 00202 if(requestImage.channels() != 3) { 00203 cvtColor(requestImage, img3ch, CV_GRAY2RGB, 3); 00204 } 00205 else { 00206 requestImage.copyTo(img3ch); 00207 } 00208 00209 // Transform the resulting BB vertices (in world coordinates) 00210 // to camera coordinates 00211 //-------------------------------------------------------------------------- 00212 vector<Point3f> bbVerticesCam(8); // Corners of estimated BB in camera coords 00213 00214 tf::Transformer t; 00215 t.setTransform(worldToSensorTf); 00216 00217 for(int i = 0; i < (int)bbVertices.size(); i++) { 00218 00219 tf::Stamped<tf::Point> vertex; 00220 vertex.frame_id_ = sceneFrameId; 00221 00222 vertex.setX(bbVertices[i].x); 00223 vertex.setY(bbVertices[i].y); 00224 vertex.setZ(bbVertices[i].z); 00225 t.transformPoint(camFrameId, vertex, vertex); 00226 bbVerticesCam[i].x = vertex.getX(); 00227 bbVerticesCam[i].y = vertex.getY(); 00228 bbVerticesCam[i].z = vertex.getZ(); 00229 } 00230 00231 // Perspective projection of the bounding box on the image plane: 00232 // world (camera) coords (3D) => image coords (2D) 00233 // 00234 // The world coordinate system is identical with the camera coordinate system 00235 // => Tx = Ty = 0 and R is identity => P[1:3,1:3] = K, where P is the camera 00236 // matrix and K is the intrinsic camera matrix. 00237 // (http://www.ros.org/wiki/image_pipeline/CameraInfo) 00238 // 00239 // The lines below implement these formulas: 00240 // x = X / Z * f_x + c_x 00241 // y = Y / Z * f_y + c_y 00242 // where [X,Y,Z] is a 3D point and [x,y] its perspective projection. 00243 //-------------------------------------------------------------------------- 00244 double bb3DVerticesArray[8][3]; 00245 for(int i = 0; i < (int)bbVertices.size(); i++) { 00246 bb3DVerticesArray[i][0] = bbVerticesCam[i].x / (double)bbVerticesCam[i].z; 00247 bb3DVerticesArray[i][1] = bbVerticesCam[i].y / (double)bbVerticesCam[i].z; 00248 bb3DVerticesArray[i][2] = 1; 00249 } 00250 Mat bb3DVertices = Mat(8, 3, CV_64F, bb3DVerticesArray); 00251 00252 // requestCamK is the intrinsic camera matrix (containing f_x, f_y, c_x, c_y) 00253 Mat bb2DVertices = requestCamK * bb3DVertices.t(); 00254 bb2DVertices = bb2DVertices.t(); 00255 00256 // Define edges of the bounding box 00257 //-------------------------------------------------------------------------- 00258 // Edges are represented by connections between vertices calculated above 00259 int edges[12][2] = { 00260 {0, 1}, {1, 2}, {2, 3}, {3, 0}, {0, 4}, {1, 5}, 00261 {2, 6}, {3, 7}, {4, 5}, {5, 6}, {6, 7}, {7, 4} 00262 }; 00263 00264 // Draw the bounding box 00265 //-------------------------------------------------------------------------- 00266 for(int i = 11; i >= 0; i--) { 00267 int j = edges[i][0]; 00268 int k = edges[i][1]; 00269 line(img3ch, 00270 Point((int)bb2DVertices.at<double>(j, 0), (int)bb2DVertices.at<double>(j, 1)), 00271 Point((int)bb2DVertices.at<double>(k, 0), (int)bb2DVertices.at<double>(k, 1)), 00272 (i < 4) ? bbColorFront : bbColor, 2); 00273 } 00274 00275 // Draw the rectangle 00276 //-------------------------------------------------------------------------- 00277 line(img3ch, 00278 Point(rectPoints[0].x, rectPoints[0].y), 00279 Point(rectPoints[0].x, rectPoints[1].y), 00280 rectColor, 1); 00281 line(img3ch, 00282 Point(rectPoints[0].x, rectPoints[1].y), 00283 Point(rectPoints[1].x, rectPoints[1].y), 00284 rectColor, 1); 00285 line(img3ch, 00286 Point(rectPoints[1].x, rectPoints[1].y), 00287 Point(rectPoints[1].x, rectPoints[0].y), 00288 rectColor, 1); 00289 line(img3ch, 00290 Point(rectPoints[1].x, rectPoints[0].y), 00291 Point(rectPoints[0].x, rectPoints[0].y), 00292 rectColor, 1); 00293 00294 // Show the image with visualized bounding box 00295 imshow(bbWinName, img3ch); 00296 } 00297 00298 00299 /*============================================================================== 00300 * Visualization of bounding box using interactive markers. 00301 */ 00302 void visualizeWithMarkers() 00303 { 00304 std::string bbPrimitiveName("estimated_bb"); 00305 00306 // No BB primitive has been created yet => create it 00307 //-------------------------------------------------------------------------- 00308 if(!isBBPrimitiveCreated) { 00309 00310 // Set parameters to the new bounding box 00311 srs_interaction_primitives::AddBoundingBox bbAddSrv; 00312 00313 bbAddSrv.request.name = bbPrimitiveName; 00314 bbAddSrv.request.frame_id = sceneFrameId; 00315 bbAddSrv.request.description = "Bounding Box"; 00316 00317 bbAddSrv.request.pose = bbPose; 00318 bbAddSrv.request.scale = bbScale; 00319 bbAddSrv.request.pose_type = srs_interaction_primitives::PoseType::POSE_CENTER; 00320 00321 bbAddSrv.request.color.r = 1.0; 00322 bbAddSrv.request.color.g = 0.0; 00323 bbAddSrv.request.color.b = 1.0; 00324 bbAddSrv.request.color.a = 1.0; 00325 00326 // Call service with specified parameters 00327 bbAddClient.call(bbAddSrv); 00328 00329 isBBPrimitiveCreated = true; 00330 } 00331 00332 // The BB primitive has already been created => modify it 00333 //-------------------------------------------------------------------------- 00334 else { 00335 // Change pose of the BB 00336 //-------------------- 00337 srs_interaction_primitives::ChangePose bbChangePoseSrv; 00338 bbChangePoseSrv.request.name = bbPrimitiveName; 00339 bbChangePoseSrv.request.pose = bbPose; 00340 00341 // Call service with specified parameters 00342 bbChangePoseClient.call(bbChangePoseSrv); 00343 00344 // Change scale of the BB 00345 //-------------------- 00346 srs_interaction_primitives::ChangeScale bbChangeScaleSrv; 00347 bbChangeScaleSrv.request.name = bbPrimitiveName; 00348 bbChangeScaleSrv.request.scale = bbScale; 00349 00350 // Call service with specified parameters 00351 bbChangeScaleClient.call(bbChangeScaleSrv); 00352 } 00353 } 00354 00355 00356 /*============================================================================== 00357 * Visualization of bounding box. 00358 */ 00359 void visualize() 00360 { 00361 visualizeInImage(); 00362 visualizeWithMarkers(); 00363 } 00364 00365 00366 /*============================================================================== 00367 * Sets the images (current and request one) based on the selected display mode. 00368 */ 00369 void setImages() 00370 { 00371 // RGB mode 00372 if(displayMode == RGB) { 00373 currentImage = currentRgb; 00374 requestImage = requestRgb; 00375 } 00376 00377 // DEPTH mode 00378 else { 00379 currentImage = currentDepth; 00380 requestImage = requestDepth; 00381 } 00382 } 00383 00384 00385 /*============================================================================== 00386 * Redraw windows according to the selected display mode. 00387 */ 00388 void redrawWindows() 00389 { 00390 // Set the current and request images based on the selected display mode 00391 setImages(); 00392 00393 // Show the current image 00394 imshow(inputVideoWinName, currentImage); 00395 00396 // Show the bounding box (if there is any calculated already) 00397 if(isBBCalculated) { 00398 visualize(); 00399 } 00400 } 00401 00402 00403 /*============================================================================== 00404 * Send request for bounding box calculation. 00405 * 00406 * @param p1 A corner of the region of interest. 00407 * @param p1 A corner of the region of interest - diagonally opposite to p1. 00408 */ 00409 void sendRequest(Point2i p1, Point2i p2) 00410 { 00411 // Return if the input points are empty 00412 if(p1 == Point2i() || p2 == Point2i()) { 00413 return; 00414 } 00415 00416 isWaitingForResponse = true; 00417 00418 // Create the request 00419 //-------------------------------------------------------------------------- 00420 // Instantiate the autogenerated service class, and assign values into 00421 // its request member 00422 srs_env_model_percp::EstimateBB srv; 00423 srv.request.p1[0] = p1.x; 00424 srv.request.p1[1] = p1.y; 00425 srv.request.p2[0] = p2.x; 00426 srv.request.p2[1] = p2.y; 00427 srv.request.mode = estimationMode; 00428 00429 // When using simulated Clock time, now() returns time 0 until first message 00430 // has been received on /clock topic => wait for that. 00431 ros::Time reqTime; 00432 do { 00433 reqTime = ros::Time::now(); 00434 } while(reqTime.sec == 0); 00435 srv.request.header.stamp = reqTime; 00436 00437 // Obtain the corresponding transformation from world to camera coordinate system 00438 // (for later visualization in an image) 00439 //-------------------------------------------------------------------------- 00440 try { 00441 tfListener->waitForTransform(sceneFrameId, camFrameId, reqTime, ros::Duration(0.2)); 00442 tfListener->lookupTransform(sceneFrameId, camFrameId, reqTime, worldToSensorTf); 00443 } 00444 catch(tf::TransformException& ex) { 00445 string errorMsg = String("Transform error: ") + ex.what(); 00446 ROS_ERROR("%s", errorMsg.c_str()); 00447 return; 00448 } 00449 00450 // Send request and obtain response (the bounding box coordinates) 00451 //-------------------------------------------------------------------------- 00452 // Call the service (calls are blocking, it will return once the call is done) 00453 if(bbEstimateClient.call(srv)) 00454 { 00455 isWaitingForResponse = false; 00456 isBBCalculated = true; 00457 00458 srs_env_model_percp::EstimateBB::Response res = srv.response; 00459 00460 // Save the response (in world coordinates) 00461 //---------------------------------------------------------------------- 00462 bbVertices[0] = Point3f(res.p1[0], res.p1[1], res.p1[2]); 00463 bbVertices[1] = Point3f(res.p2[0], res.p2[1], res.p2[2]); 00464 bbVertices[2] = Point3f(res.p3[0], res.p3[1], res.p3[2]); 00465 bbVertices[3] = Point3f(res.p4[0], res.p4[1], res.p4[2]); 00466 bbVertices[4] = Point3f(res.p5[0], res.p5[1], res.p5[2]); 00467 bbVertices[5] = Point3f(res.p6[0], res.p6[1], res.p6[2]); 00468 bbVertices[6] = Point3f(res.p7[0], res.p7[1], res.p7[2]); 00469 bbVertices[7] = Point3f(res.p8[0], res.p8[1], res.p8[2]); 00470 00471 bbPose = res.pose; 00472 bbScale = res.scale; 00473 00474 // Create the request for backward projection to the image plane test 00475 //-------------------------------------------------------------------------- 00476 // Instantiate the autogenerated service class, and assign values into 00477 // its request member 00478 srs_env_model_percp::EstimateRect srv2; 00479 srv2.request.pose = bbPose; 00480 srv2.request.scale = bbScale; 00481 srv2.request.header.stamp = reqTime; 00482 00483 // Call the service (calls are blocking, it will return once the call is done) 00484 if( rectEstimateClient.call(srv2) ) 00485 { 00486 srs_env_model_percp::EstimateRect::Response res2 = srv2.response; 00487 00488 rectPoints[0] = Point2i(res2.p1[0], res2.p1[1]); 00489 rectPoints[1] = Point2i(res2.p2[0], res2.p2[1]); 00490 } 00491 else 00492 { 00493 rectPoints[0] = Point2i(0, 0); 00494 rectPoints[1] = Point2i(0, 0); 00495 std::string errMsg = "Failed to call service " + EstimateRect_SRV + "."; 00496 ROS_ERROR("%s", errMsg.c_str()); 00497 } 00498 00499 // Show the resulting bounding box (and rectangle) 00500 visualize(); 00501 00502 // Log request 00503 //---------------------------------------------------------------------- 00504 std::cout << "----------" << std::endl; 00505 ROS_INFO("ROI (request): [%ld, %ld], [%ld, %ld]", 00506 (long int)srv.request.p1[0], (long int)srv.request.p1[1], 00507 (long int)srv.request.p2[0], (long int)srv.request.p2[1]); 00508 00509 // Log response 00510 //---------------------------------------------------------------------- 00511 std::stringstream ss; 00512 ss << "Bounding box, mode = " << estimationMode << " (response):\n"; 00513 for(int i = 0; i < (int)bbVertices.size(); i++) { 00514 ss << "[" << bbVertices[i].x << "," 00515 << bbVertices[i].y << "," 00516 << bbVertices[i].z << "] "; 00517 if(i == 3) ss << "\n"; 00518 } 00519 ss << std::endl; 00520 ss << "Rectangle (response):\n"; 00521 for(int i = 0; i < (int)rectPoints.size(); i++) { 00522 ss << "[" << rectPoints[i].x << "," 00523 << rectPoints[i].y << "] "; 00524 } 00525 ss << std::endl; 00526 ROS_INFO("%s", ss.str().c_str()); 00527 } 00528 else { 00529 isWaitingForResponse = false; 00530 std::string errMsg = "Failed to call service " + EstimateBB_SRV + "."; 00531 ROS_ERROR("%s", errMsg.c_str()); 00532 } 00533 } 00534 00535 /*============================================================================== 00536 * Test estimation service for 2D convex hull (in image plane coordinates) of a mesh 00537 * (in world coordinates). 00538 */ 00539 void estimate2DHullMesh() 00540 { 00541 // Create the request 00542 //-------------------------------------------------------------------------- 00543 srs_env_model_percp::Estimate2DHullMesh srv; 00544 00545 // Create a test mesh (in camera coordinates) 00546 //-------------------------------------------------------------------------- 00547 arm_navigation_msgs::Shape mC; 00548 mC.type = mC.MESH; 00549 mC.vertices.resize(4); 00550 mC.vertices[0].x = 0; mC.vertices[0].y = 0; mC.vertices[0].z = 1; 00551 mC.vertices[1].x = 0; mC.vertices[1].y = 0.2; mC.vertices[1].z = 1; 00552 mC.vertices[2].x = 0.2; mC.vertices[2].y = 0.2; mC.vertices[2].z = 1; 00553 mC.vertices[3].x = -0.2; mC.vertices[3].y = 0.3; mC.vertices[3].z = 2; 00554 00555 // triangle k is defined by tre vertices located at indices 00556 // triangles[3k], triangles[3k+1], triangles[3k+2] 00557 mC.triangles.resize(6); 00558 mC.triangles[0] = 0; mC.triangles[1] = 1; mC.triangles[2] = 2; 00559 mC.triangles[3] = 1; mC.triangles[4] = 2; mC.triangles[5] = 3; 00560 00561 // When using simulated Clock time, now() returns time 0 until first message 00562 // has been received on /clock topic => wait for that. 00563 ros::Time reqTime; 00564 do { 00565 reqTime = ros::Time::now(); 00566 } while(reqTime.sec == 0); 00567 srv.request.header.stamp = reqTime; 00568 00569 // Read the messages from cache (the latest ones before the request timestamp) 00570 //-------------------------------------------------------------------------- 00571 sensor_msgs::CameraInfoConstPtr camInfo = camInfoCache.getElemBeforeTime(reqTime); 00572 if(camInfo == 0) { 00573 ROS_ERROR("No camera info was obtained before the request time."); 00574 return; 00575 } 00576 00577 // Get the intrinsic camera parameters 00578 //-------------------------------------------------------------------------- 00579 Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data()); 00580 float cx = (float)K.at<double>(0, 2); 00581 float cy = (float)K.at<double>(1, 2); 00582 float fx = (float)K.at<double>(0, 0); 00583 float fy = (float)K.at<double>(1, 1); 00584 //float f = (fx + fy) / 2.0; 00585 00586 if(fx == 0 || fy == 0 || cx == 0 || cy == 0) { 00587 ROS_ERROR("Intrinsic camera parameters are undefined."); 00588 } 00589 00590 // Obtain the corresponding transformation from world to camera coordinate system 00591 //-------------------------------------------------------------------------- 00592 try { 00593 tfListener->waitForTransform(sceneFrameId, camFrameId, reqTime, ros::Duration(0.2)); 00594 tfListener->lookupTransform(sceneFrameId, camFrameId, reqTime, worldToSensorTf); 00595 } 00596 catch(tf::TransformException& ex) { 00597 string errorMsg = String("Transform error: ") + ex.what(); 00598 ROS_ERROR("%s", errorMsg.c_str()); 00599 return; 00600 } 00601 00602 // Transform the test mesh to world coordinates 00603 //-------------------------------------------------------------------------- 00604 tf::Transformer t; 00605 t.setTransform(worldToSensorTf); 00606 arm_navigation_msgs::Shape mW; 00607 mW.type = mW.MESH; 00608 mW.vertices.resize(mC.vertices.size()); 00609 for( int i = 0; i < (int)mC.vertices.size(); i++ ) 00610 { 00611 // Transformation to the world coordinates 00612 tf::Stamped<tf::Point> vertex; 00613 vertex.frame_id_ = camFrameId; 00614 vertex.setX(mC.vertices[i].x); 00615 vertex.setY(mC.vertices[i].y); 00616 vertex.setZ(mC.vertices[i].z); 00617 t.transformPoint(sceneFrameId, vertex, vertex); 00618 00619 mW.vertices[i].x = vertex.getX(); 00620 mW.vertices[i].y = vertex.getY(); 00621 mW.vertices[i].z = vertex.getZ(); 00622 } 00623 srv.request.mesh = mW; 00624 00625 // Call the service (calls are blocking, it will return once the call is done) 00626 //-------------------------------------------------------------------------- 00627 if( Hull2DMeshEstimateClient.call(srv) ) 00628 { 00629 srs_env_model_percp::Estimate2DHullMesh::Response res = srv.response; 00630 00631 // If the image does not have 3 channels => convert it (we want to visualize 00632 // the bounding box in color, thus we need 3 channels) 00633 Mat img3ch; 00634 if(currentRgb.channels() != 3) { 00635 cvtColor(currentRgb, img3ch, CV_GRAY2RGB, 3); 00636 } 00637 else { 00638 currentRgb.copyTo(img3ch); 00639 } 00640 00641 // Draw the projected test mesh 00642 //---------------------------------------------------------------------- 00643 // Project the vertices onto image plane 00644 vector<Point2i> trPoints(mC.vertices.size()); 00645 for( int i = 0; i < (int)mC.vertices.size(); i++ ) { 00646 Point3f p(mC.vertices[i].x, mC.vertices[i].y, mC.vertices[i].z); 00647 trPoints[i] = Point2i(int(p.x * fx / p.z + cx + 0.5), int(p.y * fy / p.z + cy + 0.5)); 00648 //trPoints[i] = fwdProject(Point3f(mC.vertices[i].x, mC.vertices[i].y, mC.vertices[i].z), fx, fy, cx, cy); 00649 } 00650 00651 // Draw the triangles 00652 for( int i = 2; i < (int)mC.triangles.size(); i += 3 ) { 00653 line(img3ch, 00654 Point(trPoints[mC.triangles[i - 2]].x, trPoints[mC.triangles[i - 2]].y), 00655 Point(trPoints[mC.triangles[i - 1]].x, trPoints[mC.triangles[i - 1]].y), bbColor, 3); 00656 line(img3ch, 00657 Point(trPoints[mC.triangles[i - 1]].x, trPoints[mC.triangles[i - 1]].y), 00658 Point(trPoints[mC.triangles[i]].x, trPoints[mC.triangles[i]].y), bbColor, 3); 00659 line(img3ch, 00660 Point(trPoints[mC.triangles[i - 2]].x, trPoints[mC.triangles[i - 2]].y), 00661 Point(trPoints[mC.triangles[i]].x, trPoints[mC.triangles[i]].y), bbColor, 3); 00662 } 00663 00664 // Draw the resulting convex hull 00665 //---------------------------------------------------------------------- 00666 for(int i = 0; i < (int)res.convexHull.points.size(); i++) { 00667 int j = (i + 1) % res.convexHull.points.size(); 00668 00669 line(img3ch, 00670 Point(res.convexHull.points[i].x, res.convexHull.points[i].y), 00671 Point(res.convexHull.points[j].x, res.convexHull.points[j].y), bbColorFront, 1); 00672 } 00673 00674 // Show the image with visualized bounding box 00675 imshow(hullWinName, img3ch); 00676 } 00677 else 00678 { 00679 std::string errMsg = "Failed to call service " + Estimate2DHullMesh_SRV + "."; 00680 ROS_ERROR("%s", errMsg.c_str()); 00681 } 00682 } 00683 00684 00685 /*============================================================================== 00686 * Scales depth values (for visualization purposes). 00687 * 00688 * The provided depth is typically in CV_16SC1 datatype. 00689 * Because we do not expect to have any negative depth value, we 00690 * can convert the datatype to unsigned integers, which is sufficient 00691 * for visualization purposes + more OpenCV functions can work with this 00692 * datatype (e.g. cvtColor used in visualize function). 00693 * The depth values are scaled to fit the range <0, 255>. 00694 */ 00695 void scaleDepth() 00696 { 00697 // Do not consider large ("infinity") values 00698 Mat infMask; 00699 if(subVariant == SV_1) { 00700 infMask = currentDepth > 20000; // Pixels 00701 } 00702 else if(subVariant == SV_2) { 00703 infMask = currentDepth > 20; // Meters 00704 } 00705 00706 // Get mask of known values 00707 Mat knownMask = (infMask == 0); 00708 00709 double maxDepth; 00710 minMaxLoc(currentDepth, NULL, &maxDepth, NULL, NULL, knownMask); 00711 currentDepth.convertTo(currentDepth, CV_8U, 255.0 / maxDepth); 00712 00713 // Set color of "infinity" values 00714 currentDepth.setTo(255, infMask); 00715 } 00716 00717 /*============================================================================== 00718 * Processes messages from synchronized subscription variant #1. 00719 * 00720 * @param rgb Message with rgb image. 00721 * @param depth Message with depth image. 00722 * @param camInfo Message with camera information. 00723 */ 00724 void sv1_processSubMsgs(const sensor_msgs::ImageConstPtr &rgb, 00725 const sensor_msgs::ImageConstPtr &depth, 00726 const sensor_msgs::CameraInfoConstPtr &camInfo) 00727 { 00728 if(subVariant == SV_NONE) { 00729 subVariant = SV_1; 00730 00731 // Set size of cache 00732 camInfoCache.setCacheSize(CACHE_SIZE); 00733 } 00734 else if(subVariant != SV_1) { 00735 return; 00736 } 00737 00738 camInfoCache.add(camInfo); 00739 00740 // Get the intrinsic camera matrix of our camera 00741 Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data()); 00742 K.copyTo(currentCamK); 00743 00744 // Save frame id 00745 camFrameId = camInfo->header.frame_id; 00746 00747 // Get rgb and depth image from the messages 00748 //-------------------------------------------------------------------------- 00749 try { 00750 currentRgb = cv_bridge::toCvCopy(rgb)->image; 00751 currentDepth = cv_bridge::toCvCopy(depth)->image; 00752 } 00753 catch (cv_bridge::Exception& e) { 00754 ROS_ERROR("cv_bridge exception: %s", e.what()); 00755 return; 00756 } 00757 00758 // Scales depth values (for visualization purposes). 00759 scaleDepth(); 00760 00761 // Set the current and request images based on the selected display mode 00762 setImages(); 00763 00764 // Show the current image 00765 //-------------------------------------------------------------------------- 00766 imshow(inputVideoWinName, currentImage); 00767 00768 if(DEBUG) { 00769 // Prints the timestamp of the received frame 00770 ROS_INFO("Received frame timestamp: %d.%d", 00771 rgb->header.stamp.sec, rgb->header.stamp.nsec); 00772 } 00773 } 00774 00775 00776 /*============================================================================== 00777 * Processes messages from synchronized subscription variant #2. 00778 * 00779 * @param rgb Message with rgb image. 00780 * @param pointCloud Message with point cloud. 00781 * @param camInfo Message with camera information. 00782 */ 00783 void sv2_processSubMsgs(const sensor_msgs::ImageConstPtr &rgb, 00784 const sensor_msgs::PointCloud2ConstPtr &pointCloud, 00785 const sensor_msgs::CameraInfoConstPtr &camInfo) 00786 { 00787 if(subVariant == SV_NONE) { 00788 subVariant = SV_2; 00789 00790 // Set size of cache 00791 camInfoCache.setCacheSize(CACHE_SIZE); 00792 } 00793 else if(subVariant != SV_2) { 00794 return; 00795 } 00796 00797 camInfoCache.add(camInfo); 00798 00799 // Get the intrinsic camera matrix of our camera 00800 Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data()); 00801 K.copyTo(currentCamK); 00802 00803 // Save frame id 00804 camFrameId = camInfo->header.frame_id; 00805 00806 // Convert the sensor_msgs/PointCloud2 data to depth map 00807 //-------------------------------------------------------------------------- 00808 // At first convert to pcl/PointCloud 00809 pcl::PointCloud<pcl::PointXYZ> cloud; 00810 pcl::fromROSMsg (*pointCloud, cloud); 00811 00812 currentDepth = Mat(cloud.height, cloud.width, CV_32F); 00813 00814 for(int y = 0; y < (int)cloud.height; y++) { 00815 for(int x = 0; x < (int)cloud.width; x++) { 00816 currentDepth.at<float>(y, x) = cloud.points[y * cloud.width + x].z; 00817 } 00818 } 00819 00820 // Get rgb and depth image from the Image messages 00821 //-------------------------------------------------------------------------- 00822 try { 00823 currentRgb = cv_bridge::toCvCopy(rgb)->image; 00824 } 00825 catch (cv_bridge::Exception& e) { 00826 ROS_ERROR("cv_bridge exception: %s", e.what()); 00827 return; 00828 } 00829 00830 // The image / point cloud coming from COB is flipped around X and Y axis 00831 //flip(currentRgb, currentRgb, -1); 00832 //flip(currentDepth, currentDepth, -1); 00833 00834 // Scales depth values (for visualization purposes). 00835 scaleDepth(); 00836 00837 // Set the current and request images based on the selected display mode 00838 setImages(); 00839 00840 // Show the current image 00841 //-------------------------------------------------------------------------- 00842 imshow(inputVideoWinName, currentImage); 00843 00844 if(DEBUG) { 00845 // Prints the timestamp of the received frame 00846 ROS_INFO("Received frame timestamp: %d.%d", 00847 rgb->header.stamp.sec, rgb->header.stamp.nsec); 00848 } 00849 } 00850 00851 00852 /*============================================================================== 00853 * Mouse event handler. 00854 * 00855 * @param event Mouse event to be handled. 00856 * @param x x-coordinate of the event. 00857 * @param y y-coordinate of the event. 00858 */ 00859 void onMouse(int event, int x, int y, int flags, void *param) 00860 { 00861 switch(event) { 00862 00863 // Mouse DOWN (start-point of the ROI) 00864 case CV_EVENT_LBUTTONDOWN: 00865 mouseDown.x = x; 00866 mouseDown.y = y; 00867 break; 00868 00869 // Mouse UP (end-point of the ROI) 00870 case CV_EVENT_LBUTTONUP: 00871 // Save the current images as the request ones 00872 currentImage.copyTo(requestImage); 00873 currentRgb.copyTo(requestRgb); 00874 currentDepth.copyTo(requestDepth); 00875 currentCamK.copyTo(requestCamK); 00876 00877 roiP1 = Point2i(mouseDown.x, mouseDown.y); 00878 roiP2 = Point2i(x, y); 00879 00880 // Send request for bounding box calculation 00881 // (if there is no pending one already) 00882 if(!isWaitingForResponse) { 00883 sendRequest(roiP1, roiP2); 00884 } 00885 break; 00886 } 00887 } 00888 00889 } 00890 00891 /*============================================================================== 00892 * Main function 00893 */ 00894 int main(int argc, char **argv) 00895 { 00896 using namespace srs_env_model_percp; 00897 00898 // ROS initialization (the last argument is the name of the node) 00899 ros::init(argc, argv, "bb_estimator_client"); 00900 00901 // NodeHandle is the main access point to communications with the ROS system 00902 ros::NodeHandle n; 00903 00904 // Create a client for the /bb_estimator/estimate_bb service 00905 bbEstimateClient = n.serviceClient<srs_env_model_percp::EstimateBB>(EstimateBB_SRV); 00906 00907 // Create a client for the /bb_estimator/estimate_rect service 00908 rectEstimateClient = n.serviceClient<srs_env_model_percp::EstimateRect>(EstimateRect_SRV); 00909 00910 // Create a client for the /estimate_2D_hull_mesh service 00911 Hull2DMeshEstimateClient = n.serviceClient<srs_env_model_percp::Estimate2DHullMesh>(Estimate2DHullMesh_SRV); 00912 00913 // Create clients for the add_bounding_box, change_pose and change_scale services 00914 // (for manipulation with a visualization of BB) 00915 bbAddClient = n.serviceClient<srs_interaction_primitives::AddBoundingBox>(srs_interaction_primitives::AddBoundingBox_SRV); 00916 bbChangePoseClient = n.serviceClient<srs_interaction_primitives::ChangePose>(srs_interaction_primitives::ChangePose_SRV); 00917 bbChangeScaleClient = n.serviceClient<srs_interaction_primitives::ChangeScale>(srs_interaction_primitives::ChangeScale_SRV); 00918 00919 // Create a TF listener 00920 tfListener = new tf::TransformListener(); 00921 00922 // Get private parameters from the parameter server 00923 // (the third parameter of function param is the default value) 00924 //-------------------------------------------------------------------------- 00925 ros::NodeHandle private_nh("~"); 00926 private_nh.param(GLOBAL_FRAME_PARAM, sceneFrameId, GLOBAL_FRAME_DEFAULT); 00927 00928 ROS_INFO_STREAM(GLOBAL_FRAME_PARAM << " = " << sceneFrameId); 00929 00930 // Subscription and synchronization of messages 00931 // TODO: Create the subscribers dynamically and unsubscribe the unused 00932 // subscription variant. 00933 //-------------------------------------------------------------------------- 00934 // Subscription variant #1 00935 message_filters::Subscriber<Image> sv1_rgb_sub(n, RGB_IMAGE_TOPIC_IN, 1); 00936 message_filters::Subscriber<Image> sv1_depth_sub(n, DEPTH_IMAGE_TOPIC_IN, 1); 00937 message_filters::Subscriber<CameraInfo> sv1_camInfo_sub(n, CAMERA_INFO_TOPIC_IN, 1); 00938 00939 typedef sync_policies::ApproximateTime<Image, Image, CameraInfo> sv1_MySyncPolicy; 00940 Synchronizer<sv1_MySyncPolicy> sv1_sync(sv1_MySyncPolicy(QUEUE_SIZE), sv1_rgb_sub, sv1_depth_sub, sv1_camInfo_sub); 00941 sv1_sync.registerCallback(boost::bind(&sv1_processSubMsgs, _1, _2, _3)); 00942 00943 // Subscription variant #2 00944 message_filters::Subscriber<Image> sv2_rgb_sub(n, RGB_IMAGE_TOPIC_IN, 1); 00945 message_filters::Subscriber<PointCloud2> sv2_pointCloud_sub(n, POINT_CLOUD_TOPIC_IN, 1); 00946 message_filters::Subscriber<CameraInfo> sv2_camInfo_sub(n, CAMERA_INFO_TOPIC_IN, 1); 00947 00948 typedef sync_policies::ApproximateTime<Image, PointCloud2, CameraInfo> sv2_MySyncPolicy; 00949 Synchronizer<sv2_MySyncPolicy> sv2_sync(sv2_MySyncPolicy(QUEUE_SIZE), sv2_rgb_sub, sv2_pointCloud_sub, sv2_camInfo_sub); 00950 sv2_sync.registerCallback(boost::bind(&sv2_processSubMsgs, _1, _2, _3)); 00951 00952 // Create a window to show the incoming video and set its mouse event handler 00953 //-------------------------------------------------------------------------- 00954 namedWindow(inputVideoWinName.c_str(), CV_WINDOW_AUTOSIZE); 00955 cvSetMouseCallback(inputVideoWinName.c_str(), onMouse); 00956 00957 ROS_INFO("Ready"); 00958 00959 // Enters a loop 00960 //-------------------------------------------------------------------------- 00961 while(ros::ok()) { 00962 char key = waitKey(10); // Process window events (i.e. also mouse events) 00963 00964 // If the key D was pressed -> change the display mode 00965 if(key == 'd' || key == 'D') { 00966 displayMode = (displayMode == RGB) ? DEPTH : RGB; 00967 redrawWindows(); // Redraw windows (with input video and bounding box) 00968 } 00969 // If the key E was pressed -> change the estimation mode 00970 else if(key == 'e' || key == 'E') { 00971 estimationMode = (estimationMode == 3) ? 1 : (estimationMode + 1); 00972 ROS_INFO("Estimation mode %d activated.", estimationMode); 00973 sendRequest(roiP1, roiP2); 00974 } 00975 // If the key M was pressed -> test estimation service for 2D convex hull of a mesh 00976 else if(key == 'm' || key == 'M') { 00977 estimate2DHullMesh(); 00978 } 00979 00980 ros::spinOnce(); // Call all the message callbacks waiting to be called 00981 } 00982 00983 return 0; 00984 } 00985