vcs.maemo.org Git - opencv/blob - filters/Kalman/Kalman.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 #include <cvstreams.h>
  42 #include <initguid.h>
  43 #include <olectl.h>
  44 #if (1100 > _MSC_VER)
  45 #include <olectlid.h>
  46 #endif
  47 #include "iKalman.h"
  48 #include "Kalmanprop.h"
  49 #include "Kalman.h"
  50 #include "Kalmanuids.h"
  51 #include <math.h>
  52
  53 // setup data
  54 const AMOVIESETUP_MEDIATYPE sudPinTypes =
  55 {
  56     &MEDIATYPE_Video,       // Major type
  57     &MEDIASUBTYPE_RGB24      // Minor type
  58 };
  59
  60 const AMOVIESETUP_PIN psudPins[] =
  61 {
  62     {
  63         L"Input",           // String pin name
  64         FALSE,              // Is it rendered
  65         FALSE,              // Is it an output
  66         FALSE,              // Allowed none
  67         FALSE,              // Allowed many
  68         &CLSID_NULL,        // Connects to filter
  69         L"Output",          // Connects to pin
  70         1,                  // Number of types
  71         &sudPinTypes },     // The pin details
  72       { L"Output",          // String pin name
  73         FALSE,              // Is it rendered
  74         TRUE,               // Is it an output
  75         FALSE,              // Allowed none
  76         FALSE,              // Allowed many
  77         &CLSID_NULL,        // Connects to filter
  78         L"Input",           // Connects to pin
  79         1,                  // Number of types
  80         &sudPinTypes        // The pin details
  81     }
  82 };
  83
  84
  85 const AMOVIESETUP_FILTER sudCKalmTrack =
  86 {
  87     &CLSID_CKalmTrack,        // Filter CLSID
  88     L"Kalman",                    // Filter name
  89     MERIT_DO_NOT_USE,               // Its merit
  90     2,                              // Number of pins
  91     psudPins                        // Pin details
  92 };
  93
  94
  95 // List of class IDs and creator functions for the class factory. This
  96 // provides the link between the OLE entry point in the DLL and an object
  97 // being created. The class factory will call the static CreateInstance
  98
  99 CFactoryTemplate g_Templates[2] = {
 100
 101     { L"Kalman"
 102     , &CLSID_CKalmTrack
 103     , CKalmTrack::CreateInstance
 104     , NULL
 105     , &sudCKalmTrack }
 106   ,
 107     { L"Kalman Property Page"
 108     , &CLSID_CKalmTrackPropertyPage
 109     , CKalmTrackProperties::CreateInstance }
 110 };
 111 int g_cTemplates = sizeof(g_Templates) / sizeof(g_Templates[0]);
 112
 113
 114
 115 //
 116 // Constructor
 117 //
 118 CKalmTrack::CKalmTrack(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
 119     CTransInPlaceFilter(tszName, punk, CLSID_CKalmTrack,phr)
 120
 121 {
 122     m_params.x = 0.4f;
 123     m_params.y = 0.3f;
 124     m_params.width = 0.2f;
 125     m_params.height = 0.3f;
 126
 127     m_params.Smin = 20;
 128     m_params.Vmin = 40;
 129     m_params.view = 0;
 130
 131     IsTracking = false;
 132     IsInit =false;
 133     Kalman = cvCreateKalman(4,4);
 134     CvMat Dyn = cvMat(4,4,CV_MAT4x4_32F,Kalman->DynamMatr);
 135     CvMat Mes = cvMat(4,4,CV_MAT4x4_32F,Kalman->MeasurementMatr);
 136     CvMat PNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->PNCovariance);
 137     CvMat MNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->MNCovariance);
 138     CvMat PriErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PriorErrorCovariance);
 139     CvMat PostErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PosterErrorCovariance);
 140     CvMat PriState = cvMat(4,1,CV_MAT4x1_32F,Kalman->PriorState);
 141     cvmSetIdentity(&PNC);
 142     cvmSetIdentity(&PriErr);
 143     cvmSetIdentity(&PostErr);
 144     cvmSetZero(&MNC);
 145     cvmSetZero(&PriState);
 146     cvmSetIdentity(&Dyn);
 147     cvmSet(&Dyn,0,1,0.9f);
 148     cvmSet(&Dyn,2,3,0.9f);
 149     cvmSetIdentity(&Mes);
 150     MVect = cvMat(4,1,CV_MAT4x1_32F,Measurement);
 151     ASSERT(tszName);
 152     ASSERT(phr);
 153 } // CKalmTrack
 154
 155 //Destructor
 156 CKalmTrack::~CKalmTrack()
 157 {
 158     cvReleaseKalman(&Kalman);
 159 }//~CKalmTrack
 160
 161 //
 162 // CreateInstance
 163 //
 164 // Provide the way for COM to create a CKalmTrack object
 165 //
 166 CUnknown * WINAPI CKalmTrack::CreateInstance(LPUNKNOWN punk, HRESULT *phr) {
 167
 168     CKalmTrack *pNewObject = new CKalmTrack(NAME("Kalman"), punk, phr);
 169     if (pNewObject == NULL) {
 170         *phr = E_OUTOFMEMORY;
 171     }
 172     return pNewObject;
 173
 174 } // CreateInstance
 175
 176
 177 //
 178 // NonDelegatingQueryInterface
 179 //
 180 // Reveals ICKalmTrack and ISpecifyPropertyPages
 181 //
 182 STDMETHODIMP CKalmTrack::NonDelegatingQueryInterface(REFIID riid, void **ppv)
 183 {
 184     CheckPointer(ppv,E_POINTER);
 185
 186     if (riid == IID_ICKalmTrack) {
 187         return GetInterface((ICKalmTrack *) this, ppv);
 188     } else if (riid == IID_ISpecifyPropertyPages) {
 189         return GetInterface((ISpecifyPropertyPages *) this, ppv);
 190     } else {
 191         return CTransInPlaceFilter::NonDelegatingQueryInterface(riid, ppv);
 192     }
 193
 194 } // NonDelegatingQueryInterface
 195
 196
 197 void  CKalmTrack::ApplyCamShift( CvImage* image, bool initialize )
 198 {
 199     CvSize size;
 200     int bins = 20;
 201
 202     m_cCamShift.set_hist_dims( 1, &bins );
 203     m_cCamShift.set_hist_bin_range( 0, 1, 180 );
 204     m_cCamShift.set_min_ch_val( 1, m_params.Smin );
 205     m_cCamShift.set_max_ch_val( 1, 255 );
 206     m_cCamShift.set_min_ch_val( 2, m_params.Vmin );
 207     m_cCamShift.set_max_ch_val( 2, 255 );
 208
 209     cvGetImageRawData( image, 0, 0, &size );
 210
 211     if( m_object.x < 0 ) m_object.x = 0;
 212     if( m_object.x > size.width - m_object.width - 1 )
 213         m_object.x = size.width - m_object.width - 1;
 214
 215     if( m_object.y < 0 ) m_object.y = 0;
 216     if( m_object.y > size.height - m_object.height - 1 )
 217         m_object.y = size.height - m_object.height - 1;
 218
 219     m_cCamShift.set_window(m_object);
 220
 221     if( initialize )
 222     {
 223         m_cCamShift.reset_histogram();
 224         m_cCamShift.update_histogram( image );
 225     }
 226
 227     m_cCamShift.track_object( image );
 228     m_object = m_cCamShift.get_window();
 229
 230     Measurement[0] = (float)m_object.x + m_object.width*0.5f;
 231     Measurement[1] = initialize ? 0 : Measurement[0] - m_Old.x;
 232     Measurement[2] = (float)m_object.y + m_object.height*0.5f;
 233     Measurement[3] = initialize ? 0 : Measurement[2] - m_Old.y;
 234
 235     m_Old.x = cvRound( Measurement[0] );
 236     m_Old.y = cvRound( Measurement[2] );
 237     cvKalmanUpdateByMeasurement(Kalman,&MVect);
 238 }
 239
 240
 241 void  CKalmTrack::CheckBackProject( CvImage* image )
 242 {
 243     if( m_params.view != 0 )
 244     {
 245         IplImage* src = m_cCamShift.get_back_project();
 246         if( src && src->imageData && image )
 247         {
 248             cvCvtColor( src, image, CV_GRAY2BGR );
 249         }
 250     }
 251 }
 252
 253 //
 254 // Transform
 255 // Transform the  sample 'in place'
 256 //
 257 HRESULT CKalmTrack::Transform(IMediaSample *pSample)
 258 {
 259     BYTE*   pData;
 260     CvImage image;
 261
 262     pSample->GetPointer(&pData);
 263
 264     AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType();
 265     VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat;
 266
 267     // Get the image properties from the BITMAPINFOHEADER
 268     CvSize size = cvSize( pvi->bmiHeader.biWidth, pvi->bmiHeader.biHeight );
 269     int stride = (size.width * 3 + 3) & -4;
 270
 271     cvInitImageHeader( &image, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 272     cvSetImageData( &image, pData,stride );
 273
 274     if(IsTracking == false)
 275     {
 276         if(IsInit == false)
 277         {
 278             CvPoint p1, p2;
 279             // Draw box
 280             p1.x = cvRound( size.width * m_params.x );
 281             p1.y = cvRound( size.height * m_params.y );
 282
 283             p2.x = cvRound( size.width * (m_params.x + m_params.width));
 284             p2.y = cvRound( size.height * (m_params.y + m_params.height));
 285
 286             CheckBackProject( &image );
 287
 288             cvRectangle( &image, p1, p2, -1, 1 );
 289         }
 290         else
 291         {
 292             m_object.x = cvRound( size.width * m_params.x );
 293             m_object.y = cvRound( size.height * m_params.y );
 294             m_object.width = cvRound( size.width * m_params.width );
 295             m_object.height = cvRound( size.height * m_params.height );
 296             ApplyCamShift( &image, true );
 297
 298             CheckBackProject( &image );
 299
 300             IsTracking = true;
 301         }
 302     }
 303     else
 304     {
 305         cvKalmanUpdateByTime(Kalman);
 306         m_object.x = cvRound( Kalman->PriorState[0]-m_object.width*0.5);
 307         m_object.y = cvRound( Kalman->PriorState[2]-m_object.height*0.5 );
 308
 309         ApplyCamShift( &image, false );
 310
 311         CheckBackProject( &image );
 312
 313         cvRectangle( &image,
 314                      cvPoint( m_object.x, m_object.y ),
 315                      cvPoint( m_object.x + m_object.width, m_object.y + m_object.height ),
 316                      -1, 1 );
 317
 318         Rectang(&image,m_Indicat1,-1);
 319         m_X.x = 10;
 320         m_X.y = 10;
 321         m_X.width=50*m_Old.x/size.width;
 322         m_X.height =10;
 323         Rectang(&image,m_X,CV_RGB(0,0,255));
 324         m_Y.x = 10;
 325         m_Y.y = 10;
 326         m_Y.width=10;
 327         m_Y.height = 50*m_Old.y/size.height;
 328         Rectang(&image,m_Y,CV_RGB(255,0,0));
 329         m_Indicat2.x = 0;
 330         m_Indicat2.y = size.height-50;
 331         m_Indicat2.width = 50;
 332         m_Indicat2.height = 50;
 333         Rectang(&image,m_Indicat2,-1);
 334         float Norm = cvSqrt(Measurement[1]*Measurement[1]+Measurement[3]*Measurement[3]);
 335         int VXNorm = (fabs(Measurement[1])>5)?(int)(12*Measurement[1]/Norm):0;
 336         int VYNorm = (fabs(Measurement[3])>5)?(int)(12*Measurement[3]/Norm):0;
 337         CvPoint pp1 = {25,size.height-25};
 338         CvPoint pp2 = {25+VXNorm,size.height-25+VYNorm};
 339         cvLine(&image,pp1,pp2,CV_RGB(0,0,0),3);
 340         /*CvPoint pp1 = {25,size.height-25};
 341         double angle = atan2( Measurement[3], Measurement[1] );
 342         CvPoint pp2 = {cvRound(25+12*cos(angle)),cvRound(size.height-25-12*sin(angle))};
 343         cvLine(&image,pp1,pp2,0,3);*/
 344     }
 345
 346     cvSetImageData( &image, 0, 0 );
 347
 348     return NOERROR;
 349 } // Transform
 350
 351
 352 //
 353 // CheckInputType
 354 //
 355 // Check the input type is OK, return an error otherwise
 356 //
 357 HRESULT CKalmTrack::CheckInputType(const CMediaType *mtIn)
 358 {
 359     // Check this is a VIDEOINFO type
 360
 361     if (*mtIn->FormatType() != FORMAT_VideoInfo)
 362     {
 363
 364     }
 365     if ((IsEqualGUID(*mtIn->Type(), MEDIATYPE_Video))
 366         && (IsEqualGUID(*mtIn->Subtype(), MEDIASUBTYPE_RGB24)))return NOERROR;
 367     return E_INVALIDARG;
 368
 369
 370 } // CheckInputType
 371
 372
 373
 374 // CheckTransform
 375 //
 376 // To be able to transform the formats must be identical
 377 //
 378
 379 HRESULT CKalmTrack::CheckTransform(const CMediaType *mtIn,const CMediaType *mtOut)
 380 {
 381     HRESULT hr;
 382     if (FAILED(hr = CheckInputType(mtIn))) {
 383     return hr;
 384     }
 385
 386     // format must be a VIDEOINFOHEADER
 387     if (*mtOut->FormatType() != FORMAT_VideoInfo) {
 388     return E_INVALIDARG;
 389     }
 390
 391     // formats must be big enough
 392     if (mtIn->FormatLength() < sizeof(VIDEOINFOHEADER) ||
 393     mtOut->FormatLength() < sizeof(VIDEOINFOHEADER))
 394     return E_INVALIDARG;
 395
 396     VIDEOINFO *pInput = (VIDEOINFO *) mtIn->Format();
 397     VIDEOINFO *pOutput = (VIDEOINFO *) mtOut->Format();
 398     if (memcmp(&pInput->bmiHeader,&pOutput->bmiHeader,sizeof(BITMAPINFOHEADER)) == 0) {
 399     return NOERROR;
 400     }
 401
 402     return E_INVALIDARG;
 403 } // CheckTransform
 404
 405
 406 //
 407 // DecideBufferSize
 408 //
 409 // Tell the output pin's allocator what size buffers we
 410 // require. Can only do this when the input is connected
 411 //
 412 HRESULT CKalmTrack::DecideBufferSize(IMemAllocator *pAlloc,ALLOCATOR_PROPERTIES *pProperties)
 413 {
 414     // Is the input pin connected
 415
 416     if (m_pInput->IsConnected() == FALSE) {
 417         return E_UNEXPECTED;
 418     }
 419
 420     ASSERT(pAlloc);
 421     ASSERT(pProperties);
 422     return NOERROR;
 423
 424 } // DecideBufferSize
 425
 426
 427 //
 428 // GetMediaType
 429 //
 430 // I support one type, namely the type of the input pin
 431 // We must be connected to support the single output type
 432 //
 433 HRESULT CKalmTrack::GetMediaType(int iPosition, CMediaType *pMediaType)
 434 {
 435     // Is the input pin connected
 436
 437     if (m_pInput->IsConnected() == FALSE) {
 438         return E_UNEXPECTED;
 439     }
 440
 441     // This should never happen
 442
 443     if (iPosition < 0) {
 444         return E_INVALIDARG;
 445     }
 446
 447     // Do we have more items to offer
 448
 449     if (iPosition > 0) {
 450         return VFW_S_NO_MORE_ITEMS;
 451     }
 452
 453     *pMediaType = m_pInput->CurrentMediaType();
 454     return NOERROR;
 455
 456 } // GetMediaType
 457
 458
 459 STDMETHODIMP CKalmTrack::GetState(bool* State)
 460 {
 461     *State = IsTracking;
 462     return NOERROR;
 463 }
 464
 465
 466 STDMETHODIMP CKalmTrack::SetParams(CvKalmanParams* params)
 467 {
 468     CAutoLock cAutoLock(&m_cKalmTrackLock);
 469
 470     if( params )
 471     {
 472         m_params = *params;
 473     }
 474
 475     return NOERROR;
 476 }
 477
 478
 479 STDMETHODIMP CKalmTrack::GetParams(CvKalmanParams* params)
 480 {
 481     if( params )
 482     {
 483         *params = m_params;
 484     }
 485
 486     return NOERROR;
 487 }
 488
 489
 490 STDMETHODIMP CKalmTrack::StartTracking()
 491 {
 492     CAutoLock cAutoLock(&m_cKalmTrackLock);
 493     IsTracking = false;
 494     IsInit = true;
 495     return NOERROR;
 496 }
 497
 498 STDMETHODIMP CKalmTrack::StopTracking()
 499 {
 500     CAutoLock cAutoLock(&m_cKalmTrackLock);
 501
 502     IsTracking = false;
 503     IsInit = false;
 504     return NOERROR;
 505 }
 506
 507
 508 //
 509 // GetPages
 510 //
 511 // This is the sole member of ISpecifyPropertyPages
 512 // Returns the clsid's of the property pages we support
 513 //
 514
 515 STDMETHODIMP CKalmTrack::GetPages(CAUUID *pPages)
 516 {
 517     pPages->cElems = 1;
 518     pPages->pElems = (GUID *) CoTaskMemAlloc(sizeof(GUID));
 519     if (pPages->pElems == NULL) {
 520         return E_OUTOFMEMORY;
 521     }
 522     *(pPages->pElems) = CLSID_CKalmTrackPropertyPage;
 523     return NOERROR;
 524
 525 } // GetPages
 526
 527
 528 CKalmTrack::Rectang(IplImage* img,CvRect Rect, int color)
 529 {
 530     for( int j = 0; j < Rect.height; j++ )
 531     {
 532         CvPoint p1={Rect.x,Rect.y+j};
 533         CvPoint p2={Rect.x+Rect.width,Rect.y+j};
 534         cvLine(img,p1,p2,color);
 535     }
 536 }
 537
 538
 539 //
 540 // DllRegisterServer
 541 //
 542 // Handle registration of this filter
 543 //
 544 STDAPI DllRegisterServer()
 545 {
 546     return AMovieDllRegisterServer2( TRUE );
 547
 548 } // DllRegisterServer
 549
 550
 551 //
 552 // DllUnregisterServer
 553 //
 554 STDAPI DllUnregisterServer()
 555 {
 556    return AMovieDllRegisterServer2( FALSE );
 557
 558 } // DllUnregisterServer