sample_client.cpp
Go to the documentation of this file.
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 


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:57