funcs.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Tomas Hodan (xhodan04@stud.fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 25.4.2012 (version 6.0)
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef BB_ESTIMATOR_FUNCS_H
#define BB_ESTIMATOR_FUNCS_H

#include <ros/ros.h>
#include <tf/tf.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>


namespace srs_env_model_percp
{

/*
 * Global variables
 */
static const float PI = 3.1415926535;
const bool DEBUG = false; // If true, verbose outputs are written to console.
//const bool DEBUG = true; // If true, verbose outputs are written to console.

const int CACHE_SIZE = 10;

const int QUEUE_SIZE = 10;

enum subVariantsEnum { SV_NONE = 0, SV_1, SV_2 };

enum estimationModeEnum { MODE1 = 1, MODE2, MODE3 };

typedef boost::array<int16_t, 2> point2_t;


cv::Point3f backProject(cv::Point2i p, float z, float fx, float fy);


cv::Point2i fwdProject(cv::Point3f p, float fx, float fy, float cx, float cy);


bool calcStats(cv::Mat &m, float *mean, float *stdDev);


bool calcNearAndFarFaceDistance(
                cv::Mat &m, float fx, float fy, cv::Point2i roiLB,
                cv::Point2i roiRT, float *d1, float *d2
                );


bool calcNearAndFarFaceDepth(cv::Mat &m, float f, float *z1, float *z2);


bool estimateBB(const ros::Time& stamp,
                const point2_t& p1, const point2_t& p2,
                int mode,
                cv::Point3f& bbLBF, cv::Point3f& bbRBF,
                cv::Point3f& bbRTF, cv::Point3f& bbLTF,
                cv::Point3f& bbLBB, cv::Point3f& bbRBB,
                cv::Point3f& bbRTB, cv::Point3f& bbLTB
                );


bool estimateBBPose(const cv::Point3f& bbLBF, const cv::Point3f& bbRBF,
                    const cv::Point3f& bbRTF, const cv::Point3f& bbLTF,
                    const cv::Point3f& bbLBB, const cv::Point3f& bbRBB,
                    const cv::Point3f& bbRTB, const cv::Point3f& bbLTB,
                    cv::Point3f& position,
                    tf::Quaternion& orientation,
                    cv::Point3f& scale
                    );


bool estimateRect(const ros::Time& stamp,
                  const cv::Point3f& bbLBF, const cv::Point3f& bbRBF,
                  const cv::Point3f& bbRTF, const cv::Point3f& bbLTF,
                  const cv::Point3f& bbLBB, const cv::Point3f& bbRBB,
                  const cv::Point3f& bbRTB, const cv::Point3f& bbLTB,
                  point2_t& p1, point2_t& p2
                  );

bool estimate2DConvexHull(const ros::Time& stamp,
                          const std::vector<cv::Point3f> points,
                          std::vector<cv::Point2i> &convexHull
                         );

}

#endif // BB_ESTIMATOR_FUNCS_H