matcher.h
Go to the documentation of this file.
00001 /*
00002 Copyright 2012. All rights reserved.
00003 Institute of Measurement and Control Systems
00004 Karlsruhe Institute of Technology, Germany
00005 
00006 This file is part of libviso2.
00007 Authors: Andreas Geiger
00008 
00009 libviso2 is free software; you can redistribute it and/or modify it under the
00010 terms of the GNU General Public License as published by the Free Software
00011 Foundation; either version 2 of the License, or any later version.
00012 
00013 libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY
00014 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
00015 PARTICULAR PURPOSE. See the GNU General Public License for more details.
00016 
00017 You should have received a copy of the GNU General Public License along with
00018 libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
00019 Street, Fifth Floor, Boston, MA 02110-1301, USA 
00020 */
00021 
00022 #ifndef __MATCHER_H__
00023 #define __MATCHER_H__
00024 
00025 #include <stdio.h>
00026 #include <stdlib.h>
00027 #include <string.h>
00028 #include <iostream>
00029 #include <math.h>
00030 #include <emmintrin.h>
00031 #include <algorithm>
00032 #include <vector>
00033 
00034 #include "matrix.h"
00035 
00036 class Matcher {
00037 
00038 public:
00039 
00040   // parameter settings
00041   struct parameters {
00042   
00043     int32_t nms_n;                  // non-max-suppression: min. distance between maxima (in pixels)
00044     int32_t nms_tau;                // non-max-suppression: interest point peakiness threshold
00045     int32_t match_binsize;          // matching bin width/height (affects efficiency only)
00046     int32_t match_radius;           // matching radius (du/dv in pixels)
00047     int32_t match_disp_tolerance;   // dv tolerance for stereo matches (in pixels)
00048     int32_t outlier_disp_tolerance; // outlier removal: disparity tolerance (in pixels)
00049     int32_t outlier_flow_tolerance; // outlier removal: flow tolerance (in pixels)
00050     int32_t multi_stage;            // 0=disabled,1=multistage matching (denser and faster)
00051     int32_t half_resolution;        // 0=disabled,1=match at half resolution, refine at full resolution
00052     int32_t refinement;             // refinement (0=none,1=pixel,2=subpixel)
00053     
00054     // default settings
00055     parameters () {
00056       nms_n                  = 3;
00057       nms_tau                = 50;
00058       match_binsize          = 50;
00059       match_radius           = 200;
00060       match_disp_tolerance   = 2;
00061       outlier_disp_tolerance = 5;
00062       outlier_flow_tolerance = 5;
00063       multi_stage            = 1;
00064       half_resolution        = 1;
00065       refinement             = 1;
00066     }
00067   };
00068 
00069   // constructor (with default parameters)
00070   Matcher(parameters param);
00071 
00072   // deconstructor
00073   ~Matcher();
00074 
00075   // structure for storing matches
00076   struct p_match {
00077     float   u1p,v1p; // u,v-coordinates in previous left  image
00078     int32_t i1p;     // feature index (for tracking)
00079     float   u2p,v2p; // u,v-coordinates in previous right image
00080     int32_t i2p;     // feature index (for tracking)
00081     float   u1c,v1c; // u,v-coordinates in current  left  image
00082     int32_t i1c;     // feature index (for tracking)
00083     float   u2c,v2c; // u,v-coordinates in current  right image
00084     int32_t i2c;     // feature index (for tracking)
00085     p_match(){}
00086     p_match(float u1p,float v1p,int32_t i1p,float u2p,float v2p,int32_t i2p,
00087             float u1c,float v1c,int32_t i1c,float u2c,float v2c,int32_t i2c):
00088             u1p(u1p),v1p(v1p),i1p(i1p),u2p(u2p),v2p(v2p),i2p(i2p),
00089             u1c(u1c),v1c(v1c),i1c(i1c),u2c(u2c),v2c(v2c),i2c(i2c) {}
00090   };
00091 
00092   // computes features from left/right images and pushes them back to a ringbuffer,
00093   // which interally stores the features of the current and previous image pair
00094   // use this function for stereo or quad matching
00095   // input: I1,I2 .......... pointers to left and right image (row-aligned), range [0..255]
00096   //        dims[0,1] ...... image width and height (both images must be rectified and of same size)
00097   //        dims[2] ........ bytes per line (often equals width)
00098   //        replace ........ if this flag is set, the current image is overwritten with
00099   //                         the input images, otherwise the current image is first copied
00100   //                         to the previous image (ring buffer functionality, descriptors need
00101   //                         to be computed only once)    
00102   void pushBack (uint8_t *I1,uint8_t* I2,int32_t* dims,const bool replace);
00103   
00104   // computes features from a single image and pushes it back to a ringbuffer,
00105   // which interally stores the features of the current and previous image pair
00106   // use this function for flow computation
00107   // parameter description see above
00108   void pushBack (uint8_t *I1,int32_t* dims,const bool replace) { pushBack(I1,0,dims,replace); }
00109 
00110   // match features currently stored in ring buffer (current and previous frame)
00111   // input: method ... 0 = flow, 1 = stereo, 2 = quad matching
00112   void matchFeatures(int32_t method);
00113 
00114   // feature bucketing: keeps only max_features per bucket, where the domain
00115   // is split into buckets of size (bucket_width,bucket_height)
00116   void bucketFeatures(int32_t max_features,float bucket_width,float bucket_height);
00117 
00118   // return vector with matched feature points and indices
00119   std::vector<Matcher::p_match> getMatches() { return p_matched_2; }
00120 
00121   // given a vector of inliers computes gain factor between the current and
00122   // the previous frame. this function is useful if you want to reconstruct 3d
00123   // and you want to cancel the change of (unknown) camera gain.
00124   float getGain (std::vector<int32_t> inliers);
00125 
00126 private:
00127 
00128   // structure for storing interest points
00129   struct maximum {
00130     int32_t u;   // u-coordinate
00131     int32_t v;   // v-coordinate
00132     int32_t val; // value
00133     int32_t c;   // class
00134     int32_t d1,d2,d3,d4,d5,d6,d7,d8; // descriptor
00135     maximum() {}
00136     maximum(int32_t u,int32_t v,int32_t val,int32_t c):u(u),v(v),val(val),c(c) {}
00137   };
00138   
00139   // u/v ranges for matching stage 0-3
00140   struct range {
00141     float u_min[4];
00142     float u_max[4];
00143     float v_min[4];
00144     float v_max[4];
00145   };
00146   
00147   struct delta {
00148     float val[8];
00149     delta () {}
00150     delta (float v) {
00151       for (int32_t i=0; i<8; i++)
00152         val[i] = v;
00153     }
00154   };
00155   
00156   // computes the address offset for coordinates u,v of an image of given width
00157   inline int32_t getAddressOffsetImage (const int32_t& u,const int32_t& v,const int32_t& width) {
00158     return v*width+u;
00159   }
00160 
00161   // Alexander Neubeck and Luc Van Gool: Efficient Non-Maximum Suppression, ICPR'06, algorithm 4
00162   void nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,std::vector<Matcher::maximum> &maxima,int32_t nms_n);
00163 
00164   // descriptor functions
00165   inline uint8_t saturate(int16_t in);
00166   void filterImageAll (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,int16_t* I_f1,int16_t* I_f2,const int* dims);
00167   void filterImageSobel (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,const int* dims);
00168   inline void computeDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr);
00169   inline void computeSmallDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr);
00170   void computeDescriptors (uint8_t* I_du,uint8_t* I_dv,const int32_t bpl,std::vector<Matcher::maximum> &maxima);
00171   
00172   void getHalfResolutionDimensions(const int32_t *dims,int32_t *dims_half);
00173   uint8_t* createHalfResolutionImage(uint8_t *I,const int32_t* dims);
00174 
00175   // compute sparse set of features from image
00176   // inputs:  I ........ image
00177   //          dims ..... image dimensions [width,height]
00178   //          n ........ non-max neighborhood
00179   //          tau ...... non-max threshold
00180   // outputs: max ...... vector with maxima [u,v,value,class,descriptor (128 bits)]
00181   //          I_du ..... gradient in horizontal direction
00182   //          I_dv ..... gradient in vertical direction
00183   // WARNING: max,I_du,I_dv has to be freed by yourself!
00184   void computeFeatures (uint8_t *I,const int32_t* dims,int32_t* &max1,int32_t &num1,int32_t* &max2,int32_t &num2,uint8_t* &I_du,uint8_t* &I_dv,uint8_t* &I_du_full,uint8_t* &I_dv_full);
00185 
00186   // matching functions
00187   void computePriorStatistics (std::vector<Matcher::p_match> &p_matched,int32_t method);
00188   void createIndexVector (int32_t* m,int32_t n,std::vector<int32_t> *k,const int32_t &u_bin_num,const int32_t &v_bin_num);
00189   inline void findMatch (int32_t* m1,const int32_t &i1,int32_t* m2,const int32_t &step_size,
00190                          std::vector<int32_t> *k2,const int32_t &u_bin_num,const int32_t &v_bin_num,const int32_t &stat_bin,
00191                          int32_t& min_ind,int32_t stage,bool flow,bool use_prior);
00192   void matching (int32_t *m1p,int32_t *m2p,int32_t *m1c,int32_t *m2c,
00193                  int32_t n1p,int32_t n2p,int32_t n1c,int32_t n2c,
00194                  std::vector<Matcher::p_match> &p_matched,int32_t method,bool use_prior);
00195 
00196   // outlier removal
00197   void removeOutliers (std::vector<Matcher::p_match> &p_matched,int32_t method);
00198 
00199   // parabolic fitting
00200   bool parabolicFitting(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1,
00201                         const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2,
00202                         const float &u1,const float &v1,
00203                         float       &u2,float       &v2,
00204                         Matrix At,Matrix AtA,
00205                         uint8_t* desc_buffer);
00206   void relocateMinimum(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1,
00207                        const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2,
00208                        const float &u1,const float &v1,
00209                        float       &u2,float       &v2,
00210                        uint8_t* desc_buffer);
00211   void refinement (std::vector<Matcher::p_match> &p_matched,int32_t method);
00212 
00213   // mean for gain computation
00214   inline float mean(const uint8_t* I,const int32_t &bpl,const int32_t &u_min,const int32_t &u_max,const int32_t &v_min,const int32_t &v_max);
00215 
00216   // parameters
00217   parameters param;
00218   int32_t    margin;
00219   
00220   int32_t *m1p1,*m2p1,*m1c1,*m2c1;
00221   int32_t *m1p2,*m2p2,*m1c2,*m2c2;
00222   int32_t n1p1,n2p1,n1c1,n2c1;
00223   int32_t n1p2,n2p2,n1c2,n2c2;
00224   uint8_t *I1p,*I2p,*I1c,*I2c;
00225   uint8_t *I1p_du,*I2p_du,*I1c_du,*I2c_du;
00226   uint8_t *I1p_dv,*I2p_dv,*I1c_dv,*I2c_dv;
00227   uint8_t *I1p_du_full,*I2p_du_full,*I1c_du_full,*I2c_du_full; // only needed for
00228   uint8_t *I1p_dv_full,*I2p_dv_full,*I1c_dv_full,*I2c_dv_full; // half-res matching
00229   int32_t dims_p[3],dims_c[3];
00230 
00231   std::vector<Matcher::p_match> p_matched_1;
00232   std::vector<Matcher::p_match> p_matched_2;
00233   std::vector<Matcher::range>   ranges;
00234 };
00235 
00236 #endif
00237 


libviso2
Author(s): Andreas Geiger, maintained by: Stephan Wirth/stephan.wirth@uib.es
autogenerated on Tue Jan 7 2014 11:42:14