funcs.h
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
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 #pragma once
00029 #ifndef BB_ESTIMATOR_FUNCS_H
00030 #define BB_ESTIMATOR_FUNCS_H
00031 
00032 #include <ros/ros.h>
00033 #include <tf/tf.h>
00034 
00035 #include <sensor_msgs/Image.h>
00036 #include <sensor_msgs/CameraInfo.h>
00037 #include <sensor_msgs/image_encodings.h>
00038 
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <opencv2/imgproc/imgproc.hpp>
00041 
00042 #include <pcl/io/pcd_io.h>
00043 #include <pcl/point_types.h>
00044 
00045 
00046 namespace srs_env_model_percp
00047 {
00048 
00049 /*
00050  * Global variables
00051  */
00052 static const float PI = 3.1415926535;
00053 const bool DEBUG = false; // If true, verbose outputs are written to console.
00054 //const bool DEBUG = true; // If true, verbose outputs are written to console.
00055 
00059 const int CACHE_SIZE = 10;
00060 
00064 const int QUEUE_SIZE = 10;
00065 
00069 enum subVariantsEnum { SV_NONE = 0, SV_1, SV_2 };
00070 
00087 enum estimationModeEnum { MODE1 = 1, MODE2, MODE3 };
00088 
00092 typedef boost::array<int16_t, 2> point2_t;
00093 
00094 
00119 cv::Point3f backProject(cv::Point2i p, float z, float fx, float fy);
00120 
00121 
00135 cv::Point2i fwdProject(cv::Point3f p, float fx, float fy, float cx, float cy);
00136 
00137 
00150 bool calcStats(cv::Mat &m, float *mean, float *stdDev);
00151 
00152 
00167 bool calcNearAndFarFaceDistance(
00168                 cv::Mat &m, float fx, float fy, cv::Point2i roiLB,
00169                 cv::Point2i roiRT, float *d1, float *d2
00170                 );
00171 
00172 
00184 bool calcNearAndFarFaceDepth(cv::Mat &m, float f, float *z1, float *z2);
00185 
00186 
00195 bool estimateBB(const ros::Time& stamp,
00196                 const point2_t& p1, const point2_t& p2,
00197                 int mode,
00198                 cv::Point3f& bbLBF, cv::Point3f& bbRBF,
00199                 cv::Point3f& bbRTF, cv::Point3f& bbLTF,
00200                 cv::Point3f& bbLBB, cv::Point3f& bbRBB,
00201                 cv::Point3f& bbRTB, cv::Point3f& bbLTB
00202                 );
00203 
00204 
00213 bool estimateBBPose(const cv::Point3f& bbLBF, const cv::Point3f& bbRBF,
00214                     const cv::Point3f& bbRTF, const cv::Point3f& bbLTF,
00215                     const cv::Point3f& bbLBB, const cv::Point3f& bbRBB,
00216                     const cv::Point3f& bbRTB, const cv::Point3f& bbLTB,
00217                     cv::Point3f& position,
00218                     tf::Quaternion& orientation,
00219                     cv::Point3f& scale
00220                     );
00221 
00222 
00230 bool estimateRect(const ros::Time& stamp,
00231                   const cv::Point3f& bbLBF, const cv::Point3f& bbRBF,
00232                   const cv::Point3f& bbRTF, const cv::Point3f& bbLTF,
00233                   const cv::Point3f& bbLBB, const cv::Point3f& bbRBB,
00234                   const cv::Point3f& bbRTB, const cv::Point3f& bbLTB,
00235                   point2_t& p1, point2_t& p2
00236                   );
00237 
00245 bool estimate2DConvexHull(const ros::Time& stamp,
00246                           const std::vector<cv::Point3f> points,
00247                           std::vector<cv::Point2i> &convexHull
00248                          );
00249 
00250 }
00251 
00252 #endif // BB_ESTIMATOR_FUNCS_H
00253 


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:56