outlet_pose_estimation: outlet_pose_estimation: outlet

00001 /*
00002  *  outlet_model.h
00003  *  outlet_model
00004  *
00005  *  Created by Victor  Eruhimov on 12/22/08.
00006  *  Copyright 2008 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 #if !defined(_OUTLET_MODEL_H)
00015 #define _OUTLET_MODEL_H
00016 
00017 #include <vector>
00018 #include <map>
00019 
00020 #include <cv.h>
00021 #include <ml.h>
00022 
00023 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00024 
00025 typedef struct
00026 {
00027         CvRect bbox;
00028         float weight;
00029 } outlet_feature_t;
00030 
00031 inline CvPoint feature_center(outlet_feature_t feature)
00032 {
00033         return cvPoint(feature.bbox.x + feature.bbox.width/2, feature.bbox.y + feature.bbox.height/2);
00034 }
00035 
00036 struct outlet_t
00037 {
00038         CvSeq* outlet;
00039         CvPoint hole1;
00040         CvPoint hole2;
00041     CvPoint ground_hole;
00042         outlet_feature_t feature1;
00043         outlet_feature_t feature2;
00044         CvPoint3D32f coord_hole1;
00045         CvPoint3D32f coord_hole2;
00046         CvPoint3D32f coord_hole_ground;
00047         float weight;
00048         float weight_orient;
00049         bool hole1_detected;
00050         bool hole2_detected;
00051         bool ground_hole_detected;
00052 
00053     cv::Point2f hole1f;
00054     cv::Point2f hole2f;
00055     cv::Point2f hole_groundf;
00056     bool is_subpixel;
00057 
00058     outlet_t() : is_subpixel(false) {};
00059 };
00060 
00061 inline float outlet_size(outlet_t outlet);
00062 CvRect outlet_rect(outlet_t outlet);
00063 
00064 #define __max MAX
00065 #define __min MIN
00066 
00067 // detect_outlet: detects outlets in an image.
00068 // Input parameters:
00069 //      src: input color image
00070 //      features: output array of features detected in the image
00071 //      outlets: output array of outlets detected in the image
00072 //      outlet_tuple: the optional tuple found in the image. It is used to filter out
00073 //              features outside of the tuple mask or close to its boundary.
00074 //      output_path: path for logging the results
00075 //      filename: filename for logging the results
00076 void detect_outlets(IplImage* src, std::vector<outlet_feature_t>& features, std::vector<outlet_t>& outlets,
00077                                         outlet_tuple_t* outlet_tuple, const char* output_path = 0, const char* filename = 0);
00078 
00079 inline void cvRectangle(IplImage* img, CvRect rect, CvScalar color, int thickness)
00080 {
00081         cvRectangle(img, cvPoint(rect.x, rect.y), cvPoint(rect.x + rect.width, rect.y + rect.height),
00082                                 color, thickness);
00083 }
00084 
00085 inline CvRect fit_rect_roi(CvRect rect, CvRect roi)
00086 {
00087         CvRect fit = rect;
00088         fit.x = MAX(fit.x, roi.x);
00089         fit.y = MAX(fit.y, roi.y);
00090         fit.width = MIN(fit.width, roi.x + roi.width - fit.x - 1);
00091         fit.height = MIN(fit.height, roi.y + roi.height - fit.y - 1);
00092         assert(fit.width > 0);
00093         assert(fit.height > 0);
00094         return(fit);
00095 }
00096 
00097 inline CvRect fit_rect(CvRect rect, IplImage* img)
00098 {
00099         CvRect roi = cvGetImageROI(img);
00100         return fit_rect_roi(rect, roi);
00101 }
00102 
00103 inline CvRect double_rect(CvRect small_rect)
00104 {
00105         return cvRect(small_rect.x - small_rect.width/2, small_rect.y - small_rect.height/2,
00106                                   small_rect.width*2, small_rect.height*2);
00107 }
00108 
00109 inline int is_point_inside_rect(CvRect rect, CvPoint point)
00110 {
00111         if(point.x >= rect.x && point.y >= rect.y &&
00112            point.x <= rect.x + rect.width && point.y <= rect.y + rect.height)
00113         {
00114                 return 1;
00115         }
00116         else
00117         {
00118                 return 0;
00119         }
00120 }
00121 
00122 CvRect getOutletROI(const std::vector<outlet_t>& outlets);
00123 
00124 int is_point_incenter_roi(const std::vector<CvRect>& rects, CvPoint point);
00125 
00126 //void DrawKeypoints(IplImage* img, std::vector<Keypoint> keypts);
00127 void DrawKeypoints(IplImage* img, std::vector<outlet_feature_t> features);
00128 
00129 typedef std::map<std::string, std::vector<CvRect> > outlet_roi_t;
00130 CvMat* vector2mat(const std::vector<int>& vec);
00131 void read_outlet_roi(const char* filename, outlet_roi_t& outlet_roi);
00132 void extract_intensity_features(IplImage* grey, const std::vector<outlet_feature_t>& keypts, CvMat** mat,
00133                                                                 int equalize_hist = 0,
00134                                                                 const std::vector<int>& labels = std::vector<int>(), const char* buf = 0);
00135 //void calc_labels(const std::vector<CvRect>& rects, const std::vector<Keypoint>& keypts, std::vector<int>& labels);
00136 void calc_labels(const std::vector<CvRect>& rects, const std::vector<outlet_feature_t>& keypts, std::vector<int>& labels);
00137 void FilterPoints(IplImage* grey, std::vector<outlet_feature_t>& keypts, const CvRTrees* rtrees);
00138 void filter_outlets(IplImage* grey, std::vector<outlet_t>& outlets, CvRTrees* rtrees);
00139 
00140 //void outletfarr2keypointarr(const std::vector<outlet_feature_t>& features, std::vector<Keypoint>& keypoints);
00141 //void keypointarr2outletfarr(const std::vector<Keypoint>& keypoints, std::vector<outlet_feature_t>& features);
00142 
00143 void find_outlet_features(IplImage* src, std::vector<outlet_feature_t>& features, const char* filename);
00144 void find_outlet_features_fast(IplImage* src, std::vector<outlet_feature_t>& features, float hole_contrast,
00145                                const char* output_path, const char* filename);
00146 
00147 // generates several perspective distortions of the original outlet and
00148 // extracts intensity values into CvMat format for subsequent learning of a classifier
00149 int generate_outlet_samples(IplImage* grey, outlet_t outlet, int count, CvMat** predictors, const char* filename = 0);
00150 
00151 // train an outlet model by generating perspective distorted outlets
00152 void train_outlet_model(const char* path, const char* config_filename,
00153                                                 const char* roi_filename, const char* forest_filename);
00154 
00155 void write_pr(const char* pr_filename, const char* image_filename, const outlet_roi_t& outlet_roi,
00156                           const std::vector<outlet_t>& outlets);
00157 
00158 void filter_negative_samples(const std::vector<CvRect>& rects, std::vector<outlet_feature_t>& keypts, float fraction);
00159 void calc_contrast_factor(IplImage* grey, CvRect rect, float& contrast, float& variation);
00160 
00161 void select_central_outlets(std::vector<outlet_t>& outlets, int count);
00162 void select_orient_outlets(CvPoint2D32f orientation, std::vector<outlet_t>& outlets, int count = 0);
00163 int select_orient_outlets_ex(IplImage* grey, std::vector<outlet_t>& outlets, const char* filename = 0);
00164 
00165 void draw_outlets(IplImage* temp, const std::vector<outlet_t>& outlets);
00166 void filter_canny(IplImage* grey, std::vector<outlet_feature_t>& features);
00167 
00168 // a temporary solution for finding image homography
00169 int load_homography_map(const char* filename, CvMat** map_matrix);
00170 IplImage* load_match_template_mask(const char* filename);
00171 
00172 CvMat* calc_image_homography(IplImage* src);
00173 
00174 
00175 // uses template matching, currently done offline
00176 void filter_outlets_templ_ex(std::vector<outlet_t>& outlets, CvMat* map_matrix, IplImage* mask);
00177 int filter_outlets_templ(std::vector<outlet_t>& outlets, const char* filename);
00178 int filter_outlets_templmatch(IplImage* src, std::vector<outlet_t>& outlets, IplImage* templ, const char* output_path,
00179                                                           const char* filename = 0, CvMat** homography = 0, CvPoint3D32f* origin = 0, CvPoint2D32f* scale = 0);
00180 
00181 IplImage* calc_tuple_distance_map(IplImage* tuple_mask);
00182 
00183 
00184 // calc_outlet_coords: calculate outlets 3D coordinates in camera coordinate system. The result is stored
00185 // in the input vector of outlet_t objects and can be retrieved by looking into outlet_t::coord_hole* fields or
00186 // by calling get_outlet_coordinates(...).
00187 // Input parameters:
00188 //      outlets: input vector of outlets.
00189 //      map_matrix: homography matrix that maps a camera image into a rectified image.
00190 //      origin, scale: parameters for calculating 3D coordinates of a point in a rectified image.
00191 //              Are calculated by calc_origin_scale(...) function.
00192 //      rotation_vector, translation_vector: vectors for mapping from an outlet coordinate system into
00193 //              a camera coordinate system. Are calculated by calc_camera_pose(...).
00194 int calc_outlet_coords(std::vector<outlet_t>& outlets, CvMat* map_matrix, CvPoint3D32f origin, CvPoint2D32f scale,
00195         CvMat* rotation_vector, CvMat* translation_vector, CvMat* inv_map_matrix = 0);
00196 
00197 void calc_outlet_coords(std::vector<outlet_t>& outlets, const outlet_template_t& outlet_template,
00198                         CvMat* intrinsic_matrix, CvMat* distortion_params);
00199 void calc_outlet_coords_ground(std::vector<outlet_t>& outlets, const outlet_template_t& outlet_template,
00200                         CvMat* intrinsic_matrix, CvMat* distortion_params);
00201 
00202 
00203 void calc_outlet_dist_stat(const std::vector<outlet_t>& outlets, float& mean, float& stddev);
00204 void calc_outlet_tuple_dist_stat(const std::vector<outlet_t>& outlets, float& ground_dist_x1,
00205                                                                  float& ground_dist_x2, float& ground_dist_y);
00206 void calc_outlet_coords(CvMat* rotation_vector, CvMat* translation_vector, const std::vector<cv::Point3f>& object_points, std::vector<outlet_t>& outlets);
00207 
00208 void filter_features_mask(std::vector<outlet_feature_t>& features, IplImage* mask);
00209 void filter_outlets_mask(std::vector<outlet_t>& outlets, IplImage* mask);
00210 void filter_outlets_size(std::vector<outlet_t>& outlets);
00211 
00212 void filter_features_distance_mask(std::vector<outlet_feature_t>& features, IplImage* distance_map);
00213 
00214 int find_origin_chessboard(IplImage* src, CvMat* map_matrix, CvPoint3D32f& origin, float bar_length);
00215 
00216 // filter_outlet_tuple: enforces one outlet per orange connected component.
00217 // Input parameters:
00218 //      outlets: input/output array of outlets
00219 //      tuple_mask: a tuple mask
00220 //      hor_dir: the horizontal direction (can be computed as teh difference between two outlet centers)
00221 void filter_outlets_tuple(std::vector<outlet_t>& outlets, IplImage* tuple_mask, CvPoint2D32f hor_dir);
00222 
00223 // retrieves coordinates of outlet holes in the following order: ground hole, left hole, right hole.
00224 // the size of points array should be at least 3
00225 void get_outlet_coordinates(const outlet_t& outlet, CvPoint3D32f* points);
00226 
00227 void move_features(std::vector<outlet_feature_t>& features, CvPoint origin);
00228 
00229 void estimateCameraPosition(const std::vector<KeyPointEx>& image_points, const std::vector<cv::Point3f>& object_points,
00230                         CvMat* intrinsic_matrix, CvMat* distortion_params, CvMat* rotation_vector, CvMat* translation_vector);
00231 
00232 void getImagePoints(const std::vector<outlet_t>& outlets, std::vector<cv::Point2f>& image_points, std::vector<bool>& is_detected);
00233 
00234 
00235 void findPrecisePowerHoleLocation(IplImage* grey, cv::Point2f center, cv::Point2f dir, cv::Point2f dir_perp, cv::Point2f& hole);
00236 void findPreciseGroundHoleLocation(IplImage* grey, cv::Point2f center, cv::Point2f& hole);
00237 void findPreciseOutletLocations(IplImage* grey, const outlet_template_t& outlet_template, std::vector<outlet_t>& outlets);
00238 void findPreciseOutletLocationsAvg(IplImage* grey, const outlet_template_t& outlet_template, std::vector<outlet_t>& outlets);
00239 
00240 void flipOutlet(std::vector<outlet_t>& outlets);
00241 
00242 #endif //_OUTLET_MODEL_H