git.maemo.org Git - opencv/blob - apps/LkDemo/LkTracker.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/#include "stdafx.h"
  41
  42 #include "LkTracker.h"
  43
  44
  45 CLkTracker::CLkTracker()
  46 {
  47     m_tracked = m_count = 0;
  48 }
  49
  50
  51 CLkTracker::~CLkTracker()
  52 {
  53 }
  54
  55
  56 void CLkTracker::Initialize( CImage& image, int max_features, double quality,
  57                              double min_distance )
  58 {
  59     IplImage* img = image.GetImage();
  60
  61     if( img )
  62     {
  63         IplImage* img0 = 0;
  64         IplImage* img1 = 0;
  65
  66         for( int i = 0; i < 2; i++ )
  67         {
  68             m_features[i].SetSize( max_features, 10 );
  69             m_frame[i].Create( image.Width(), image.Height(), 8 );
  70             m_pyr[i].Create( image.Width(), image.Height(), 8 );
  71             m_temp[i].Create( image.Width(), image.Height(), 32 );
  72             {
  73                 IplImage* tmp_img = m_temp[i].GetImage();
  74                 tmp_img->depth = IPL_DEPTH_32F;
  75                 tmp_img->nChannels = 1;
  76             }
  77         }
  78         m_status.SetSize( max_features, 10 );
  79         m_matrices.SetSize( max_features*2, 20 );
  80
  81         img0 = m_frame[0].GetImage();
  82         img1 = m_frame[1].GetImage();
  83
  84         m_frame[0].CopyOf( image, 0 );
  85
  86         int count = max_features;
  87         cvGoodFeaturesToTrack( img0, m_temp[0].GetImage(), m_temp[1].GetImage(),
  88                                &m_features[0][0], &count, quality, min_distance );
  89
  90         if( count > 0 )
  91         {
  92             cvFindCornerSubPix( img0, &m_features[0][0], count,
  93                                 cvSize(5,5), cvSize(-1,-1),
  94                                 cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
  95             for( int i = 0; i < m_count; i++ )
  96             {
  97                 m_status[i] = 1;
  98             }
  99             m_tracked = 0;
 100             m_idx = 0;
 101             m_count = count;
 102         }
 103     }
 104 }
 105
 106
 107 void CLkTracker::PushFrame( CImage& image )
 108 {
 109     static int frame_idx = 0;
 110
 111     IplImage* img = image.GetImage();
 112
 113     if( m_count > 0 && img )
 114     {
 115         int I = m_idx, J = m_idx ^ 1;
 116         int pyr0_ready = m_tracked != 0;
 117
 118         int i, k, count;
 119
 120         assert( m_idx == 0 || m_idx == 1 );
 121
 122         m_frame[I].Create( image.Width(), image.Height(), 8 );
 123         m_frame[J].Create( image.Width(), image.Height(), 8 );
 124         m_pyr[I].Create( image.Width(), image.Height(), 8 );
 125         m_pyr[J].Create( image.Width(), image.Height(), 8 );
 126
 127         m_frame[J].CopyOf( image, 0 );
 128
 129         IplImage* img0 = m_frame[I].GetImage();
 130         IplImage* img1 = m_frame[J].GetImage();
 131         IplImage* pyr0 = m_pyr[I].GetImage();
 132         IplImage* pyr1 = m_pyr[J].GetImage();
 133
 134         cvCalcOpticalFlowPyrLK( img0, img1, pyr0, pyr1,
 135                                 &m_features[I][0], &m_features[J][0],
 136                                 m_count, cvSize(10,10), 3,
 137                                 &m_status[0], 0,
 138                                 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
 139                                 pyr0_ready ? CV_LKFLOW_PYR_A_READY : 0 );
 140
 141         frame_idx++;
 142
 143         /*cvCalcAffineFlowPyrLK( img0, img1, pyr0, pyr1,
 144                                &m_features[I][0], &m_features[J][0],
 145                                &(m_matrices[0].x),
 146                                m_count, cvSize(5,5), 3,
 147                                &m_status[0], 0,
 148                                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,5,0.3),
 149                                pyr0_ready ? CV_LKFLOW_PYR_A_READY : 0 );*/
 150
 151         count = m_count;
 152         for( i = 0, k = 0; i < count; i++ )
 153         {
 154             if( m_status[i] )
 155             {
 156                 m_features[J][k++] = m_features[J][i];
 157             }
 158             else
 159             {
 160                 m_count--;
 161             }
 162         }
 163
 164         m_tracked++;
 165         m_idx = J;
 166     }
 167     /*
 168     if( img.GetImage() )
 169     {
 170         cvGoodFeaturesToTrack( img.GetImage(), m_temp[0].GetImage(),
 171                                m_temp[1].GetImage(),
 172                                &m_features[0][0], &m_count, 0.1, 10 );
 173     }*/
 174 }
 175
 176
 177
 178 void  CLkTracker::RemovePoint( int index )
 179 {
 180     if( (unsigned)index < (unsigned)m_count )
 181     {
 182         m_features[m_idx].RemoveAt( index );
 183         m_features[m_idx^1].RemoveAt( index );
 184         m_matrices.RemoveAt( m_count*2 - 1 );
 185         m_matrices.RemoveAt( m_count*2 - 2 );
 186         m_count--;
 187     }
 188 }
 189
 190
 191 void  CLkTracker::AddPoint( CPoint p, CImage& image )
 192 {
 193     CvPoint2D32f pt = AdjustPoint( p, image );
 194
 195     if( m_count < m_features[m_idx].GetSize() )
 196     {
 197         m_features[m_idx][m_count] = pt;
 198     }
 199     else
 200     {
 201         m_features[m_idx].Add(pt);
 202         m_features[m_idx^1].Add(pt);
 203         m_matrices.Add(pt);
 204         m_matrices.Add(pt);
 205         m_status.Add(0);
 206     }
 207     m_count++;
 208 }
 209
 210
 211 void  CLkTracker::MovePoint( int index, CPoint p, CImage& image )
 212 {
 213     if((unsigned)index < (unsigned)m_count )
 214     {
 215         CvPoint2D32f pt = AdjustPoint( p, image );
 216         m_features[m_idx][index] = pt;
 217     }
 218 }
 219
 220
 221 CvPoint2D32f  CLkTracker::AdjustPoint( CPoint p, CImage& image )
 222 {
 223     CvPoint2D32f pt;
 224     pt.x = (float)p.x;
 225     pt.y = (float)p.y;
 226     cvFindCornerSubPix( image.GetImage(), &pt, 1, cvSize(5,5), cvSize(-1,-1),
 227                         cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
 228     return pt;
 229 }