outlet_tuple.h
Go to the documentation of this file.
00001 /*
00002  *  outlet_tuple.h
00003  *  rectify_outlets
00004  *
00005  *  Created by Victor  Eruhimov on 1/25/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 //*****************************************************************************************
00011 // Warning: this is research code with poor architecture, performance and no documentation!
00012 //*****************************************************************************************
00013 
00014 #include <cstdio>
00015 
00016 #if !defined(_OUTLET_TUPLE_H)
00017 #define _OUTLET_TUPLE_H
00018 
00019 #include <cv.h>
00020 #include <vector>
00021 
00022 #include "one_way_descriptor.h"
00023 #include "one_way_descriptor_base.h"
00024 #include "features.h"
00025 #include "geometric_hash.h"
00026 
00027 inline CvPoint cvPoint(CvPoint2D32f point)
00028 {
00029         return cvPoint(int(point.x), int(point.y));
00030 }
00031 
00032 inline CvPoint rect_center(CvRect rect)
00033 {
00034         return cvPoint(rect.x + rect.width/2, rect.y + rect.height/2);
00035 }
00036 
00037 inline CvPoint2D32f operator +(CvPoint2D32f p1, CvPoint2D32f p2)
00038 {
00039         return cvPoint2D32f(p1.x + p2.x, p1.y + p2.y);
00040 }
00041 
00042 inline CvPoint2D32f operator -(CvPoint2D32f p1, CvPoint2D32f p2)
00043 {
00044         return cvPoint2D32f(p1.x - p2.x, p1.y - p2.y);
00045 }
00046 
00047 inline CvPoint3D32f operator -(CvPoint3D32f p1, CvPoint3D32f p2)
00048 {
00049         return cvPoint3D32f(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
00050 }
00051 
00052 inline CvPoint operator*(CvPoint pt, float scalar)
00053 {
00054     return cvPoint(pt.x*scalar, pt.y*scalar);
00055 }
00056 
00057 inline float length(const CvPoint2D32f& p)
00058 {
00059         return sqrt(p.x*p.x + p.y*p.y);
00060 }
00061 
00062 inline float length(const CvPoint3D32f& p)
00063 {
00064         return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
00065 }
00066 
00067 inline CvPoint3D32f operator +(CvPoint3D32f p1, CvPoint3D32f p2)
00068 {
00069         return cvPoint3D32f(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z);
00070 }
00071 
00072 
00073 void readTrainingBase(const char* config_filename, char* outlet_filename,
00074                       char* nonoutlet_filename, std::vector<feature_t>& train_features);
00075 void readCvPointByName(CvFileStorage* fs, CvFileNode* parent, const char* name, CvPoint& pt);
00076 
00077 struct outlet_tuple_t
00078 {
00079         CvPoint2D32f centers[4];
00080         std::vector<CvPoint2D32f> borders[4];
00081         IplImage* tuple_mask;
00082         CvRect roi;
00083 
00084         ~outlet_tuple_t()
00085         {
00086                 cvReleaseImage(&tuple_mask);
00087         }
00088 };
00089 
00090 typedef enum
00091 {
00092     outletWhite = 0,
00093     outletOrange = 1
00094 } outlet_color_t;
00095 
00096 const float default_hole_contrast = 1.1f;
00097 
00098 class outlet_template_t
00099 {
00100 public:
00101         outlet_template_t(int count = 4, const CvPoint2D32f* templ = 0) : 
00102         geometric_matcher(cv::Size(30,30), cv::Point2f(-5,-5), cv::Point2f(5, 5),
00103         PointMatcher::PointMatcherParams(0.5,100.0,10.0,3,5.0))
00104         {
00105         m_hole_contrast = default_hole_contrast;
00106                 initialize(count, templ);
00107         };
00108 
00109         outlet_template_t(const outlet_template_t& outlet_templ) : geometric_matcher(cv::Size(30,30), cv::Point2f(-5,-5), cv::Point2f(5, 5))
00110         {
00111         m_hole_contrast = default_hole_contrast;
00112                 initialize(outlet_templ.get_count(), outlet_templ.get_template());
00113         };
00114 
00115         ~outlet_template_t()
00116         {
00117                 delete []centers;
00118         if(m_base)
00119         {
00120             delete m_base;
00121         }
00122         };
00123 
00124 public:
00125         void initialize(int count, const CvPoint2D32f* templ, outlet_color_t outlet_color = outletOrange)
00126         {
00127                 outlet_count = count;
00128                 centers = new CvPoint2D32f[count];
00129                 if(templ)
00130                 {
00131                         memcpy(centers, templ, count*sizeof(CvPoint2D32f));
00132                 }
00133                 else
00134                 {
00135                         set_default_template();
00136                 }
00137 
00138         m_base = 0;
00139         m_pose_count = 500;
00140         m_patch_size = cvSize(24, 24);
00141         m_outlet_color = outlet_color;
00142 };
00143 
00144         int get_count() const
00145         {
00146                 return outlet_count;
00147         };
00148 
00149         const CvPoint2D32f* get_template() const
00150         {
00151         return centers;
00152         };
00153 
00154     const PointMatcher& getGeometricMatcher() const {return geometric_matcher;};
00155 
00156         void set_default_template()
00157         {
00158                 centers[0] = cvPoint2D32f(0.0f, 0.0f);
00159                 centers[1] = cvPoint2D32f(46.0f, 0.0f);
00160                 centers[2] = cvPoint2D32f(46.15f, 38.7f);
00161                 centers[3] = cvPoint2D32f(-0.15f, 38.7f);
00162         };
00163 
00164     void load_one_way_descriptor_base(CvSize patch_size, int pose_count, const char* train_path,
00165                                   const char* train_config, const char* pca_config, const char* pca_hr_config = 0)
00166     {
00167         m_train_path = train_path;
00168         m_train_config = train_config;
00169         m_pca_config = pca_config;
00170         if(pca_hr_config) m_pca_hr_config = pca_hr_config;
00171         create_one_way_descriptor_base();
00172     }
00173 
00174     void create_one_way_descriptor_base()
00175     {
00176                 //m_pose_count = 50;
00177                 //printf("mpose: %d\n",m_pose_count);
00178         m_base = new CvOneWayDescriptorObject(m_patch_size, m_pose_count, m_train_path.c_str(),
00179                                             m_pca_config.c_str(), m_pca_hr_config.c_str(),
00180                                             m_pca_desc_config.c_str());
00181         char outlet_filename[1024];
00182         char nonoutlet_filename[1024];
00183         char train_config_filename[1024];
00184         std::vector<feature_t> train_features;
00185         sprintf(train_config_filename, "%s/%s", m_train_path.c_str(), m_train_config.c_str());
00186         readTrainingBase(train_config_filename, outlet_filename, nonoutlet_filename, train_features);
00187         m_base->SetLabeledFeatures(train_features);
00188 
00189         char train_image_filename[1024];
00190         char train_image_filename1[1024];
00191         sprintf(train_image_filename, "%s/%s", m_train_path.c_str(), outlet_filename);
00192         sprintf(train_image_filename1, "%s/%s", m_train_path.c_str(), nonoutlet_filename);
00193 
00194         LoadTrainingFeatures(*m_base, train_image_filename, train_image_filename1);
00195     }
00196 
00197     void initialize_geometric_hash();
00198 
00199     const CvOneWayDescriptorObject* get_one_way_descriptor_base() const {return m_base;};
00200 
00201     outlet_color_t get_color() const {return m_outlet_color;};
00202     void get_holes_3d(CvPoint3D32f* holes) const;
00203     void get_holes_3d(std::vector<cv::Point3f>& holes) const;
00204     void get_holes_2d(CvPoint2D32f* holes) const;
00205 
00206     void save(const char* filename);
00207     int load(const char* path);
00208 
00209     float GetHoleContrast() const {return m_hole_contrast;}
00210 
00211 protected:
00212         int outlet_count;
00213         CvPoint2D32f* centers;
00214     CvOneWayDescriptorObject* m_base;
00215 
00216     std::string m_train_path;
00217     std::string m_train_config;
00218     std::string m_pca_config;
00219     std::string m_pca_hr_config;
00220     std::string m_pca_desc_config;
00221     CvSize m_patch_size;
00222     int m_pose_count;
00223     outlet_color_t m_outlet_color;
00224 
00225     PointMatcher geometric_matcher;
00226 
00227     float m_hole_contrast; // hole minimum contrast
00228 };
00229 
00230 typedef struct
00231 {
00232         CvPoint2D32f center;
00233         float angle;
00234         int idx;
00235         CvSeq* seq;
00236 } outlet_elem_t;
00237 
00238 
00239 CvPoint2D32f calc_center(CvSeq* seq);
00240 int find_dir(const CvPoint2D32f* dir, int xsign, int ysign);
00241 int order_tuple(CvPoint2D32f* centers);
00242 int order_tuple2(std::vector<outlet_elem_t>& tuple);
00243 
00244 void map_point_homography(CvPoint2D32f point, CvMat* homography, CvPoint2D32f& result);
00245 void map_vector_homography(const std::vector<CvPoint2D32f>& points, CvMat* homography, std::vector<CvPoint2D32f>& result);
00246 CvPoint2D32f calc_center(const std::vector<CvPoint2D32f>& points);
00247 
00248 
00249 
00250 // find_outlet_centroids: finds a tuple of 4 orange outlets.
00251 // Input parameters:
00252 //      src: input color image
00253 //      outlet_tuple: the output outlet tuple
00254 //      output_path: a path for logging the results
00255 //      filename: filename for logging the results
00256 // Returns 1 if a tuple is found, 0 otherwise.
00257 int find_outlet_centroids(IplImage* img, outlet_tuple_t& outlet_tuple, const char* output_path, const char* filename);
00258 
00259 
00260 int find_tuple(std::vector<outlet_elem_t>& candidates, CvPoint2D32f* centers);
00261 
00262 // calc_origin_scale: function for calculating the 3D position of origin (center of the upper left outlet)
00263 // and scale (pixels to mm in rectified image)
00264 // Input parameters:
00265 //      centers: tuple outlet centers (an array with 4 elements)
00266 //      map_matrix: homography matrix from a camera image to a rectified image
00267 //      origin: output coordinates of origin
00268 //      scale: output scale in horizontal and vertical direction. A vector p = (p.x, p.y) in a rectified image
00269 //              in outlets plane corresponds to p1 = (p.x*scale.x, p.y*scale.y, 0) in 3D.
00270 void calc_origin_scale(const CvPoint2D32f* centers, CvMat* map_matrix, CvPoint3D32f* origin, CvPoint2D32f* scale);
00271 
00272 // calc_outlet_homography: calculates homography for rectifying the outlet tuple image
00273 // Input parameters:
00274 //      centers: outlet centers
00275 //      map_matrix: output homography matrix (map from a camera image to a rectified image)
00276 //  templ: outlet template
00277 //      inverse_map_matrix: optional inverse homography matrix
00278 void calc_outlet_homography(const CvPoint2D32f* centers, CvMat* map_matrix,
00279                                                         const outlet_template_t& templ = outlet_template_t(),
00280                                                         CvMat* inverse_map_matrix = 0);
00281 
00282 void calc_outlet_homography(const CvPoint2D32f* centers, CvSize src_size, CvMat* map_matrix, CvSize* dst_size);
00283 
00284 int calc_image_homography(IplImage* src, CvMat* map_matrix, CvSize* dst_size, CvPoint2D32f* hor_dir = 0,
00285                                                   CvPoint3D32f* origin = 0, CvPoint2D32f* scale = 0, const char* output_path = 0,
00286                                                   const char* filename = 0, CvPoint2D32f* _centers = 0);
00287 IplImage* find_templates(IplImage* img, IplImage* templ);
00288 
00289 void calc_bounding_rect(int count, const CvRect* rects, CvRect& bounding_rect);
00290 
00291 void generate_object_points_2x1(CvPoint3D32f* points);
00292 void generate_object_points_2x1(CvPoint2D32f* points);
00293 
00294 // calc_camera_pose: pose estimation function. Assumes that camera calibration was done with real length.
00295 // Input parameters:
00296 //      intrinsic_mat: matrix of camera intrinsic parameters
00297 //      distortion_coeffs: vector of distortion coefficients. If 0, all coefficients are assumed 0.
00298 //  point_count: the number of points in object_points and image_points arrays
00299 //  object_points: the 3D coordinates of the detected points
00300 //  image_points: image coordinates of the detected points
00301 //      rotation_vector, translation_vector: output transformation from outlet coordinate system (origin in the
00302 //      center of the upper left outlet) to camera coordinate system
00303 void calc_camera_pose(CvMat* intrinsic_mat, CvMat* distortion_coeffs, int point_count, const CvPoint3D32f*object_points,
00304                       const CvPoint2D32f* image_points, CvMat* rotation_vector, CvMat* translation_vector);
00305 
00306 void calc_camera_pose_2x1(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const CvPoint2D32f* centers,
00307                           CvMat* rotation_vector, CvMat* translation_vector);
00308 
00309 void calc_camera_pose_2x2(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const CvPoint2D32f* centers,
00310                           CvMat* rotation_vector, CvMat* translation_vector);
00311 
00312 void calc_camera_outlet_pose(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const outlet_template_t& outlet_template,
00313                              const CvPoint2D32f* image_points, CvMat* rotat, CvMat* translation_vector);
00314 
00315 CvSeq* close_seq(CvSeq* seq, CvMemStorage* storage, int closure_dist, IplImage* workspace);
00316 
00317 
00318 #endif //_OUTLET_TUPLE_H


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Nov 28 2013 11:46:23