vcs.maemo.org Git - opencv/blob - apps/cvlkdemo/tracker.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*/
  41
  42 #include "tracker.h"
  43
  44
  45 CLkTracker::CLkTracker()
  46 {
  47     m_tracked = m_count = 0;
  48     m_init = false;
  49 }
  50
  51
  52 CLkTracker::~CLkTracker()
  53 {
  54 }
  55
  56
  57 void CLkTracker::Initialize( CImage& image, int max_features, double quality,
  58                              double min_distance )
  59 {
  60     IplImage* img = image.GetImage();
  61
  62     if( img )
  63     {
  64         IplImage* img0 = 0;
  65         IplImage* img1 = 0;
  66
  67         for( int i = 0; i < 2; i++ )
  68         {
  69             //m_features[i].SetSize( max_features, 10 );
  70             m_features[i].reserve(max_features+10);
  71
  72             m_frame[i].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
  73             m_pyr[i].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
  74             m_temp[i].Create( image.Width(), image.Height(), 32, image.GetImage()->origin );
  75             {
  76                 IplImage* tmp_img = m_temp[i].GetImage();
  77                 tmp_img->depth = IPL_DEPTH_32F;
  78                 tmp_img->nChannels = 1;
  79             }
  80         }
  81         //m_status.SetSize( max_features, 10 );
  82         m_status.reserve(max_features+10);
  83         //m_matrices.SetSize( max_features*2, 20 );
  84         m_matrices.reserve(max_features+20);
  85
  86         img0 = m_frame[0].GetImage();
  87         img1 = m_frame[1].GetImage();
  88
  89         m_frame[0].CopyOf( image, 0 );
  90
  91         int count = max_features;
  92         cvGoodFeaturesToTrack( img0, m_temp[0].GetImage(), m_temp[1].GetImage(),
  93                                &m_features[0][0], &count, quality, min_distance );
  94
  95         if( count > 0 )
  96         {
  97             cvFindCornerSubPix( img0, &m_features[0][0], count,
  98                                 cvSize(5,5), cvSize(-1,-1),
  99                                 cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
 100             for( int i = 0; i < m_count; i++ )
 101             {
 102                 m_status[i] = 1;
 103             }
 104             m_tracked = 0;
 105             m_idx = 0;
 106             m_count = count;
 107             m_init = true;
 108         }
 109     }
 110 }
 111
 112
 113 void CLkTracker::PushFrame( CImage& image )
 114 {
 115     static int frame_idx = 0;
 116
 117     IplImage* img = image.GetImage();
 118
 119     if( m_count > 0 && img )
 120     {
 121         int I = m_idx, J = m_idx ^ 1;
 122         int pyr0_ready = m_tracked != 0;
 123
 124         int i, k, count;
 125
 126         assert( m_idx == 0 || m_idx == 1 );
 127
 128         m_frame[I].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
 129         m_frame[J].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
 130         m_pyr[I].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
 131         m_pyr[J].Create( image.Width(), image.Height(), 8, image.GetImage()->origin );
 132
 133         m_frame[J].CopyOf( image, 0 );
 134
 135         IplImage* img0 = m_frame[I].GetImage();
 136         IplImage* img1 = m_frame[J].GetImage();
 137         IplImage* pyr0 = m_pyr[I].GetImage();
 138         IplImage* pyr1 = m_pyr[J].GetImage();
 139
 140         cvCalcOpticalFlowPyrLK( img0, img1, pyr0, pyr1,
 141                                 &m_features[I][0], &m_features[J][0],
 142                                 m_count, cvSize(10,10), 3,
 143                                 &m_status[0], 0,
 144                                 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
 145                                 pyr0_ready ? CV_LKFLOW_PYR_A_READY : 0 );
 146
 147         frame_idx++;
 148
 149         /*cvCalcAffineFlowPyrLK( img0, img1, pyr0, pyr1,
 150                                &m_features[I][0], &m_features[J][0],
 151                                &(m_matrices[0].x),
 152                                m_count, cvSize(5,5), 3,
 153                                &m_status[0], 0,
 154                                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,5,0.3),
 155                                pyr0_ready ? CV_LKFLOW_PYR_A_READY : 0 );*/
 156
 157         count = m_count;
 158         for( i = 0, k = 0; i < count; i++ )
 159         {
 160             if( m_status[i] )
 161             {
 162                 m_features[J][k++] = m_features[J][i];
 163             }
 164             else
 165             {
 166                 m_count--;
 167             }
 168         }
 169
 170         m_tracked++;
 171         m_idx = J;
 172     }
 173     /*
 174     if( img.GetImage() )
 175     {
 176         cvGoodFeaturesToTrack( img.GetImage(), m_temp[0].GetImage(),
 177                                m_temp[1].GetImage(),
 178                                &m_features[0][0], &m_count, 0.1, 10 );
 179     }*/
 180 }
 181
 182
 183
 184 void  CLkTracker::RemovePoint( int index )
 185 {
 186     if( (unsigned)index < (unsigned)m_count )
 187     {
 188         //m_features[m_idx].RemoveAt( index );
 189         m_features[m_idx].erase(m_features[m_idx].begin() + index);
 190
 191         //m_features[m_idx^1].RemoveAt( index );
 192         m_features[m_idx^1].erase(m_features[m_idx^1].begin() + index);
 193
 194         //m_matrices.RemoveAt( m_count*2 - 1 );
 195         m_matrices.erase(m_matrices.begin() + m_count*2 - 1);
 196         //m_matrices.RemoveAt( m_count*2 - 2 );
 197         m_matrices.erase(m_matrices.begin() + m_count*2 - 2);
 198         m_count--;
 199     }
 200 }
 201
 202
 203 void  CLkTracker::AddPoint( CvPoint p, CImage& image )
 204 {
 205     CvPoint2D32f pt = AdjustPoint( p, image );
 206
 207     if( m_count < m_features[m_idx].size() )
 208     {
 209         m_features[m_idx][m_count] = pt;
 210     }
 211     else
 212     {
 213         //m_features[m_idx].Add(pt);
 214         m_features[m_idx].push_back(pt);
 215         //m_features[m_idx].Add(pt);
 216         m_features[m_idx].push_back(pt);
 217         //m_features[m_idx^1].Add(pt);
 218         m_features[m_idx^1].push_back(pt);
 219         //m_matrices.Add(pt);
 220         m_matrices.push_back(pt);
 221         //m_matrices.Add(pt);
 222         m_matrices.push_back(pt);
 223         //m_status.Add(0);
 224         m_status.push_back(0);
 225     }
 226     m_count++;
 227 }
 228
 229
 230 void  CLkTracker::MovePoint( int index, CvPoint p, CImage& image )
 231 {
 232     if((unsigned)index < (unsigned)m_count )
 233     {
 234         CvPoint2D32f pt = AdjustPoint( p, image );
 235         m_features[m_idx][index] = pt;
 236     }
 237 }
 238
 239
 240 CvPoint2D32f  CLkTracker::AdjustPoint( CvPoint p, CImage& image )
 241 {
 242     CvPoint2D32f pt;
 243     pt.x = (float)p.x;
 244     pt.y = (float)p.y;
 245     cvFindCornerSubPix( image.GetImage(), &pt, 1, cvSize(5,5), cvSize(-1,-1),
 246                         cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
 247     return pt;
 248 }