Basic Image AlgorithmS Library 2.8.0

TrackerBaseInterface.hh

00001 /* This file is part of the BIAS library (Basic ImageAlgorithmS).
00002    
00003    Copyright (C) 2003-2009    (see file CONTACT for details)
00004    Multimediale Systeme der Informationsverarbeitung
00005    Institut fuer Informatik
00006    Christian-Albrechts-Universitaet Kiel
00007    
00008    BIAS is free software; you can redistribute it and/or modify
00009    it under the terms of the GNU Lesser General Public License as published by
00010    the Free Software Foundation; either version 2.1 of the License, or
00011    (at your option) any later version.
00012    
00013    BIAS is distributed in the hope that it will be useful,
00014    but WITHOUT ANY WARRANTY; without even the implied warranty of
00015    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016    GNU Lesser General Public License for more details.
00017    
00018    You should have received a copy of the GNU Lesser General Public License
00019    along with BIAS; if not, write to the Free Software
00020    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA*/
00021 #ifndef __TrackerBaseInterface__hh__
00022 #define __TrackerBaseInterface__hh__
00023 
00024 #include <Base/Common/BIASpragmaStart.hh>
00025 
00026 #include <Base/Debug/Debug.hh>
00027 #include <Base/Geometry/HomgPoint2D.hh>
00028 #include <Base/Math/Matrix2x2.hh>
00029 #include <Base/Image/Image.hh>
00030 #include <MathAlgo/Median1D.hh>
00031 #include <Filter/FilterMask.hh>
00032 
00033 #include <vector>
00034 
00035 #define KLT_TYPE double
00036 
00037 #define D_TRACKERB_INIT        0x00000001
00038 #define D_TRACKERB_KLT         0x00000002
00039 #define D_TRACKERB_BILINEAR    0x00000004
00040 #define D_TRACKERB_INPUT       0x00000008
00041 #define D_TRACKERB_REJECT      0x00000010
00042 #define D_TRACKERB_AFFINE      0x00000020
00043 
00044 #define TRACKERBASE_INVALID_PIXEL -1e10
00045 
00046 namespace BIAS {
00047 
00048   enum ERejectionType { RT_Invalid=-1, RT_MAD, RT_X84, RT_X84M };
00049 
00050   enum ETrackerResult {
00051     TRACKER_SUCCESS        =  0, ///< success (error < maxerror)
00052     TRACKER_NOSTRUCTURE    = -1, ///< no spatial structure is present
00053     TRACKER_OUTOFIMAGE     = -2, ///< point slid out of image
00054     TRACKER_TOOMANYITER    = -3, ///< maxiter is reached and error is above maxerror
00055     TRACKER_OUTOFROI       = -4, ///< a point lies outside of valid region in images
00056     TRACKER_MEANABSDIF     = -5, ///< point was rejected by MAD criterion
00057     TRACKER_IGNOREPOINT    = -6, ///< input point was at infinity
00058     TRACKER_X84            = -7, ///< point is rejected by X84/X84M
00059     TRACKER_ODDAFFINEWARP  = -8, ///< affine warp seems degenerated
00060     TRACKER_ERRORINCREASED =  1  ///< error increased from last iteration 
00061   };
00062 
00063   std::ostream& operator<<(std::ostream& os, const enum ETrackerResult& res);
00064 
00065   std::ostream& operator<<(std::ostream& os, const enum ERejectionType& rt);
00066 
00067   /** @class TrackerBaseInterface
00068       @ingroup g_tracker
00069       @brief  Base class for the different tracking algorithms, defining the interfaces
00070       for the tracking functions. Also stores some basic options and datas, 
00071       i.e. the pointer to the image datas. See ExampleTrackerBaseInterface for brief 
00072       exemplary usage.
00073       @author woelk 09/2004, kollmann 07/2006 */
00074   template <class StorageType>
00075   class BIASMatcher2D_EXPORT TrackerBaseInterface : public Debug
00076   {
00077   public :
00078     TrackerBaseInterface();
00079     virtual ~TrackerBaseInterface();
00080 
00081     /** @brief Prepare for tracking with prefiltered images.
00082 
00083     The ROI must be set correctly in gradient images (representing the 
00084     gradient filter mask size). 
00085     For good results im1 and im2 should be lowpass filtered images.
00086 
00087     @param im1      (low-pass filtered) image 1
00088     @param im2      (low-pass filtered) image 2
00089     @param gradx1   horizontal gradient image of im1
00090     @param grady1   vertical gradient image of im1
00091     @param gradx2   horizontal gradient image of im2
00092     @param grady2   vertical gradient image of im2 */
00093     void Init(Image<StorageType>& im1, Image<StorageType>& im2, 
00094               Image<StorageType>& gradx1, Image<StorageType>& grady1, 
00095               Image<StorageType>& gradx2, Image<StorageType>& grady2);
00096 
00097     /** @brief Prepare for tracking with on-demand filtering.
00098 
00099     Lowpass and gradient filtering is done only where needed, using the
00100     given filter masks. All filter masks must have odd size.
00101 
00102     @param im1              (low-pass filtered) image 1
00103     @param im2              (low-pass filtered) image 2
00104     @param lowpass_mask     filter mask for lowpass filtering
00105     @param gradx_mask       filter mask for horizontal gradient computation
00106     @param grady_mask       filter mask for vertical gradient computation */
00107     void Init(Image<StorageType>& im1, Image<StorageType>& im2,
00108               FilterMask& lowpass_mask, FilterMask& gradx_mask, 
00109               FilterMask& grady_mask);
00110 
00111     /** @brief Calculates correspondence from image1 to image2. The two images
00112         and corresponding gradients must specified using the function Init()
00113         before calling this function.
00114 
00115         The algorithm searches for best correspondence to point p1 from image1 
00116         in image2. The search starts at p2 in image 2.
00117 
00118         All homogenous points must be homogenized (i.e. w=1.0).
00119         
00120         The algorithms either terminates if the displacement resulting from
00121         one iteration drops below *_Maxerror or the maximum number of 
00122         iterations *_MaxIterations is reached.
00123 
00124         @param[in]  p1           Point in image1
00125         @param[in]  p2           Algorithm starts searching at this point in image2
00126         @param[out] p2tracked    Best matching point in image2
00127         @param[out] error        Change in displacement calculated in last iteration
00128         @param[out] iter         Number of iterations used
00129         @param[out] residuumMAD  Mean absolute grey value difference  between p1 in
00130         image 1 and p2tracked in image 2 over the tracking
00131         support window
00132         @param[out] residuumMSD  Squared grey value difference between p1 in image 1
00133         and p2tracked in image 2 summed over the tracking
00134         support window
00135         @param[out] cov          Approximation to the covarinace matrix image1 and
00136         result in image2 over the tracking window (= sum of
00137         absolute difference divided by number of pixels in
00138         tracking value)
00139         @param[in]  AffinePred   Predicted affine matrix (only used in affine tracking)
00140         @param[out] AffineResult Result of affine tracking (only used in affine tracking)
00141         @return                  See TRACKER_* */
00142     int Track(HomgPoint2D& p1, HomgPoint2D& p2, HomgPoint2D& p2tracked, 
00143               KLT_TYPE& error, int& iter, KLT_TYPE& residuumMAD,
00144               KLT_TYPE& residuumMSD, Matrix<KLT_TYPE>& cov,
00145               const Matrix2x2<KLT_TYPE>& AffinePred =
00146               Matrix2x2<KLT_TYPE>(MatrixIdentity),
00147               Matrix2x2<KLT_TYPE>* AffineResult = NULL);
00148 
00149     /** @brief Same as Track(), but a correspondence is searched for every 
00150         point in the vector p1. */
00151     void Track(std::vector<HomgPoint2D>& p1, 
00152                std::vector<HomgPoint2D>& p2, 
00153                std::vector<HomgPoint2D>& p2tracked, 
00154                std::vector<KLT_TYPE>& error, 
00155                std::vector<int>& numiter,
00156                std::vector<KLT_TYPE>& residuiMAD, 
00157                std::vector<KLT_TYPE>& residuiMSD, 
00158                std::vector<int>& res,
00159                std::vector<Matrix<KLT_TYPE> >& cov,
00160                const std::vector<Matrix2x2<KLT_TYPE> >& AffinePred =
00161                std::vector<Matrix2x2<KLT_TYPE> >(),
00162                std::vector<Matrix2x2<KLT_TYPE> >* AffineResult = NULL);
00163 
00164     inline void SetHalfWinSize(const int hws)              
00165     { _HalfWinSize=hws;  _WinSize=(hws<<1)+1; }
00166     inline void SetMaxIterations(const int maxiter)        
00167     { _MaxIterations=maxiter; }
00168     inline void SetMaxError(const KLT_TYPE maxerr)         
00169     { _MaxError=maxerr; }
00170 
00171     /** !!!
00172         Also used for X84M as maximal residuum. That is features with a 
00173         residuum <= 'maxres' will be accept automatically!
00174         !!! 
00175     **/
00176     inline void SetMaxResiduumMAD(const KLT_TYPE maxres)   
00177     { _MaxResiduumMAD=maxres; }
00178     inline void SetRejectionType(const int rejection_type) 
00179     { _RejectionType = rejection_type; }
00180     
00181 
00182     /** enable brightness variance and offset invariant computation */
00183     virtual void SetAffineBrightnessInvariance(bool bi) { 
00184       _AffineBrightnessInvariance = bi;
00185     }
00186 
00187     // these are needed in the highlevel Tracker class
00188     inline int GetHalfWinSize()   { return _HalfWinSize; }
00189     inline int GetMaxIterations() { return _MaxIterations; }
00190     inline KLT_TYPE GetMaxError() { return _MaxError; }
00191     inline int GetRejectionType() { return _RejectionType; }
00192 
00193     virtual void SetWeightMatrix(const BIAS::Matrix<KLT_TYPE>& weight) {}
00194     
00195   protected:
00196 
00197     ///////////////////////////////////////////////////////////////////////////
00198     //  params
00199     ///////////////////////////////////////////////////////////////////////////
00200     
00201     int       _HalfWinSize;    ///< use support window of size 2*hws+1
00202     int       _MaxIterations;  ///< iteration stops after _MaxIterations
00203     /// iteration stops if position refinement is less than *_MaxError
00204     KLT_TYPE  _MaxError;       
00205     KLT_TYPE  _MaxResiduumMAD; ///< outlier rejection via MAD and
00206                                  // acception via X84M respectively
00207     int       _RejectionType; ///< The rejection type: RT_MAD,RT_X84 or RT_X84M
00208 
00209 
00210     /// compute brightness "invariant"
00211     bool _AffineBrightnessInvariance;
00212     
00213     /// mean and std deviation of image patch
00214     KLT_TYPE mean1_, mean2_, dev1_, dev2_;
00215 
00216     ///////////////////////////////////////////////////////////////////////////
00217 
00218     // pointers to images
00219     Image<StorageType> *_im1, *_gradim1x, *_gradim1y;
00220     Image<StorageType> *_im2, *_gradim2x, *_gradim2y;
00221 
00222     // filtered images are computed when set to true
00223     bool _ComputeFilteredImages;
00224 
00225     // lowpass filter computation type
00226     enum LowpassFilter
00227       {
00228         LOWPASS_BINOMIAL3X3,
00229         LOWPASS_FILTERMASK,
00230         LOWPASS_FILTERMASK_SEPARABLE
00231       };
00232 
00233     // gradient filter computation type
00234     enum GradientFilter
00235       {
00236         GRAD_SOBEL3X3,
00237         GRAD_FILTERMASK,
00238         GRAD_FILTERMASK_SEPARABLE
00239       };
00240     
00241     LowpassFilter  _LowpassFilter;
00242     GradientFilter _GradXFilter, _GradYFilter;
00243     FilterMask     _LowpassMask, _GradXMask, _GradYMask;
00244     KLT_TYPE       _LowpassMaskSum, _GradXMaskSum, _GradYMaskSum;
00245     int            _LowpassWindowExtraSize;
00246     
00247     // the image borders modulo filter and support windows or ROI
00248     // for image 1 and image 2
00249     int _maxx1, _maxy1, _minx1, _miny1;
00250     int _maxx2, _maxy2, _minx2, _miny2;
00251     
00252     // remember last support window size (for recalculation of gaussian weight)
00253     int _LastHalfWinSize, _WinSize;
00254     
00255     // used in iteration
00256     Matrix2x2<KLT_TYPE> _G, _Ginv;
00257     Vector2<KLT_TYPE> _e, _d, _disp;
00258     Matrix<KLT_TYPE> _gx, _gy, _bl1, _bl2, _gx1, _gy1, _gx2, _gy2, _gsx, _gsy;
00259     KLT_TYPE _gxx, _gxy, _gyy, _idet;
00260     Matrix<KLT_TYPE> _weight; // the weight matrix used in TrackWeighted_
00261     
00262     ///////////////////////////////////////////////////////////////////
00263     //  tracking
00264     ///////////////////////////////////////////////////////////////////
00265 
00266     /** @brief Interface of all tracking algorithms implemented in
00267         derived classes. */
00268     virtual int Track_(Vector2<KLT_TYPE>& p1, Vector2<KLT_TYPE>& p2,
00269                        Vector2<KLT_TYPE>& result, KLT_TYPE& error, 
00270                        int &iter,
00271                        const Matrix2x2<KLT_TYPE>& AffinePred,
00272                        Matrix2x2<KLT_TYPE>& AffineResult) = 0;
00273 
00274     ///////////////////////////////////////////////////////////////////
00275     //  calculation of cov, sad and ssd
00276     ///////////////////////////////////////////////////////////////////
00277 
00278     /** Uses _bl1, _bl2, _gxx, _gxy and _gyy to calculate the residui
00279         and covariance matrix and must hence be called immediatly after the 
00280         Track_() function call!!
00281         @author woelk */
00282     virtual void EvaluateResult_(KLT_TYPE& mad,KLT_TYPE& msd, 
00283                                  Matrix<KLT_TYPE>& cov);
00284 
00285     ///////////////////////////////////////////////////////////////////
00286     //  outlier rejection
00287     ///////////////////////////////////////////////////////////////////
00288 
00289     /** @author woelk */
00290     int RejectMAD_(std::vector<KLT_TYPE>& sad, std::vector<int>& res);
00291 
00292     /** @author woelk */
00293     int RejectX84_(std::vector<KLT_TYPE>& ssd, std::vector<KLT_TYPE>& sad,
00294                    std::vector<int>& res);
00295 
00296     ///////////////////////////////////////////////////////////////////
00297     //  helper functions
00298     ///////////////////////////////////////////////////////////////////
00299     
00300     /** resizes the internal matrices _gx1, _gy1, _bl1, _gx2, _gy2 and _bl2
00301         @author woelk */
00302     inline void Resize_(const int halfwinsize);
00303 
00304     ////////////////////////////////////////////////////////////////////
00305     //  bilinear interpolation functions 
00306     ////////////////////////////////////////////////////////////////////
00307 
00308     KLT_TYPE ComputeMaskSum(const FilterMask& mask);
00309 
00310     /** The lowpass filter functions apply a filter to a window of
00311         the source image and store the result in a matrix. */
00312     //@{
00313 
00314     /// Lowpass filtering using a kernel filter mask.
00315     inline void FilterLowpass_ByMask(int minX, int minY,
00316                                      int rows, int cols,
00317                                      const Image<StorageType>& image,
00318                                      Matrix<KLT_TYPE>& out,
00319                                      const FilterMask& mask,
00320                                      KLT_TYPE maskSum);
00321 
00322     /// Lowpass filtering using a separable filter mask.
00323     inline void FilterLowpass_BySeparableMask(int minX, int minY,
00324                                               int rows, int cols,
00325                                               const Image<StorageType>& image,
00326                                               Matrix<KLT_TYPE>& out,
00327                                               const FilterMask& mask,
00328                                               KLT_TYPE maskSum);
00329 
00330     /// Optimized lowpass filtering using the binomial 3x3 filter.
00331     inline void FilterLowpass_Binomial3x3(int minX, int minY,
00332                                           int rows, int cols,
00333                                           const Image<StorageType>& image,
00334                                           Matrix<KLT_TYPE>& out);
00335     //@}
00336 
00337     /** The gradient filter functions apply a filter to an image window
00338         stored in a matrix and store the result in another matrix. */
00339     //@{
00340 
00341     /// Optimized gradient X sobel3x3 filter.
00342     inline void Filter_GradXSobel3x3(const Matrix<KLT_TYPE>& in,
00343                                      Matrix<KLT_TYPE>& out);
00344 
00345     /// Optimized gradient Y sobel3x3 filter.
00346     inline void Filter_GradYSobel3x3(const Matrix<KLT_TYPE>& in,
00347                                      Matrix<KLT_TYPE>& out);
00348 
00349     /// Generic gradient filter.
00350     inline void Filter_ByMask(const Matrix<KLT_TYPE>& in,
00351                               Matrix<KLT_TYPE>& out,
00352                               const FilterMask& mask,
00353                               KLT_TYPE maskSum);
00354 
00355     /// Generic gradient filter using separable filter mask.
00356     inline void Filter_BySeparableMask(const Matrix<KLT_TYPE>& in,
00357                                        Matrix<KLT_TYPE>& out,
00358                                        const FilterMask& mask,
00359                                        KLT_TYPE maskSum);
00360 
00361     //@}
00362 
00363     /** Fills the matrices _gx1, _gy1 and _bl1 by interpolating gradients
00364         and lowpass filtered image at positions between (x-hws, y-hws) and
00365         (x+hws, y+hws). */
00366     inline void BilinearRegion1_(KLT_TYPE x, KLT_TYPE y, int hws);
00367 
00368     /** Fills the matrices _gx2, _gy2 and _bl2 by interpolating gradients
00369         and lowpass filtered image at positions between (x-hws, y-hws) and
00370         (x+hws, y+hws). */
00371     inline void BilinearRegion2_(KLT_TYPE x, KLT_TYPE y, int hws);
00372 
00373     /** Bilinear filtering using images as source (for prefiltered images) */
00374     inline KLT_TYPE 
00375     BilinearRegionFromImages_(KLT_TYPE x, KLT_TYPE y, int hws,
00376                               Image<StorageType>& image,
00377                               Image<StorageType>& gradx,
00378                               Image<StorageType>& grady,
00379                               Matrix<KLT_TYPE>& bl,
00380                               Matrix<KLT_TYPE>& gx,
00381                               Matrix<KLT_TYPE>& gy);
00382 
00383     /** Bilinear filtering using matrices as source */
00384     inline KLT_TYPE 
00385     BilinearRegionFromMatrices_(KLT_TYPE x, KLT_TYPE y, int hws,
00386                                 const Matrix<KLT_TYPE>& image,
00387                                 const Matrix<KLT_TYPE>& gradx,
00388                                 const Matrix<KLT_TYPE>& grady,
00389                                 Matrix<KLT_TYPE>& bl,
00390                                 Matrix<KLT_TYPE>& gx,
00391                                 Matrix<KLT_TYPE>& gy);
00392 
00393     /** bring region to mean zero and stddev 1 for brightness invariance 
00394         @return scale of region which has been applied */
00395     inline KLT_TYPE NormalizeRegion_(Matrix<KLT_TYPE>& bl,
00396                                      Matrix<KLT_TYPE>& gx,
00397                                      Matrix<KLT_TYPE>& gy,
00398                                      const KLT_TYPE&  min_value = -1e9);
00399     
00400   }; // class
00401 
00402   ////////////////////////////////////////////////////////////////////
00403   //  inline functions
00404   ////////////////////////////////////////////////////////////////////
00405 
00406   template <class StorageType>
00407   inline void TrackerBaseInterface<StorageType>::
00408   Resize_(const int halfwinsize)
00409   {
00410     _LastHalfWinSize=halfwinsize;
00411     _WinSize=(halfwinsize<<1)+1;
00412     _gx1.newsize(_WinSize, _WinSize);
00413     _gy1.newsize(_WinSize, _WinSize);
00414     _gx2.newsize(_WinSize, _WinSize);
00415     _gy2.newsize(_WinSize, _WinSize);
00416     _gsx.newsize(_WinSize, _WinSize);
00417     _gsy.newsize(_WinSize, _WinSize);
00418     _bl1.newsize(_WinSize, _WinSize);
00419     _bl2.newsize(_WinSize, _WinSize);
00420   }
00421 
00422   template <class StorageType>
00423   inline void TrackerBaseInterface<StorageType>::
00424   FilterLowpass_ByMask(int minX, int minY, int rows, int cols,
00425                        const Image<StorageType>& image,
00426                        Matrix<KLT_TYPE>& out,
00427                        const FilterMask& mask,
00428                        KLT_TYPE maskSum)
00429   {
00430     BIASASSERT(!mask.IsSeparable());
00431     
00432     const Matrix<FM_FLOAT>& kernel = *mask.GetKernelf();
00433 
00434     int borderRows = (kernel.GetRows()-1) / 2;
00435     int borderCols = (kernel.GetCols()-1) / 2;
00436   
00437     out.newsize(rows, cols);
00438 
00439     const StorageType** ida = image.GetImageDataArray();
00440 
00441     for (int i = 0; i < rows; i++)
00442       {
00443         for (int j = 0; j < cols; j++)
00444           {
00445             out[i][j] = 0;
00446             for (int k = 0; k < (int)kernel.GetRows(); k++)
00447               {
00448                 for (int l = 0; l < (int)kernel.GetCols(); l++)
00449                   {
00450                     out[i][j] +=
00451                       kernel[k][l] * ida[i+k-borderRows+minY][j+l-borderCols+minX];
00452                   }
00453               }
00454             out[i][j] /= maskSum;
00455           }
00456       }
00457   }
00458 
00459   template <class StorageType>
00460   inline void TrackerBaseInterface<StorageType>::
00461   FilterLowpass_BySeparableMask(int minX, int minY, int rows, int cols,
00462                                 const Image<StorageType>& image,
00463                                 Matrix<KLT_TYPE>& out,
00464                                 const FilterMask& mask,
00465                                 KLT_TYPE maskSum)
00466   {
00467     BIASASSERT(mask.IsSeparable());
00468     
00469     const Vector<FM_FLOAT>& vert = *mask.GetSepfv();
00470     const Vector<FM_FLOAT>& horz = *mask.GetSepfh();
00471     
00472     int borderRows = (vert.Size()-1) / 2;
00473     int borderCols = (horz.Size()-1) / 2;
00474   
00475     out.newsize(rows, cols);
00476 
00477     const StorageType** ida = image.GetImageDataArray();
00478 
00479     for (int i = 0; i < rows; i++)
00480       {
00481         for (int j = 0; j < cols; j++)
00482           {
00483             out[i][j] = 0;
00484             for (int k = 0; k < (int)vert.Size(); k++)
00485               {
00486                 for (int l = 0; l < (int)horz.Size(); l++)
00487                   {
00488                     out[i][j] +=
00489                       vert[k] * horz[l]
00490                       * ida[i+k-borderRows+minY][j+l-borderCols+minX];
00491                   }
00492               }
00493             out[i][j] /= maskSum;
00494           }
00495       }
00496   }
00497 
00498   template <class StorageType>
00499   inline void TrackerBaseInterface<StorageType>::
00500   FilterLowpass_Binomial3x3(int minX, int minY, int rows, int cols,
00501                             const Image<StorageType>& image,
00502                             Matrix<KLT_TYPE>& out)
00503   {
00504     out.newsize(rows, cols);
00505 
00506     const StorageType** ida = image.GetImageDataArray();
00507 
00508     for (int i = 0; i < rows; i++)
00509       {
00510         for (int j = 0; j < cols; j++)
00511           {
00512             int x = j + minX;
00513             int y = i + minY;
00514 
00515             /* 1 2 1
00516                2 4 2
00517                1 2 1 */
00518             out[i][j] =
00519               ida[y-1][x-1] + 2*ida[y-1][x] +   ida[y-1][x+1]
00520               +2*ida[y  ][x-1] + 4*ida[y  ][x] + 2*ida[y  ][x+1]
00521               +  ida[y+1][x-1] + 2*ida[y+1][x] +   ida[y+1][x+1];
00522             out[i][j] /= 16.0;
00523           }
00524       }
00525   }
00526 
00527   template <class StorageType>
00528   inline void TrackerBaseInterface<StorageType>::
00529   Filter_GradXSobel3x3(const Matrix<KLT_TYPE>& in, Matrix<KLT_TYPE>& out)
00530   {
00531     int rows = in.GetRows();
00532     int cols = in.GetCols();
00533     
00534     out.newsize(rows, cols);
00535 
00536     for (int i = 1; i < rows-1; i++)
00537       {
00538         for (int j = 1; j < cols-1; j++)
00539           {
00540             /* -1 0 1
00541                -2 0 2
00542                -1 0 1 */
00543             out[i][j] =
00544               -  in[i-1][j-1] +   in[i-1][j+1]  // row 0
00545               -2*in[i  ][j-1] + 2*in[i  ][j+1]  // row 1
00546               -  in[i+1][j-1] +   in[i+1][j+1]; // row 2
00547             out[i][j] /= 8.0;
00548           }
00549       }
00550   }
00551 
00552   template <class StorageType>
00553   inline void TrackerBaseInterface<StorageType>::
00554   Filter_GradYSobel3x3(const Matrix<KLT_TYPE>& in, Matrix<KLT_TYPE>& out)
00555   {
00556     int rows = in.GetRows();
00557     int cols = in.GetCols();
00558   
00559     out.newsize(rows, cols);
00560 
00561     for (int i = 1; i < rows-1; i++)
00562       {
00563         for (int j = 1; j < cols-1; j++)
00564           {
00565             /* -1 -2 -1
00566                0  0  0
00567                1  2  1 */
00568             out[i][j] = 
00569               -in[i-1][j-1] - 2*in[i-1][j] - in[i-1][j+1]  // row 0
00570               +in[i+1][j-1] + 2*in[i+1][j] + in[i+1][j+1]; // row 2
00571             out[i][j] /= (StorageType)8.0;
00572           }
00573       }
00574   }
00575 
00576   template <class StorageType>
00577   inline void TrackerBaseInterface<StorageType>::
00578   Filter_ByMask(const Matrix<KLT_TYPE>& in,
00579                 Matrix<KLT_TYPE>& out,
00580                 const FilterMask& mask,
00581                 KLT_TYPE maskSum)
00582   {
00583     BIASASSERT(!mask.IsSeparable());
00584     
00585     const Matrix<FM_FLOAT>& kernel = *mask.GetKernelf();
00586 
00587     int borderRows = (kernel.GetRows()-1) / 2;
00588     int borderCols = (kernel.GetCols()-1) / 2;
00589   
00590     int rows = in.GetRows();
00591     int cols = in.GetCols();
00592   
00593     out.newsize(rows, cols);
00594 
00595     for (int i = borderRows; i < rows-borderRows; i++)
00596       {
00597         for (int j = borderCols; j < cols-borderCols; j++)
00598           {
00599             out[i][j] = 0;
00600             for (int k = 0; k < (int)kernel.GetRows(); k++)
00601               {
00602                 for (int l = 0; l < (int)kernel.GetCols(); l++)
00603                   {
00604                     out[i][j] += kernel[k][l] * in[i+k-borderRows][j+l-borderCols];
00605                   }
00606               }
00607             out[i][j] /= maskSum;
00608           }
00609       }
00610   }
00611 
00612   template <class StorageType>
00613   inline void TrackerBaseInterface<StorageType>::
00614   Filter_BySeparableMask(const Matrix<KLT_TYPE>& in,
00615                          Matrix<KLT_TYPE>& out,
00616                          const FilterMask& mask,
00617                          KLT_TYPE maskSum)
00618   {
00619     BIASASSERT(mask.IsSeparable());
00620     
00621     const Vector<FM_FLOAT>& vert = *mask.GetSepfv();
00622     const Vector<FM_FLOAT>& horz = *mask.GetSepfh();
00623     
00624     int borderRows = (vert.Size()-1) / 2;
00625     int borderCols = (horz.Size()-1) / 2;
00626 
00627     int rows = in.GetRows();
00628     int cols = in.GetCols();
00629   
00630     out.newsize(rows, cols);
00631 
00632     for (int i = borderRows; i < rows-borderRows; i++)
00633       {
00634         for (int j = borderCols; j < cols-borderCols; j++)
00635           {
00636             out[i][j] = 0;
00637             for (int k = 0; k < (int)vert.Size(); k++)
00638               {
00639                 for (int l = 0; l < (int)horz.Size(); l++)
00640                   {
00641                     out[i][j] +=
00642                       vert[k] * horz[l]
00643                       * in[i+k-borderRows][j+l-borderCols];
00644                   }
00645               }
00646             out[i][j] /= maskSum;
00647           }
00648       }
00649   }
00650 
00651   template <class StorageType>
00652   inline void TrackerBaseInterface<StorageType>::
00653   BilinearRegion1_(KLT_TYPE x, KLT_TYPE y, int hws)
00654   {
00655     if (_ComputeFilteredImages)
00656       {
00657         Matrix<KLT_TYPE> lowpass;
00658         int lowpassHWS = hws + _LowpassWindowExtraSize;
00659         int lowpassWS = 2*lowpassHWS+2;
00660         int lowpassMinX = (int)floor(x) - lowpassHWS;
00661         int lowpassMinY = (int)floor(y) - lowpassHWS;
00662 
00663         switch (_LowpassFilter)
00664           {
00665           case LOWPASS_BINOMIAL3X3:
00666             FilterLowpass_Binomial3x3(lowpassMinX, lowpassMinY,
00667                                       lowpassWS, lowpassWS,
00668                                       *_im1, lowpass);
00669             break;
00670           case LOWPASS_FILTERMASK:
00671             FilterLowpass_ByMask(lowpassMinX, lowpassMinY,
00672                                  lowpassWS, lowpassWS,
00673                                  *_im1, lowpass,
00674                                  _LowpassMask, _LowpassMaskSum);
00675             break;
00676           case LOWPASS_FILTERMASK_SEPARABLE:
00677             FilterLowpass_BySeparableMask(lowpassMinX, lowpassMinY,
00678                                           lowpassWS, lowpassWS,
00679                                           *_im1, lowpass,
00680                                           _LowpassMask, _LowpassMaskSum);
00681             break;
00682           }
00683 
00684         Matrix<KLT_TYPE> gradx;
00685         switch (_GradXFilter)
00686           {
00687           case GRAD_SOBEL3X3:
00688             Filter_GradXSobel3x3(lowpass, gradx);
00689             break;
00690           case GRAD_FILTERMASK:
00691             Filter_ByMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
00692             break;
00693           case GRAD_FILTERMASK_SEPARABLE:
00694             Filter_BySeparableMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
00695             break;
00696           }
00697 
00698         Matrix<KLT_TYPE> grady;
00699         switch (_GradYFilter)
00700           {
00701           case GRAD_SOBEL3X3:
00702             Filter_GradYSobel3x3(lowpass, grady);
00703             break;
00704           case GRAD_FILTERMASK:
00705             Filter_ByMask(lowpass, grady, _GradYMask, _GradYMaskSum);
00706             break;
00707           case GRAD_FILTERMASK_SEPARABLE:
00708             Filter_BySeparableMask(lowpass, grady, _GradYMask, _GradYMaskSum);
00709             break;
00710           }
00711       
00712         dev1_ = this->BilinearRegionFromMatrices_(x - lowpassMinX,
00713                                                   y - lowpassMinY,
00714                                                   hws,
00715                                                   lowpass, gradx, grady,
00716                                                   _bl1, _gx1, _gy1);
00717       }
00718     else
00719       {
00720         dev1_ = this->BilinearRegionFromImages_(x, y, hws,
00721                                                 *_im1, *_gradim1x, *_gradim1y,
00722                                                 _bl1, _gx1, _gy1);
00723       }
00724   }
00725 
00726   template <class StorageType>
00727   inline void TrackerBaseInterface<StorageType>::
00728   BilinearRegion2_(KLT_TYPE x, KLT_TYPE y, int hws)
00729   {
00730 
00731     if (_ComputeFilteredImages)
00732       {
00733         Matrix<KLT_TYPE> lowpass;
00734         int lowpassHWS = hws + _LowpassWindowExtraSize;
00735         int lowpassWS = 2*lowpassHWS+2;
00736         int lowpassMinX = (int)floor(x) - lowpassHWS;
00737         int lowpassMinY = (int)floor(y) - lowpassHWS;
00738 
00739         switch (_LowpassFilter)
00740           {
00741           case LOWPASS_BINOMIAL3X3:
00742             FilterLowpass_Binomial3x3(lowpassMinX, lowpassMinY,
00743                                       lowpassWS, lowpassWS,
00744                                       *_im2, lowpass);
00745             break;
00746           case LOWPASS_FILTERMASK:
00747             FilterLowpass_ByMask(lowpassMinX, lowpassMinY,
00748                                  lowpassWS, lowpassWS,
00749                                  *_im2, lowpass,
00750                                  _LowpassMask, _LowpassMaskSum);
00751             break;
00752           case LOWPASS_FILTERMASK_SEPARABLE:
00753             FilterLowpass_BySeparableMask(lowpassMinX, lowpassMinY,
00754                                           lowpassWS, lowpassWS,
00755                                           *_im2, lowpass,
00756                                           _LowpassMask, _LowpassMaskSum);
00757             break;
00758           }
00759 
00760         Matrix<KLT_TYPE> gradx;
00761         switch (_GradXFilter)
00762           {
00763           case GRAD_SOBEL3X3:
00764             Filter_GradXSobel3x3(lowpass, gradx);
00765             break;
00766           case GRAD_FILTERMASK:
00767             Filter_ByMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
00768             break;
00769           case GRAD_FILTERMASK_SEPARABLE:
00770             Filter_BySeparableMask(lowpass, gradx, _GradXMask, _GradXMaskSum);
00771             break;
00772           }
00773 
00774         Matrix<KLT_TYPE> grady;
00775         switch (_GradYFilter)
00776           {
00777           case GRAD_SOBEL3X3:
00778             Filter_GradYSobel3x3(lowpass, grady);
00779             break;
00780           case GRAD_FILTERMASK:
00781             Filter_ByMask(lowpass, grady, _GradYMask, _GradYMaskSum);
00782             break;
00783           case GRAD_FILTERMASK_SEPARABLE:
00784             Filter_BySeparableMask(lowpass, grady, _GradYMask, _GradYMaskSum);
00785             break;
00786           }
00787       
00788         dev2_ = this->BilinearRegionFromMatrices_(x - lowpassMinX,
00789                                                   y - lowpassMinY,
00790                                                   hws,
00791                                                   lowpass, gradx, grady,
00792                                                   _bl2, _gx2, _gy2);
00793       } else {
00794         dev2_ = this->BilinearRegionFromImages_(x, y, hws,
00795                                                 *_im2, *_gradim2x, *_gradim2y,
00796                                                 _bl2, _gx2, _gy2);
00797       }
00798   }
00799 
00800   template <class StorageType>
00801   inline KLT_TYPE TrackerBaseInterface<StorageType>::
00802   BilinearRegionFromImages_(KLT_TYPE x, KLT_TYPE y, int hws,
00803                             Image<StorageType>& image,
00804                             Image<StorageType>& gradx,
00805                             Image<StorageType>& grady,
00806                             Matrix<KLT_TYPE>& bl,
00807                             Matrix<KLT_TYPE>& gx,
00808                             Matrix<KLT_TYPE>& gy)  {
00809     StorageType** image_ida = image.GetImageDataArray();
00810     StorageType** gradx_ida = gradx.GetImageDataArray();
00811     StorageType** grady_ida = grady.GetImageDataArray();
00812 
00813     int x_floor = (int)floor(x);
00814     int y_floor = (int)floor(y);
00815     int x_floor_end = x_floor+hws+1;
00816     int y_floor_end = y_floor+hws+1;
00817     KLT_TYPE brdy = y - (KLT_TYPE)(y_floor);
00818     KLT_TYPE brdx = x - (KLT_TYPE)(x_floor);
00819     KLT_TYPE brdxy = brdx*brdy;
00820 
00821     int rf, rc, cf, cc, r, c;
00822     for (rf=y_floor-hws, rc=rf+1, r=0; rf<y_floor_end; rf++, rc++, r++) {
00823       for (cf=x_floor-hws, cc=cf+1, c=0; cf<x_floor_end; cf++, cc++, c++) {
00824         KLT_TYPE ul, ur, ll, lr;
00825     
00826         ul = (KLT_TYPE)image_ida[rf][cf];
00827         ur = (KLT_TYPE)image_ida[rf][cc];
00828         ll = (KLT_TYPE)image_ida[rc][cf];
00829         lr = (KLT_TYPE)image_ida[rc][cc];
00830         bl[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00831                     brdxy*(ul + lr - ll - ur));
00832         ul = (KLT_TYPE)(gradx_ida[rf][cf]);
00833         ur = (KLT_TYPE)(gradx_ida[rf][cc]);
00834         ll = (KLT_TYPE)(gradx_ida[rc][cf]);
00835         lr = (KLT_TYPE)(gradx_ida[rc][cc]);
00836         gx[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00837                     brdxy*(ul + lr - ll - ur));
00838         ul = (KLT_TYPE)(grady_ida[rf][cf]);
00839         ur = (KLT_TYPE)(grady_ida[rf][cc]);
00840         ll = (KLT_TYPE)(grady_ida[rc][cf]);
00841         lr = (KLT_TYPE)(grady_ida[rc][cc]);
00842         gy[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00843                     brdxy*(ul + lr - ll - ur));
00844       }
00845     }
00846     if (_AffineBrightnessInvariance) return NormalizeRegion_(bl, gx, gy);
00847     return 1.0;
00848   }
00849 
00850   template <class StorageType>
00851   inline KLT_TYPE TrackerBaseInterface<StorageType>::
00852   BilinearRegionFromMatrices_(KLT_TYPE x, KLT_TYPE y, int hws,
00853                               const Matrix<KLT_TYPE>& image,
00854                               const Matrix<KLT_TYPE>& gradx,
00855                               const Matrix<KLT_TYPE>& grady,
00856                               Matrix<KLT_TYPE>& bl,
00857                               Matrix<KLT_TYPE>& gx,
00858                               Matrix<KLT_TYPE>& gy) {
00859     int x_floor = (int)floor(x);
00860     int y_floor = (int)floor(y);
00861     int x_floor_end = x_floor+hws+1;
00862     int y_floor_end = y_floor+hws+1;
00863     KLT_TYPE brdy = y - (KLT_TYPE)(y_floor);
00864     KLT_TYPE brdx = x - (KLT_TYPE)(x_floor);
00865     KLT_TYPE brdxy = brdx*brdy;
00866 
00867     int rf, rc, cf, cc, r, c;
00868     for (rf=y_floor-hws, rc=rf+1, r=0; rf<y_floor_end; rf++, rc++, r++) {
00869       for (cf=x_floor-hws, cc=cf+1, c=0; cf<x_floor_end; cf++, cc++, c++) {
00870         KLT_TYPE ul, ur, ll, lr;
00871     
00872         ul = (KLT_TYPE)image[rf][cf];
00873         ur = (KLT_TYPE)image[rf][cc];
00874         ll = (KLT_TYPE)image[rc][cf];
00875         lr = (KLT_TYPE)image[rc][cc];
00876         bl[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00877                     brdxy*(ul + lr - ll - ur));
00878         ul = (KLT_TYPE)(gradx[rf][cf]);
00879         ur = (KLT_TYPE)(gradx[rf][cc]);
00880         ll = (KLT_TYPE)(gradx[rc][cf]);
00881         lr = (KLT_TYPE)(gradx[rc][cc]);
00882         gx[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00883                     brdxy*(ul + lr - ll - ur));
00884         ul = (KLT_TYPE)(grady[rf][cf]);
00885         ur = (KLT_TYPE)(grady[rf][cc]);
00886         ll = (KLT_TYPE)(grady[rc][cf]);
00887         lr = (KLT_TYPE)(grady[rc][cc]);
00888         gy[r][c] = (ul + brdy*(ll - ul) + brdx*(ur - ul) + 
00889                     brdxy*(ul + lr - ll - ur));
00890       }
00891     }
00892     if (_AffineBrightnessInvariance) return NormalizeRegion_(bl, gx, gy);
00893     return 1.0;
00894   }
00895 
00896   template <class StorageType>
00897   inline KLT_TYPE TrackerBaseInterface<StorageType>::
00898   NormalizeRegion_(Matrix<KLT_TYPE>& bl,
00899                    Matrix<KLT_TYPE>& gx,
00900                    Matrix<KLT_TYPE>& gy,
00901                    const KLT_TYPE&  min_value)  {
00902 
00903     // mean value of patch
00904     KLT_TYPE mean = 0;
00905  
00906     int matrixsize = bl.num_cols()*bl.num_rows();
00907     // number of pixels above a certain threshold (for mask tracker)
00908     int validpixels = 0;
00909     
00910     // compute mean on not-black pixels
00911     KLT_TYPE *pbl = bl.GetData()-1, *pblend = bl.GetData() + matrixsize;
00912     while (++pbl<pblend) {
00913       if (*pbl > min_value) {
00914         mean += *pbl;
00915         validpixels++;
00916       } else *pbl = min_value;
00917     }
00918     // normalize mean for stddev computation
00919     if (validpixels>0) mean *= 1.0 / KLT_TYPE(validpixels);
00920 
00921 
00922     // estimate stddev
00923     KLT_TYPE stddev = 0.0;
00924     pbl = bl.GetData()-1;
00925     while (++pbl<pblend) {
00926       if (*pbl > min_value) {
00927         stddev += (*pbl - mean)*(*pbl - mean);
00928       }
00929     }
00930     if (fabs(stddev)>1e-10) { 
00931       stddev = sqrt(KLT_TYPE(validpixels)/stddev);
00932     } else {
00933       stddev = 1.0;
00934     }
00935 
00936     // normalize image and gradient values such that new image patch has 
00937     // zero mean and stddev 1
00938     KLT_TYPE *pgx = gx.GetData(),*pgy = gy.GetData();
00939     pbl = bl.GetData()-1;
00940     while (++pbl<pblend) {
00941       if (*pbl > min_value) {
00942         *pbl  = (*pbl-mean) * stddev;
00943         *pgx *= stddev;
00944         *pgy *= stddev;
00945       } else {
00946         *pgx = *pgy = 0;
00947         *pbl = TRACKERBASE_INVALID_PIXEL;
00948       }
00949       pgx++; pgy++;
00950     }
00951     return stddev;
00952   }
00953 
00954 
00955 } // namespace
00956 
00957 #include <Base/Common/BIASpragmaEnd.hh>
00958 
00959 #endif // __TrackerBaseInterface__hh__
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends