funcs.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: bb_estimator_server.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 
00028 #include <srs_env_model_percp/bb_estimator/funcs.h>
00029 #include <srs_env_model_percp/services_list.h>
00030 #include <srs_env_model_percp/topics_list.h>
00031 
00032 #include <algorithm>
00033 
00034 #include <message_filters/sync_policies/approximate_time.h>
00035 #include <message_filters/cache.h>
00036 
00037 #include <tf/transform_listener.h>
00038 
00039 using namespace cv;
00040 using namespace sensor_msgs;
00041 using namespace message_filters;
00042 
00043 
00044 namespace srs_env_model_percp
00045 {
00046 
00047 // Global variables
00048 //------------------------------------------------------------------------------
00049 
00050 // Cache
00051 extern message_filters::Cache<Image> depthCache; // Cache of Image messages
00052 extern message_filters::Cache<PointCloud2> pointCloudCache; // Cache of PointCloud2 messages
00053 extern message_filters::Cache<CameraInfo> camInfoCache; // Cache of CameraInfo messages
00054 
00055 // Frame IDs
00056 extern std::string camFrameId; // Camera frame id (will be obtained automatically)
00057 extern std::string sceneFrameId; // Scene (world) frame id
00058 
00059 // Subscription variants
00060 extern int subVariant;
00061 
00062 // TF listener
00063 extern tf::TransformListener *tfListener;
00064 
00065 // Percentage of furthest points from mean considered as outliers when
00066 // calculating statistics of ROI
00067 extern int outliersPercent;
00068 
00069 // Percentage of rows and columns considered for sampling (for calculation of statistics)
00070 extern int samplingPercent;
00071 
00072 // The required maximum ratio of sides length (the longer side is at maximum
00073 // sidesRatio times longer than the shorter one)
00074 extern double sidesRatio;
00075 
00076 // Modes of bounding box estimation
00077 extern int estimationMode;
00078 
00079 
00080 /******************************************************************************
00081  * Back perspective projection of a 2D point with a known depth.
00082  */
00083 
00084 Point3f backProject(Point2i p, float z, float fx, float fy)
00085 {
00086     return Point3f((p.x * z) / fx, (p.y * z) / fy, z);
00087 }
00088 
00089 
00090 /******************************************************************************
00091  * Perspective projection of a 3D point to the image plane.
00092  */
00093 
00094 Point2i fwdProject(Point3f p, float fx, float fy, float cx, float cy)
00095 {
00096     return Point2i(int(p.x * fx / p.z + cx + 0.5), int(p.y * fy / p.z + cy + 0.5));
00097 }
00098 
00099 
00100 /******************************************************************************
00101  * Calculation of statistics (mean, standard deviation, min and max).
00102  */
00103 
00104 bool calcStats(Mat &m, float *mean, float *stdDev)
00105 {
00106 
00107     // Get the mask of known values (the unknown are represented by 0, the
00108     // known by 255)
00109     Mat negMask = m <= 0; // Negative values
00110     Mat infMask = m > 20; // "Infinite" values (more than 20 meters)
00111     Mat knownMask = ((negMask + infMask) == 0);
00112     
00113     // Sampling (to speed up the calculation we consider only pixels on a sampling
00114     // grid, whose density is given by the specified parameter samplingPercent,
00115     // which says how many percent of rows and columns should be considered)
00116     int rowSamples = knownMask.rows * (samplingPercent / 100.0);
00117     int columnSamples = knownMask.cols * (samplingPercent / 100.0);
00118     int samplingStepY = max(rowSamples, 1);
00119     int samplingStepX = max(columnSamples, 1);
00120     
00121     // Keep only pixels on a sampling grid
00122     for(int y = 0; y < knownMask.rows; y++) {
00123         if(y % samplingStepY != 0) {
00124             for(int x = 0; x < knownMask.cols; x++) {
00125                 if(x % samplingStepX != 0) {
00126                     knownMask.at<uchar>(y, x) = 0;
00127                 }
00128             }
00129         }
00130     }
00131 
00132     // Mean and standard deviation
00133     Scalar meanS, stdDevS;
00134     float meanValue, stdDevValue;
00135     meanStdDev(m, meanS, stdDevS, knownMask);
00136     meanValue = meanS[0];
00137     stdDevValue = stdDevS[0];
00138     
00139     // When all the knownMask elements are zeros, the function meanStdDev returns
00140     // roiMean == roiStdDev == 0
00141     if(meanValue == 0 && stdDevValue == 0) {
00142         ROS_WARN("No depth information available in the region of interest");
00143         return false;
00144     }
00145 
00146     // Number of known values and outliers to be ignored
00147     int knownCount = sum(knownMask * (1.0/255.0))[0];
00148     int outliersCount = (knownCount * outliersPercent) / 100;
00149 
00150     // Just for sure check if there will be some known values left after removal
00151     // of outliers.
00152     if((knownCount - outliersCount) > 0) {
00153     
00154         // Values of m relative to the mean value
00155         Mat mMeanRel = abs(m - meanValue);
00156 
00157         // Find and ignore the given percentage of outliers (the furthest points
00158         // from the mean value)
00159         //----------------------------------------------------------------------
00160         vector<Point3f> outliers; // A vector to store a given number of the furthest
00161                                   // points from the mean value (z of each Point3f
00162                                   // stores an outlier value)
00163         float currMin = 0; // Current min value included in the vector of outliers
00164         int currMinIndex = 0;
00165         
00166         // Go through all the samples and find outliersCount of the furthest points
00167         // from the mean value (outliers)
00168         for(int y = 0; y < mMeanRel.rows; y++) {
00169             for(int x = 0; x < mMeanRel.cols; x++) {
00170                 if(knownMask.at<uchar>(y, x)
00171                    && (mMeanRel.at<float>(y, x) > currMin || (int)outliers.size() < outliersCount)) {
00172                    
00173                     Point3f outlier(x, y, mMeanRel.at<float>(y, x));
00174                     
00175                     // If the vector of outliers is not full yet
00176                     if((int)outliers.size() < outliersCount) {
00177                         outliers.push_back(outlier);
00178                     }
00179                     
00180                     // If the vector of outliers is full already, replace the outlier with minimum value
00181                     else {
00182                         outliers.erase(outliers.begin() + currMinIndex);
00183                         outliers.insert(outliers.begin() + currMinIndex, outlier);
00184                     }
00185                     
00186                     // Update the current min value
00187                     float min = 100;
00188                     int minIndex = 0;
00189                     for(unsigned int i = 0; i < outliers.size(); i++) {
00190                         if(outliers[i].z < min) {
00191                             min = outliers[i].z;
00192                             minIndex = i;
00193                         }
00194                     }
00195                     currMin = min;
00196                     currMinIndex = minIndex;
00197                 }
00198             }
00199         }
00200         
00201         // Mask out all the outliers
00202         for(unsigned int i = 0; i < outliers.size(); i++) {
00203             knownMask.at<uchar>(outliers[i].y, outliers[i].x) = 0;
00204         }
00205 
00206         // Mean and standard deviation (now ignoring the outliers)
00207         meanStdDev(m, meanS, stdDevS, knownMask);
00208         meanValue = meanS[0];
00209         stdDevValue = stdDevS[0];
00210     }
00211 
00212     *mean = meanValue;
00213     *stdDev = stdDevValue;
00214 
00215 
00216     if(DEBUG) {
00217         // Min and max
00218         double min, max;
00219         minMaxLoc(m, &min, &max, 0, 0, knownMask);
00220 
00221         // Print the calculated statistics
00222         std::cout << "DEPTH STATISTICS "
00223                   << "- Mean: " << *mean << ", StdDev: " << *stdDev
00224                   << ", Min: " << min << ", Max: " << max << std::endl;
00225     }
00226     
00227     return true;
00228 }
00229 
00230 
00231 /******************************************************************************
00232  * Calculation of distances from origin to the BB front and back face vertices.
00233  */
00234 
00235 bool calcNearAndFarFaceDistance(Mat &m, float fx, float fy, Point2i roiLB,
00236                                 Point2i roiRT, float *d1, float *d2)
00237 {
00238         // Get mean and standard deviation
00239         float mean, stdDev;
00240         if(!calcStats(m, &mean, &stdDev)) {
00241                 return false;
00242         }
00243 
00244         // Distance from origin to the front and back face vertices
00245         *d1 = mean - stdDev;
00246         *d2 = mean + stdDev;
00247 
00248     // TODO: The following check can be done only after conversion the focal
00249     // length to the same units as the distances have (= meters)
00250     // ----------
00251         // Check if all BB vertices are in the front of the image plane (only the
00252         // vertices of the front face can be behind (= closer to origin),
00253         // so it is enough to check them). A vector from origin to the depth = f
00254         // in direction of each of the front face vertices is constructed.
00255         // If the length l of such vector is bigger than the distance d1
00256         // (= distance from origin to the front face vertices), it means that
00257         // the corresponding vertex is behind the image plane, thus set d1 to l
00258         // (the vertex is moved to depth = f). This is done for all front face
00259         // vertices...
00260         /*
00261     float f = (fx + fy) / 2.0;
00262         
00263         Point3f vecLBFf = backProject(roiLB, f, fx, fy);
00264         float vecLBFlen = norm(vecLBFf);
00265         if(vecLBFlen > *d1 || *d1 < 0) *d1 = vecLBFlen;
00266 
00267         Point3f vecRBFf = backProject(Point2i(roiRT.x, roiLB.y), f, fx, fy);
00268         float vecRBFlen = norm(vecRBFf);
00269         if(vecRBFlen > *d1) *d1 = vecRBFlen;
00270 
00271         Point3f vecRTFf = backProject(roiRT, f, fx, fy);
00272         float vecRTFlen = norm(vecRTFf);
00273         if(vecRTFlen > *d1) *d1 = vecRTFlen;
00274 
00275         Point3f vecLTFf = backProject(Point2i(roiLB.x, roiRT.y), f, fx, fy);
00276         float vecLTFlen = norm(vecLTFf);
00277         if(vecLTFlen > *d1) *d1 = vecLTFlen;
00278         */
00279         
00280         return true;
00281 }
00282 
00283 
00284 /******************************************************************************
00285  * Calculation of depth of near and far face of BB (perpendicular with all axis).
00286  */
00287 
00288 bool calcNearAndFarFaceDepth(Mat &m, float f, float *z1, float *z2)
00289 {    
00290     // Get statistics of depth in ROI
00291     float mean, stdDev;
00292     if(!calcStats(m, &mean, &stdDev)) {
00293        return false;
00294     }
00295     
00296     // The near and far faces of BB are positioned in the same distance
00297     // (given by the depth standard deviation) from the depth mean.
00298     *z1 = mean - stdDev; // Depth of the near BB face
00299     *z2 = mean + stdDev; // Depth of the far BB face
00300     
00301     // TODO: The following check can be done only after conversion the focal
00302     // length to the same units as the depth values have (= meters)
00303     // ----------
00304     // There cannot be any object whose depth is smaller than the focal length
00305     // => if we have obtained such value, set the value to be equal to f.
00306     /*
00307     *z1 = (*z1 > f) ? *z1 : f;
00308     */
00309     
00310     return true;
00311 }
00312 
00313 
00314 /******************************************************************************
00315  * Bounding box estimation.
00316  */
00317 
00318 bool estimateBB(const ros::Time& stamp,
00319                 const point2_t& p1, const point2_t& p2, int mode,
00320                 Point3f& bbLBF, Point3f& bbRBF,
00321                 Point3f& bbRTF, Point3f& bbLTF,
00322                 Point3f& bbLBB, Point3f& bbRBB,
00323                 Point3f& bbRTB, Point3f& bbLTB
00324                 )
00325 {
00326     // Set estimation mode. If it is not specified in the request or the value
00327     // is not valid => set MODE1 (== 1) as default estimation mode.
00328     estimationMode = mode;
00329     if(estimationMode <= 0 || estimationMode > 3) {
00330         estimationMode = 1;
00331     }
00332     
00333     Mat depthMap;
00334 
00335     // Read the messages from cache (the latest ones before the request timestamp)
00336     //--------------------------------------------------------------------------
00337     sensor_msgs::CameraInfoConstPtr camInfo = camInfoCache.getElemBeforeTime(stamp);
00338     if(camInfo == 0) {
00339         ROS_ERROR("Cannot calculate the bounding box. "
00340                   "No frames were obtained before the request time.");
00341         return false;
00342     }
00343     
00344     // Subscription variant #1 - there is an Image message with a depth map
00345     //----------------------
00346     if(subVariant == SV_1) {
00347         sensor_msgs::ImageConstPtr depth = depthCache.getElemBeforeTime(stamp);
00348 
00349         // Get depth from the message
00350         try {
00351             cv_bridge::CvImagePtr cvDepth;
00352             cvDepth = cv_bridge::toCvCopy(depth);
00353             cvDepth->image.convertTo(depthMap, CV_32F);
00354         }
00355         catch (cv_bridge::Exception& e) {
00356           ROS_ERROR("cv_bridge exception: %s", e.what());
00357           return false;
00358         }
00359         
00360         // Convert the depth coming in milimeters to meters
00361         depthMap *= 1.0/1000.0;
00362     }
00363     
00364     // Subscription variant #2 - the depth map must be created from a point clound
00365     //----------------------
00366     else if(subVariant == SV_2) {
00367         sensor_msgs::PointCloud2ConstPtr pointCloud = pointCloudCache.getElemBeforeTime(stamp);
00368     
00369         // Convert the sensor_msgs/PointCloud2 data to depth map
00370         // At first convert to pcl/PointCloud
00371         pcl::PointCloud<pcl::PointXYZ> cloud;
00372         pcl::fromROSMsg (*pointCloud, cloud);
00373         
00374         depthMap = Mat(cloud.height, cloud.width, CV_32F);
00375         
00376         for(int y = 0; y < (int)cloud.height; y++) {
00377             for(int x = 0; x < (int)cloud.width; x++) {
00378                 float z = cloud.points[y * cloud.width + x].z;
00379                 if(cvIsNaN(z)) z = 0;
00380                 depthMap.at<float>(y, x) = z;
00381             }
00382         }
00383     }
00384     else {
00385         ROS_ERROR("Unknown subscription variant!");
00386         return false;
00387     }
00388     
00389     // Obtain the corresponding transformation from camera to world coordinate system
00390     //--------------------------------------------------------------------------
00391     tf::StampedTransform sensorToWorldTf;
00392     camFrameId = camInfo->header.frame_id;
00393     try {
00394         tfListener->waitForTransform(camFrameId, sceneFrameId,
00395                                      camInfo->header.stamp, ros::Duration(2.0));
00396 //                                   camInfo->header.stamp, ros::Duration(0.2));
00397         tfListener->lookupTransform(camFrameId, sceneFrameId,
00398                                     camInfo->header.stamp, sensorToWorldTf);
00399     }
00400     catch(tf::TransformException& ex) {
00401         string errorMsg = String("Transform error: ") + ex.what();
00402         ROS_ERROR("%s", errorMsg.c_str());
00403         return false;
00404     }
00405     
00406     // Width and height of the depth image
00407     int width = depthMap.cols;
00408     int height = depthMap.rows;
00409 
00410     // Get the coordinates of the specified ROI (Region of Interest)
00411     //--------------------------------------------------------------------------
00412     // Determine the left-bottom and the right-top ROI corner.
00413     // It is assumed that the origin (0,0) is in the top-left image corner.
00414     Point2i roiLBi; // Left-bottom corner of ROI (in image coordinates)
00415     Point2i roiRTi; // Right-top corner of ROI (in image coordinates)
00416     
00417     if(p1[0] < p2[0]) {roiLBi.x = p1[0]; roiRTi.x = p2[0];}
00418     else {roiLBi.x = p2[0]; roiRTi.x = p1[0];}
00419     
00420     if(p1[1] < p2[1]) {roiRTi.y = p1[1]; roiLBi.y = p2[1];}
00421     else {roiRTi.y = p2[1]; roiLBi.y = p1[1];}
00422     
00423     // Consider only the part of the ROI which is within the image
00424     roiLBi.x = min(max(roiLBi.x, 0), width);
00425     roiLBi.y = min(max(roiLBi.y, 0), height);
00426     roiRTi.x = min(max(roiRTi.x, 0), width);
00427     roiRTi.y = min(max(roiRTi.y, 0), height);
00428     
00429     // Get depth information in the ROI
00430     Mat roi = Mat(depthMap, Rect(roiLBi.x, roiRTi.y,
00431                                  roiRTi.x - roiLBi.x, roiLBi.y - roiRTi.y));
00432 
00433     // Get the intrinsic camera parameters (needed for back-projection)
00434     //--------------------------------------------------------------------------
00435     Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data());
00436     float cx = (float)K.at<double>(0, 2);
00437     float cy = (float)K.at<double>(1, 2);
00438     float fx = (float)K.at<double>(0, 0);
00439     float fy = (float)K.at<double>(1, 1);
00440     float f = (fx + fy) / 2.0;
00441     
00442     if(fx == 0 || fy == 0 || cx == 0 || cy == 0) {
00443         ROS_ERROR("Intrinsic camera parameters are undefined.");
00444     }
00445     
00446     // Transform the corners of ROI from image coordinates to coordinates
00447     // with center in the middle of the image
00448     // (Notice that the Y axis is flipped! = going downwards)
00449     //--------------------------------------------------------------------------
00450     Point2i roiLB(roiLBi.x - cx, roiLBi.y - cy);
00451     Point2i roiRT(roiRTi.x - cx, roiRTi.y - cy);
00452     
00453     // Vertices (corners) defining the bounding box.
00454     // Each is noted using this template:
00455     // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back}
00456     // (This notation provides more readable code, in comparison with
00457     // e.g. an array with 8 items.)
00458 //    Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB;
00459     
00460     // Vector with pointers to the BB vertices (to be able to iterate over them)
00461     vector<Point3f *> bbVertices(8);
00462     bbVertices[0] = &bbLBF;
00463     bbVertices[1] = &bbRBF;
00464     bbVertices[2] = &bbRTF;
00465     bbVertices[3] = &bbLTF;
00466     bbVertices[4] = &bbLBB;
00467     bbVertices[5] = &bbRBB;
00468     bbVertices[6] = &bbRTB;
00469     bbVertices[7] = &bbLTB;
00470     
00471     // MODE #1
00473     if(estimationMode == MODE1) {
00474 
00475         // Convert depth to distance from origin (0,0,0) (= optical center).
00476         Mat roiD(roi.size(), CV_32F);
00477         for(int i = 0; i < roi.rows; i++) {
00478             for(int j = 0; j < roi.cols; j++) {
00479                 float z = roi.at<float>(i, j);
00480                 Point3f P = backProject(Point2i(roiLB.x + j, roiRT.y + i), z, fx, fy);
00481                 
00482                 // Get the distance from the origin
00483                 roiD.at<float>(i, j) = norm(P);
00484             }
00485         }
00486 
00487         // Get distance from origin to the front and back face vertices
00488         float d1, d2;
00489         if(!calcNearAndFarFaceDistance(roiD, fx, fy, roiLB, roiRT, &d1, &d2)) {
00490             return false;
00491         }
00492 
00493         // Front-face vertices of BB
00494         // Each of them is obtained by construction of a vector from origin with
00495         // length = d1 in direction of the corresponding ROI corner.
00496         //----------------------------------------------------------------------
00497         Point3f vecLBF = backProject(roiLB, 1, fx, fy);
00498         bbLBF = vecLBF * (d1 / (float)norm(vecLBF));
00499         
00500         Point3f vecRBF = backProject(Point2i(roiRT.x, roiLB.y), 1, fx, fy);
00501         bbRBF = vecRBF * (d1 / (float)norm(vecRBF));
00502         
00503         Point3f vecRTF = backProject(roiRT, 1, fx, fy);
00504         bbRTF = vecRTF * (d1 / (float)norm(vecRTF));
00505         
00506         Point3f vecLTF = backProject(Point2i(roiLB.x, roiRT.y), 1, fx, fy);
00507         bbLTF = vecLTF * (d1 / (float)norm(vecLTF));
00508         
00509         // Back-face vertices of BB
00510         // These vertices are obtained by translation of the corresponding
00511         // front face vertices by vector vecFFtoBF, which is the vector from the
00512         // intersection of the viewing frustum axis (A) and the circle with radius
00513         // d1 to the intersection of A and the circle with radius d2.
00514         //----------------------------------------------------------------------
00515         // Calculate an unit vector pointing to the center of the front face of BB
00516         Point3f vecMid = bbLBF + bbRBF + bbRTF + bbLTF;
00517         vecMid *= 1.0 / (float)norm(vecMid);
00518         
00519         // Calculate a vector from the front face to the back face (perpendicular
00520         // to them). We know that this vector must have the same direction as
00521         // vecMid, we only don't know its magnitude. We can express the magnitude
00522         // (k) from this equation: d2 = |vecLBF + k*vecMid|, which is describing
00523         // the geometry of our problem. After expressing k we get a quadratic
00524         // equation: a*k^2 + b*k + c = 0, where:
00525         // a = vecMid.x*vecMid.x + vecMid.y*vecMid.y + vecMid.z*vecMid.z = 1
00526         // b = 2*(bbLBF.x*vecMid.x + bbLBF.y*vecMid.y + bbLBF.z*vecMid.z)
00527         // c = d1*d1 - d2*d2
00528         // Discriminant D is always positive, because c <= 0, since d2 >= d1, so
00529         // we know that the equation has at least one solution. We do not
00530         // want to flip the direction of vecMid, thus we can observe that we are
00531         // interested only in the positiove solution (the bigger one).
00532         float b = 2.0*(bbLBF.x*vecMid.x + bbLBF.y*vecMid.y + bbLBF.z*vecMid.z);
00533         float c = d1*d1 - d2*d2;
00534         float D = b*b - 4.0*c;
00535         float k = (-b + sqrt(D)) / 2.0;
00536         Point3f vecFFtoBF = k * vecMid;
00537 
00538         // Back-face vertices
00539         bbLBB = bbLBF + vecFFtoBF;
00540         bbRBB = bbRBF + vecFFtoBF;
00541         bbRTB = bbRTF + vecFFtoBF;
00542         bbLTB = bbLTF + vecFFtoBF;
00543     }
00544     
00545     // MODE #2
00547     // Only bbLBF and bbRTF are calculated in this mode. The rest of the BB
00548     // vertices is expressed using these points (it is possible because the
00549     // resulting BB is parallel with all axis in this mode).
00550     else if(estimationMode == MODE2) {
00551         
00552         // Get depth of near and far face of BB
00553         float z1, z2;
00554         if(!calcNearAndFarFaceDepth(roi, f, &z1, &z2)) {
00555             return false;
00556         }
00557 
00558         // If necessary, flip axis (this is done to unify the calculation in
00559         // different situations). There can appear these situations (#1 and #2
00560         // can appear simultaneously):
00561         // 1) The X coordinates of both specified ROI corners are positive
00562         // => we flip them to be negative (the resulting 3D points will be then
00563         // flipped back). We want that, because we want bbLBF to be on the border
00564         // of viewing frustum - calculation for this case is implemented.
00565         // 2) The Y coordinates of both specified ROI corners are negative
00566         // => we flip them to be positive (the resulting 3D points will be then
00567         // flipped back). We want that, because we want bbRTB to be on the border
00568         // of viewing frustum - calculation for this case is implemented.
00569         // 3) If one X (Y) coordinate is negative and one positive, we know
00570         // that the X (Y) coordinates of the back BB face projection won't
00571         // be outside the range given by the X (Y) coordinates of the front BB
00572         // face projection, thus the calculation is the same as in MODE #3 in
00573         // this situation.
00574         //-----------------------------
00575         // Copy the corners of ROI (because they may be modified to unify the
00576         // calculation)
00577         Point2i roiLBm = roiLB;
00578         Point2i roiRTm = roiRT;
00579         
00580         bool flippedX = false;
00581         bool flippedY = false;
00582         
00583         // If both corners of ROI have positive X-coordinate => flip X-axis.
00584         if(roiLBm.x > 0) {
00585             roiLBm.x = -roiRT.x;
00586             roiRTm.x = -roiLB.x;
00587             flippedX = true;
00588         }
00589         
00590         // If both corners of ROI have negative Y-coordinate => flip Y-axis.
00591         if(roiLBm.y < 0) {
00592             roiLBm.y = -roiRT.y;
00593             roiRTm.y = -roiLB.y;
00594             flippedY = true;
00595         }
00596         
00597         // X coordinates of BB vertices
00598         //-----------------------------
00599         if(roiLBm.x <= 0 && roiRTm.x >= 0) {
00600             bbLBF.x = (roiLBm.x * z1) / fx;
00601             bbRTB.x = (roiRTm.x * z1) / fx;
00602         }
00603         else {
00604             bbLBF.x = (roiLBm.x * z1) / fx;
00605             bbRTB.x = (roiRTm.x * z2) / fx;
00606         
00607             // Degenerate case #1 - the back-projection of the left ROI corner
00608             // is on the right of the back-projection of the right ROI corner
00609             // => set X-coord of both to the middle value.
00610             if(bbLBF.x > bbRTB.x) {
00611                 float midX = (bbLBF.x + bbRTB.x) / 2.0;
00612                 bbLBF.x = midX;
00613                 bbRTB.x = midX;
00614                 z1 = (midX * fx) / roiLBm.x;
00615                 z2 = (midX * fx) / roiRTm.x;
00616             }
00617         }
00618         
00619         // Y coordinates of BB vertices
00620         //-----------------------------
00621         if(roiLBm.y >= 0 && roiRTm.y <= 0) {
00622             bbLBF.y = (roiLBm.y * z1) / fy;
00623             bbRTB.y = (roiRTm.y * z1) / fy;
00624         }
00625         else {
00626             bbLBF.y = (roiLBm.y * z1) / fy;
00627             bbRTB.y = (roiRTm.y * z2) / fy;
00628             
00629             // Degenerate case #2 - the back-projection of the bottom ROI corner
00630             // is on the top of the back-projection of the top ROI corner.
00631             // => set Y-coord of both to the middle value
00632             if(bbLBF.y < bbRTB.y) {
00633                 float midY = (bbLBF.y + bbRTB.y) / 2.0;
00634                 bbLBF.y = midY;
00635                 bbRTB.y = midY;
00636                 z1 = (midY * fy) / roiLBm.y;
00637                 z2 = (midY * fy) / roiRTm.y;
00638                 
00639                 // Adjust X coordinates to the new depth
00640                 if(roiLBm.x <= 0 && roiRTm.x >= 0) {
00641                     bbLBF.x = (roiLBm.x * z1) / fx;
00642                     bbRTB.x = (roiRTm.x * z1) / fx;
00643                 }
00644                 else {
00645                     bbLBF.x = (roiLBm.x * z1) / fx;
00646                     bbRTB.x = (roiRTm.x * z2) / fx;
00647                 }
00648             }
00649         }
00650         
00651         // Z coordinates of BB vertices
00652         //-----------------------------
00653         bbLBF.z = z1;
00654         bbRTB.z = z2;
00655         
00656         // If any axis was flipped => flip back
00657         if(flippedX) {
00658             float bbLBFx = bbLBF.x;
00659             bbLBF.x = -bbRTB.x;
00660             bbRTB.x = -bbLBFx;
00661         }
00662         if(flippedY) {
00663             float bbLBFy = bbLBF.y;
00664             bbLBF.y = -bbRTB.y;
00665             bbRTB.y = -bbLBFy;
00666         }
00667         
00668         // Get the remaining vertices (in this mode the BB is perpendicular
00669         // with all axis, so we can use the coordinates which have already
00670         // been calculated).
00671         bbRBF = Point3f(bbRTB.x, bbLBF.y, bbLBF.z);
00672         bbRTF = Point3f(bbRTB.x, bbRTB.y, bbLBF.z);
00673         bbLTF = Point3f(bbLBF.x, bbRTB.y, bbLBF.z);
00674         bbLBB = Point3f(bbLBF.x, bbLBF.y, bbRTB.z);
00675         bbRBB = Point3f(bbRTB.x, bbLBF.y, bbRTB.z);
00676         bbLTB = Point3f(bbLBF.x, bbRTB.y, bbRTB.z);
00677     }
00678     
00679     // MODE #3
00681     // Only bbLBF and bbRTF are calculated in this mode. The rest of the BB
00682     // vertices is expressed using these points (it is possible because the
00683     // resulting BB is parallel with all axis in this mode).
00684     else if(estimationMode == MODE3) {
00685             
00686         // Get depth of near and far face of BB
00687         float z1, z2;
00688         if(!calcNearAndFarFaceDepth(roi, f, &z1, &z2)) {
00689             return false;
00690         }
00691     
00692         // Left-bottom-front vertex of BB
00693         bbLBF = Point3f((roiLB.x * z1) / fx, (roiLB.y * z1) / fy, z1);
00694         
00695         // Right-top-back vertex of BB
00696         // X and Y coordinates of the back projected point is obtained using
00697         // the near depth, but its Z coordinate is set to the further
00698         // depth (this is so to ensure perpendicularity of the bounding box).
00699         bbRTB = Point3f((roiRT.x * z1) / fx, (roiRT.y * z1) / fy, z2);
00700         
00701         // Get the remaining vertices (in this mode the BB is perpendicular
00702         // with all axis, so we can use the coordinates which have already
00703         // been calculated).
00704         bbRBF = Point3f(bbRTB.x, bbLBF.y, bbLBF.z);
00705         bbRTF = Point3f(bbRTB.x, bbRTB.y, bbLBF.z);
00706         bbLTF = Point3f(bbLBF.x, bbRTB.y, bbLBF.z);
00707         bbLBB = Point3f(bbLBF.x, bbLBF.y, bbRTB.z);
00708         bbRBB = Point3f(bbRTB.x, bbLBF.y, bbRTB.z);
00709         bbLTB = Point3f(bbLBF.x, bbRTB.y, bbRTB.z);
00710     }
00711 
00712     // Transform the resulting BB vertices to the world coordinates
00713     //--------------------------------------------------------------------------
00714     tf::Transformer t;
00715     t.setTransform(sensorToWorldTf);
00716     
00717     for(int i = 0; i < (int)bbVertices.size(); i++) {
00718         tf::Stamped<tf::Point> vertex;
00719         vertex.frame_id_ = camFrameId;
00720     
00721         vertex.setX(bbVertices[i]->x);
00722         vertex.setY(bbVertices[i]->y);
00723         vertex.setZ(bbVertices[i]->z);
00724         t.transformPoint(sceneFrameId, vertex, vertex);
00725         bbVertices[i]->x = vertex.getX();
00726         bbVertices[i]->y = vertex.getY();
00727         bbVertices[i]->z = vertex.getZ();
00728     }
00729     
00730     
00731     // The resulting BB cannot go under the floor (z = 0). If it does so, then try
00732     // to raise the bottom face of BB so the lowest BB corner is on the floor.
00733     // This process is feasible only if the whole BB top face is above the floor.
00734     //--------------------------------------------------------------------------
00735     Point3f lowestCorner(0, 0, 0);
00736     Point3f vec(0, 0, 0);
00737     bool feasible = true;
00738     bool isSensorFlipped = false; // Indicates if the camera sensor is flipped
00739                                   // (if the top face in the sensor coordinates is also
00740                                   // the top one in the world coordinates or not).
00741     
00742     // The top face in the sensor coordinates is also the top one in the world coordinates
00743     if(bbLBF.z < lowestCorner.z) {
00744         if(bbLTF.z <= 0) feasible = false;
00745         vec = bbLTF - bbLBF;
00746         lowestCorner = bbLBF;
00747     }
00748     if(bbRBF.z < lowestCorner.z && feasible) {
00749         if(bbRTF.z <= 0) feasible = false;
00750         vec = bbRTF - bbRBF;
00751         lowestCorner = bbRBF;
00752     }
00753     if(bbLBB.z < lowestCorner.z && feasible) {
00754         if(bbLTB.z <= 0) feasible = false;
00755         vec = bbLTB - bbLBB;
00756         lowestCorner = bbLBB;
00757     }
00758     if(bbRBB.z < lowestCorner.z && feasible) {
00759         if(bbRTB.z <= 0) feasible = false;
00760         vec = bbRTB - bbRBB;
00761         lowestCorner = bbRBB;
00762     }
00763     
00764     // The top face in the sensor coordinates is the bottom one in the world coordinates
00765     // (sensor is flipped)
00766     if(bbLTF.z < lowestCorner.z) {
00767         if(bbLBF.z <= 0) feasible = false;
00768         isSensorFlipped = true;
00769         vec = bbLBF - bbLTF;
00770         lowestCorner = bbLTF;
00771     }
00772     if(bbRTF.z < lowestCorner.z && feasible) {
00773         if(bbRBF.z <= 0) feasible = false;
00774         isSensorFlipped = true;
00775         vec = bbRBF - bbRTF;
00776         lowestCorner = bbRTF;
00777     }
00778     if(bbLTB.z < lowestCorner.z && feasible) {
00779         if(bbLBB.z <= 0) feasible = false;
00780         isSensorFlipped = true;
00781         vec = bbLBB - bbLTB;
00782         lowestCorner = bbLTB;
00783     }
00784     if(bbRTB.z < lowestCorner.z && feasible) {
00785         if(bbRBB.z <= 0) feasible = false;
00786         isSensorFlipped = true;
00787         vec = bbRBB - bbRTB;
00788         lowestCorner = bbRTB;
00789     }
00790     
00791     if(lowestCorner.z < 0) {
00792         if(!feasible) {
00793             return false;
00794         }
00795         else {
00796             // Compute a vector moving the bottom corners along the BB edges so the
00797             // lowest corner is on the floor.
00798             float scale = (-lowestCorner.z) / vec.z;
00799             vec = vec * scale;
00800             
00801             // Raise the bottom/top corners along the BB edges
00802             if(!isSensorFlipped) {
00803                 bbLBF += vec;
00804                 bbRBF += vec;
00805                 bbLBB += vec;
00806                 bbRBB += vec;
00807             }
00808             else {
00809                 bbLTF += vec;
00810                 bbRTF += vec;
00811                 bbLTB += vec;
00812                 bbRTB += vec;
00813             }
00814         }
00815     }
00816     
00817 
00818     return true;
00819 }
00820 
00821 
00822 /******************************************************************************
00823  * Bounding box pose estimation.
00824  */
00825 
00826 bool estimateBBPose(const Point3f& bbLBF, const Point3f& bbRBF,
00827                     const Point3f& bbRTF, const Point3f& bbLTF,
00828                     const Point3f& bbLBB, const Point3f& bbRBB,
00829                     const Point3f& bbRTB, const Point3f& bbLTB,
00830                     Point3f& position,
00831                     tf::Quaternion& orientation,
00832                     Point3f& scale
00833                     )
00834 {
00835     // Calculate also a representation of the BB given by position, orientation
00836     // and scale
00837     //--------------------------------------------------------------------------
00838     // Save vectors representing the 3 edges emanating from bbLBF. We will align
00839     // them to X, Y and Z axis by a rotation (roll, pitch, yaw), from which is
00840     // then created a quaternion representing the BB orientation. All permutations
00841     // from set of these three edges to set of X, Y and Z axis are considered. The
00842     // one with the smallest sum of roll + pitch + yaw (= rotation which yields
00843     // the BB to be axis-aligned) is selected - we want to describe the BB
00844     // orientation by minimal rotation.
00845     vector<Point3f> edges(3), edgesNorm(3);
00846     edges[0] = bbRBF - bbLBF;
00847     edges[1] = bbLTF - bbLBF;
00848     edges[2] = bbLBB - bbLBF;
00849     
00850     // Normalization of edge vectors
00851     for(int i = 0; i < 3; i++) edgesNorm[i] = edges[i] * (1.0 / norm(edges[i]));
00852     
00853     Point3f rotBest; // The best rotation yielding to axis-aligned BB (roll, pitch and yaw)
00854     float anglesMinSum = -1; // Minimal sum of roll, pitch and yaw
00855     Point3f bbSize; // Size (scale) of the BB, w.r.t. X, Y and Z axis
00856 
00857     // For each permutation (edges -> axis) the smallest rotation which yields
00858     // to the axis-aligned BB is obtained.
00859     // At first, an angle between an edge (the one which is currently selected
00860     // as the one to be aligned with X axis) and X-axis is found. The edges are
00861     // then rotated by this angle. Similar steps are then applied for Y and Z axis.
00862     // These angles then define the BB orientation.
00863     //
00864     // Here are detailed steps to calculation an angle of rotation around X axis
00865     // (the steps for Y and Z axis are analogous):
00866     // 1) Project edges (the ones to be aligned with Y and Z axis; the one to be
00867     //    aligned with X axis is not considered here, because its rotation around
00868     //    X axis won't get us any closer tbtQuaterniono the axis-aligned BB) onto YZ plane
00869     //    and normalize them.
00870     // 2) Angle = arcus cosinus of the dot product of a normalized
00871     //    projected edge and a unit vector describing the corresponding axis.
00872     // 3) The direction correspondence of the vectors is not important,
00873     //    so convert the angle to the range from 0 to PI/2
00874     //    (=> the normalized projected vector can have, after rotation
00875     //    by this angle, the opposite direction than the axis vector).
00876     // 4) Set the direction of rotation (note: rotation is going
00877     //    counter-clockwise around given axis, when the coordinate system is
00878     //    viewed as this axis is going the the eye).
00879     // 5) Take the smaller angle (from the angle aligning one edge to Y axis and
00880     //    the angle aligning the second edge to Z axis).
00881     for(int xi = 0; xi < 3; xi++) {
00882         for(int yi = 0; yi < 3; yi++) {
00883             if(xi == yi) continue;
00884             for(int zi = 0; zi < 3; zi++) {
00885                 if(xi == zi || yi == zi) continue;
00886                 
00887                 Point3f rot; // Current rotation
00888                 vector<Point3f> edgesTmp = edgesNorm; // Local copy of edges
00889 
00890                 // Roll - rotation around X axis
00891                 //----------
00892                 // Step 1:
00893                 Point3f edgeYyz(0, edgesTmp[yi].y, edgesTmp[yi].z);
00894                 Point3f edgeZyz(0, edgesTmp[zi].y, edgesTmp[zi].z);
00895                 edgeYyz *= (1.0 / norm(edgeYyz));
00896                 edgeZyz *= (1.0 / norm(edgeZyz));
00897                 // Step 2:
00898                 float rollY = acos(edgeYyz.y);
00899                 float rollZ = acos(edgeZyz.z);
00900                 // Step 3:
00901                 if(rollY > PI/2.0) rollY = -PI + rollY;
00902                 if(rollZ > PI/2.0) rollZ = -PI + rollZ;
00903                 // Step 4:
00904                 if(edgeYyz.z < 0) rollY = -rollY;
00905                 if(edgeZyz.y > 0) rollZ = -rollZ;
00906                 // Step 5:
00907                 rot.x = (fabs(rollY) < fabs(rollZ)) ? -rollY : -rollZ;
00908 
00909                 // Rotate edges around X axis
00910                 for(int m = 0; m < 3; m++) {
00911                         edgesTmp[m] = Point3f(
00912                                             edgesTmp[m].x,
00913                                             cos(rot.x)*edgesTmp[m].y - sin(rot.x)*edgesTmp[m].z,
00914                                     sin(rot.x)*edgesTmp[m].y + cos(rot.x)*edgesTmp[m].z);
00915                 }
00916 
00917                 // Pitch - rotation around Y axis
00918                 //----------
00919                 // Step 1:
00920                 // Project edges (the ones to be aligned with X and Z axis) onto XZ plane
00921                 Point3f edgeXxz(edgesTmp[xi].x, 0, edgesTmp[xi].z);
00922                 Point3f edgeZxz(edgesTmp[zi].x, 0, edgesTmp[zi].z);
00923                 edgeXxz *= (1.0 / norm(edgeXxz));
00924                 edgeZxz *= (1.0 / norm(edgeZxz));
00925                 // Step 2:
00926                 float pitchX = acos(edgeXxz.x);
00927                 float pitchZ = acos(edgeZxz.z);
00928                 // Step 3:
00929                 if(pitchX > PI/2.0) pitchX = -PI + pitchX;
00930                 if(pitchZ > PI/2.0) pitchZ = -PI + pitchZ;
00931                 // Step 4:
00932                 if(edgeXxz.z > 0) pitchX = -pitchX;
00933                 if(edgeZxz.x < 0) pitchZ = -pitchZ;
00934                 // Step 5:
00935                 rot.y = (fabs(pitchX) < fabs(pitchZ)) ? -pitchX : -pitchZ;
00936 
00937                 // Rotate edges around Y axis
00938                 for(int m = 0; m < 3; m++) {
00939                         edgesTmp[m] = Point3f(
00940                                             cos(rot.y)*edgesTmp[m].x + sin(rot.y)*edgesTmp[m].z,
00941                                             edgesTmp[m].y,
00942                                             -sin(rot.y)*edgesTmp[m].x + cos(rot.y)*edgesTmp[m].z);
00943                 }
00944 
00945                 // Yaw - rotation around Z axis
00946                 //----------
00947                 // Step 1:
00948                 // Project edges (the ones to be aligned with X and Y axis) onto XY plane
00949                 Point3f edgeXxy(edgesTmp[xi].x, edgesTmp[xi].y, 0);
00950                 Point3f edgeYxy(edgesTmp[yi].x, edgesTmp[yi].y, 0);
00951                 edgeXxy *= (1.0 / norm(edgeXxy));
00952                 edgeYxy *= (1.0 / norm(edgeYxy));
00953                 // Step 2:
00954                 float yawX = acos(edgeXxy.x);
00955                 float yawY = acos(edgeYxy.y);
00956                 // Step 3:
00957                 if(yawX > PI/2.0) yawX = -PI + yawX;
00958                 if(yawY > PI/2.0) yawY = -PI + yawY;
00959                 // Step 4:
00960                 if(edgeXxy.y < 0) yawX = -yawX;
00961                 if(edgeYxy.x > 0) yawY = -yawY;
00962                 // Step 5:
00963                 rot.z = (fabs(yawX) < fabs(yawY)) ? -yawX : -yawY;
00964 
00965                 // Evaluate the actual rotation - the smaller sum of angles, the better
00966                 //--------------------------------------------------------------
00967                 float actualSum = fabs(rot.x) + fabs(rot.y) + fabs(rot.z);
00968                 if(anglesMinSum == -1 || anglesMinSum > actualSum) {
00969                         anglesMinSum = actualSum;
00970                         rotBest = rot;
00971 
00972                     // Calculate size (scale) of tbtQuaternionhe BB, w.r.t. X, Y and Z axis
00973                     // (using the current permutation)
00974                         bbSize.x = norm(edges[xi]);
00975                         bbSize.y = norm(edges[yi]);
00976                         bbSize.z = norm(edges[zi]);
00977                 }
00978             }
00979         }
00980     }
00981     
00982     // Create a quaternion representing the calculated rotation
00983     //--------------------------------------------------------------------------
00984     orientation = tf::createQuaternionFromRPY(-rotBest.x, -rotBest.y, -rotBest.z);
00985     
00986     // Get the center of mass of the BB (in world coordinates)
00987     //--------------------------------------------------------------------------
00988 //    position = Point3f(0, 0, 0);
00989 //    for(int i = 0; i < (int)bbVertices.size(); i++) {
00990 //        position += *(bbVertices[i]);
00991 //    }
00992     position = bbLBF;
00993     position += bbRBF;
00994     position += bbRTF;
00995     position += bbLTF;
00996     position += bbLBB;
00997     position += bbRBB;
00998     position += bbRTB;
00999     position += bbLTB;
01000     position *= 1.0/8.0;
01001 
01002     // Scale
01003     //--------------------------------------------------------------------------
01004     scale.x = bbSize.x;
01005     scale.y = bbSize.y;
01006     scale.z = bbSize.z;
01007     
01008     return true;
01009 }
01010 
01011 
01012 /******************************************************************************
01013  * Image rectangle estimation.
01014  */
01015 
01016 bool estimateRect(const ros::Time& stamp,
01017                   const Point3f& bbLBF, const Point3f& bbRBF,
01018                   const Point3f& bbRTF, const Point3f& bbLTF,
01019                   const Point3f& bbLBB, const Point3f& bbRBB,
01020                   const Point3f& bbRTB, const Point3f& bbLTB,
01021                   point2_t& p1, point2_t& p2
01022                   )
01023 {
01024     // Read the messages from cache (the latest ones before the request timestamp)
01025     //--------------------------------------------------------------------------
01026     sensor_msgs::CameraInfoConstPtr camInfo = camInfoCache.getElemBeforeTime(stamp);
01027     if(camInfo == 0) {
01028         ROS_ERROR("Cannot calculate the image rectangle. "
01029                   "No camera info was obtained before the request time.");
01030         return false;
01031     }
01032     
01033     // Obtain the corresponding transformation from camera to world coordinate system
01034     //--------------------------------------------------------------------------
01035     tf::StampedTransform worldToSensorTf;
01036     camFrameId = camInfo->header.frame_id;
01037     try {
01038         tfListener->waitForTransform(sceneFrameId, camFrameId,
01039                                      camInfo->header.stamp, ros::Duration(2.0));
01040 
01041         tfListener->lookupTransform(sceneFrameId, camFrameId,
01042                                     camInfo->header.stamp, worldToSensorTf);
01043     }
01044     catch(tf::TransformException& ex) {
01045         string errorMsg = String("Transform error: ") + ex.what();
01046         ROS_ERROR("%s", errorMsg.c_str());
01047         return false;
01048     }
01049     
01050     // Width and height of the image
01051     int width = camInfo->width;
01052     int height = camInfo->height;
01053 
01054     // Get the intrinsic camera parameters (needed for back-projection)
01055     //--------------------------------------------------------------------------
01056     Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data());
01057     float cx = (float)K.at<double>(0, 2);
01058     float cy = (float)K.at<double>(1, 2);
01059     float fx = (float)K.at<double>(0, 0);
01060     float fy = (float)K.at<double>(1, 1);
01061     //float f = (fx + fy) / 2.0;
01062     
01063     if(fx == 0 || fy == 0 || cx == 0 || cy == 0) {
01064         ROS_ERROR("Intrinsic camera parameters are undefined.");
01065     }
01066 
01067     // Vertices (corners) defining the bounding box.
01068     // Each is noted using this template:
01069     // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back}
01070     // (This notation provides more readable code, in comparison with
01071     // e.g. an array with 8 items.)
01072 //    Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB;
01073     
01074     // Vector with pointers to the BB vertices (to be able to iterate over them)
01075     vector<const Point3f *> bbVertices(8);
01076     bbVertices[0] = &bbLBF;
01077     bbVertices[1] = &bbRBF;
01078     bbVertices[2] = &bbRTF;
01079     bbVertices[3] = &bbLTF;
01080     bbVertices[4] = &bbLBB;
01081     bbVertices[5] = &bbRBB;
01082     bbVertices[6] = &bbRTB;
01083     bbVertices[7] = &bbLTB;
01084 
01085     // 2D points after projection to the image plane
01086     vector<Point2i> trVertices(8);
01087 
01088     // Transform the BB vertices to the image plane
01089     //--------------------------------------------------------------------------
01090     tf::Transformer t;
01091     t.setTransform(worldToSensorTf);
01092     
01093     for( int i = 0; i < (int)bbVertices.size(); i++ )
01094     {
01095         // Transformation to the camera coordinates
01096         tf::Stamped<tf::Point> vertex;
01097         vertex.frame_id_ = sceneFrameId;
01098         vertex.setX(bbVertices[i]->x);
01099         vertex.setY(bbVertices[i]->y);
01100         vertex.setZ(bbVertices[i]->z);
01101         t.transformPoint(camFrameId, vertex, vertex);
01102         
01103         // Projection to the image plane
01104         trVertices[i] = fwdProject(Point3f(vertex.getX(), vertex.getY(), vertex.getZ()), fx, fy, cx, cy);
01105     }
01106 
01107     // Get the coordinates of the ROI (Region of Interest)
01108     //--------------------------------------------------------------------------
01109 
01110     // Determine the left-bottom and the right-top ROI corner.
01111     // It is assumed that the origin (0,0) is in the top-left image corner.
01112     Point2i ap1(trVertices[0].x, trVertices[0].y);
01113     Point2i ap2(trVertices[0].x, trVertices[0].y);
01114     for( int i = 1; i < (int)bbVertices.size(); i++ )
01115     {
01116         ap1.x = min(trVertices[i].x, ap1.x);
01117         ap1.y = min(trVertices[i].y, ap1.y);
01118         ap2.x = max(trVertices[i].x, ap2.x);
01119         ap2.y = max(trVertices[i].y, ap2.y);
01120     }
01121     
01122     // Consider only the part of the ROI which is within the image
01123     p1[0] = min(max(ap1.x, 0), width);
01124     p1[1] = min(max(ap1.y, 0), height);
01125     p2[0] = min(max(ap2.x, 0), width);
01126     p2[1] = min(max(ap2.y, 0), height);
01127 
01128     return true;
01129 }
01130 
01131 
01132 /******************************************************************************
01133  * 2D convex hull estimation.
01134  */
01135 bool estimate2DConvexHull(const ros::Time& stamp,
01136                           const std::vector<cv::Point3f> points,
01137                           std::vector<cv::Point2i> &convexHull
01138                          )
01139 {
01140     // Read the messages from cache (the latest ones before the request timestamp)
01141     //--------------------------------------------------------------------------
01142     sensor_msgs::CameraInfoConstPtr camInfo = camInfoCache.getElemBeforeTime(stamp);
01143     if(camInfo == 0) {
01144         ROS_ERROR("Cannot calculate the image rectangle. "
01145                   "No camera info was obtained before the request time.");
01146         return false;
01147     }
01148     
01149     // Obtain the corresponding transformation from camera to world coordinate system
01150     //--------------------------------------------------------------------------
01151     tf::StampedTransform worldToSensorTf;
01152     camFrameId = camInfo->header.frame_id;
01153     try {
01154         tfListener->waitForTransform(sceneFrameId, camFrameId,
01155                                      camInfo->header.stamp, ros::Duration(2.0));
01156 
01157         tfListener->lookupTransform(sceneFrameId, camFrameId,
01158                                     camInfo->header.stamp, worldToSensorTf);
01159     }
01160     catch(tf::TransformException& ex) {
01161         string errorMsg = String("Transform error: ") + ex.what();
01162         ROS_ERROR("%s", errorMsg.c_str());
01163         return false;
01164     }
01165 
01166     // Get the intrinsic camera parameters (needed for back-projection)
01167     //--------------------------------------------------------------------------
01168     Mat K = Mat(3, 3, CV_64F, (double *)camInfo->K.data());
01169     float cx = (float)K.at<double>(0, 2);
01170     float cy = (float)K.at<double>(1, 2);
01171     float fx = (float)K.at<double>(0, 0);
01172     float fy = (float)K.at<double>(1, 1);
01173     //float f = (fx + fy) / 2.0;
01174     
01175     if(fx == 0 || fy == 0 || cx == 0 || cy == 0) {
01176         ROS_ERROR("Intrinsic camera parameters are undefined.");
01177     }
01178     
01179     // Transform the BB vertices to the image plane
01180     //--------------------------------------------------------------------------
01181     tf::Transformer t;
01182     t.setTransform(worldToSensorTf);
01183     
01184     // 2D points after projection to the image plane
01185     vector<Point2i> trPoints(points.size());
01186     
01187     for( int i = 0; i < (int)points.size(); i++ )
01188     {
01189         // Transformation to the camera coordinates
01190         tf::Stamped<tf::Point> vertex;
01191         vertex.frame_id_ = sceneFrameId;
01192         vertex.setX(points[i].x);
01193         vertex.setY(points[i].y);
01194         vertex.setZ(points[i].z);
01195         t.transformPoint(camFrameId, vertex, vertex);
01196         
01197         // Projection to the image plane
01198         trPoints[i] = fwdProject(Point3f(vertex.getX(), vertex.getY(), vertex.getZ()), fx, fy, cx, cy);
01199     }
01200 
01201     // Get a convex hull
01202     cv::convexHull(trPoints, convexHull);
01203     
01204     return true;
01205 }
01206 
01207 }


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:07:20