git.maemo.org Git - opencv/blob - filters/CalibFilter/CalibFilter.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 #pragma warning( disable: 4201 4710 )
  43
  44 #define MIRROR_POINTS
  45
  46 #include <windows.h>
  47 #include <cvstreams.h>
  48 #include <initguid.h>
  49 #include <olectl.h>
  50 #if (1100 > _MSC_VER)
  51 #include <olectlid.h>
  52 #endif
  53 #include "iCalibFilter.h"
  54 #include "CalibFilterprop.h"
  55 #include "CalibFilter.h"
  56 #include "CalibFilteruids.h"
  57 #include <assert.h>
  58 #include "math.h"
  59 #include <stdio.h>
  60 #include "Calib3DWindow.h"
  61 #include "CV.h"
  62
  63 // setup data
  64 const AMOVIESETUP_MEDIATYPE sudPinTypes =
  65 {
  66     &MEDIATYPE_Video,       // Major type
  67     &MEDIASUBTYPE_NULL      // Minor type
  68 };
  69
  70 const AMOVIESETUP_PIN psudPins[] =
  71 {
  72     {
  73         L"Input",           // String pin name
  74         FALSE,              // Is it rendered
  75         FALSE,              // Is it an output
  76         FALSE,              // Allowed none
  77         FALSE,              // Allowed many
  78         &CLSID_NULL,        // Connects to filter
  79         L"Output",          // Connects to pin
  80         1,                  // Number of types
  81         &sudPinTypes },     // The pin details
  82       { L"Output",          // String pin name
  83         FALSE,              // Is it rendered
  84         TRUE,               // Is it an output
  85         FALSE,              // Allowed none
  86         FALSE,              // Allowed many
  87         &CLSID_NULL,        // Connects to filter
  88         L"Input",           // Connects to pin
  89         1,                  // Number of types
  90         &sudPinTypes        // The pin details
  91     }
  92 };
  93
  94
  95 const AMOVIESETUP_FILTER sudCCalibFilter =
  96 {
  97     &CLSID_CCalibFilter,        // Filter CLSID
  98     L"CalibFilter",                    // Filter name
  99     MERIT_DO_NOT_USE,               // Its merit
 100     2,                              // Number of pins
 101     psudPins                        // Pin details
 102 };
 103
 104
 105 // List of class IDs and creator functions for the class factory. This
 106 // provides the link between the OLE entry point in the DLL and an object
 107 // being created. The class factory will call the static CreateInstance
 108
 109 CFactoryTemplate g_Templates[2] = {
 110
 111     { L"CalibFilter"
 112     , &CLSID_CCalibFilter
 113     , CCalibFilter::CreateInstance
 114     , NULL
 115     , &sudCCalibFilter }
 116   ,
 117     { L"CalibFilter Property Page"
 118     , &CLSID_CCalibFilterPropertyPage
 119     , CCalibFilterProperties::CreateInstance }
 120 };
 121 int g_cTemplates = sizeof(g_Templates) / sizeof(g_Templates[0]);
 122
 123 static DWORD WINAPI _3DWindowThreadProc( void* window );
 124
 125 /* Constructor */
 126 CCalibFilter::CCalibFilter(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
 127         CTransInPlaceFilter(tszName, punk, CLSID_CCalibFilter,phr),
 128         CPersistStream(punk, phr)
 129 {
 130     m_initial_params.etalon_type = CalibEtalon_ChessBoard;
 131
 132     m_initial_params.etalon_params[0] = 6;
 133     m_initial_params.etalon_params[1] = 8;
 134     m_initial_params.etalon_params[2] = 3;
 135     m_initial_params.etalon_params_count = 3;
 136
 137     m_initial_params.show_feature_points_flag = 1;
 138     m_initial_params.frame_interval = 1000;
 139     m_initial_params.frames_to_collect = 10;
 140     m_initial_params.frames_collected = 0;
 141     m_initial_params.frames_passed = 0;
 142     m_initial_params.calib_state = CalibState_Initial;
 143     m_initial_params.last_frame_time = -1e6f;
 144
 145     m_initial_params.enable_undistortion    = 0;
 146     m_initial_params.show_3d_window         = 1;
 147
 148     m_params = m_initial_params;
 149
 150     m_max_points = 0;
 151     m_imagePoints = 0;
 152     m_objectPoints = 0;
 153     m_transVects = 0;
 154     m_rotMatrs = 0;
 155
 156     m_gray_img   = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 1 );
 157     m_thresh_img = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 1 );
 158     m_rgb_img    = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 3 );
 159     m_undist_img = cvCreateImage( cvSize(1,1), IPL_DEPTH_8U, 3 );
 160
 161     memset( &m_undistort_params, 0, sizeof(m_undistort_params));
 162     m_undistort_data = 0;
 163
 164     m_window3D = 0;
 165
 166     DWORD threadId;
 167     m_thread = CreateThread( 0, 0, _3DWindowThreadProc, &m_window3D, 0, &threadId );
 168 }
 169
 170 CvSize  CCalibFilter::GetEtalonSize()
 171 {
 172     return cvSize( cvRound(m_params.etalon_params[0]) - 1,
 173                       cvRound(m_params.etalon_params[1]) - 1 );
 174 }
 175
 176 /* a whole life of 3D window in the single routine */
 177 DWORD WINAPI _3DWindowThreadProc( void* window )
 178 {
 179     CCalib3DWindow* window3D = new CCalib3DWindow;
 180     *((CCalib3DWindow**)window) = window3D;
 181
 182     MSG msg;
 183
 184     // Main message loop:
 185     while( GetMessage(&msg, NULL, 0, 0))
 186     {
 187         TranslateMessage(&msg);
 188         DispatchMessage(&msg);
 189     }
 190
 191     *((CCalib3DWindow**)window) = 0;
 192     delete window3D;
 193     return 0;
 194 }
 195
 196 CCalibFilter::~CCalibFilter()
 197 {
 198     SendMessage( m_window3D->m_hwnd, WM_CLOSE, 0, 0 );
 199     WaitForSingleObject( m_thread, 100 );
 200     CloseHandle( m_thread );
 201 }
 202
 203
 204 /* CreateInstance */
 205 CUnknown * WINAPI CCalibFilter::CreateInstance(LPUNKNOWN punk, HRESULT *phr)
 206 {
 207     CCalibFilter *pNewObject = new CCalibFilter(NAME("CalibFilter"), punk, phr);
 208     if( !pNewObject ) *phr = E_OUTOFMEMORY;
 209     return pNewObject;
 210 }
 211
 212 /* NonDelegatingQueryInterface */
 213 STDMETHODIMP CCalibFilter::NonDelegatingQueryInterface(REFIID riid, void **ppv)
 214 {
 215     CheckPointer(ppv,E_POINTER);
 216
 217     if( riid == IID_ICalibFilter )
 218         return GetInterface((ICalibFilter *) this, ppv);
 219     else if( riid == IID_ISpecifyPropertyPages )
 220         return GetInterface((ISpecifyPropertyPages *) this, ppv);
 221     else if( riid == IID_IPersistStream )
 222         return GetInterface((IPersistStream *) this, ppv);
 223     else
 224         return CTransInPlaceFilter::NonDelegatingQueryInterface(riid, ppv);
 225 }
 226
 227
 228 /* CheckReallocBuffers */
 229 void  CCalibFilter::CheckReallocBuffers( IplImage* rgb_img )
 230 {
 231     CvSize etalon_size = GetEtalonSize();
 232     int etalon_points = etalon_size.width*etalon_size.height;
 233
 234     if( m_gray_img->imageData == 0 ||
 235         m_gray_img->width != rgb_img->width ||
 236         m_gray_img->height != rgb_img->height )
 237     {
 238         cvReleaseImageData( m_gray_img );
 239         cvInitImageHeader( m_gray_img, cvSize(rgb_img->width, rgb_img->height),
 240                             IPL_DEPTH_8U, 1, IPL_ORIGIN_TL, 4 );
 241         cvCreateImageData( m_gray_img );
 242
 243         cvReleaseImageData( m_thresh_img );
 244         cvInitImageHeader( m_thresh_img, cvSize(rgb_img->width, rgb_img->height),
 245                             IPL_DEPTH_8U, 1, IPL_ORIGIN_TL, 4 );
 246         cvCreateImageData( m_thresh_img );
 247
 248         cvReleaseImageData( m_undist_img );
 249         cvInitImageHeader( m_undist_img, cvSize(rgb_img->width, rgb_img->height),
 250                             IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 251
 252         cvCreateImageData( m_undist_img );
 253     }
 254
 255     if( etalon_points * m_params.frames_to_collect > m_max_points )
 256     {
 257         int  new_max_points = etalon_points * (m_params.frames_to_collect + 1) + 128;
 258         CvPoint2D32f* imagePoints = (CvPoint2D32f*)malloc( new_max_points *
 259                                                            sizeof(CvPoint2D32f));
 260
 261         memcpy( imagePoints, m_imagePoints, m_max_points * sizeof(CvPoint2D32f));
 262
 263         free( m_imagePoints );
 264         free( m_objectPoints );
 265         free( m_transVects );
 266         free( m_rotMatrs );
 267
 268         m_imagePoints  = imagePoints;
 269         m_objectPoints  = (CvPoint3D32f*)malloc( new_max_points * sizeof(CvPoint3D32f) );
 270         m_transVects = (CvPoint3D32f*)malloc( new_max_points * sizeof(CvPoint2D32f) );
 271         m_rotMatrs = (float*)malloc( new_max_points * 9 * sizeof(float));
 272         m_numsPoints = (int*)calloc( m_params.frames_to_collect,sizeof(int));
 273
 274         m_max_points = new_max_points;
 275     }
 276 }
 277
 278
 279 void  CCalibFilter::DrawEtalon( IplImage* rgb_img, CvPoint2D32f* corners,
 280                                 int corner_count, CvSize etalon_size,
 281                                 int draw_ordered )
 282 {
 283     const int r = 4;
 284     int i;
 285
 286     if( corner_count == etalon_size.width * etalon_size.height && draw_ordered )
 287     {
 288         int x, y;
 289         CvPoint prev_pt = { 0, 0};
 290         const int line_max = 7;
 291         CvScalar line_colors[line_max];
 292
 293         line_colors[0] = CV_RGB(255,0,0);
 294         line_colors[1] = CV_RGB(255,128,0);
 295         line_colors[2] = CV_RGB(200,200,0);
 296         line_colors[3] = CV_RGB(0,255,0);
 297         line_colors[4] = CV_RGB(0,200,200);
 298         line_colors[5] = CV_RGB(0,0,255);
 299         line_colors[6] = CV_RGB(255,0,255);
 300
 301         for( y = 0, i = 0; y < etalon_size.height; y++ )
 302         {
 303             CvScalar color = line_colors[y % line_max];
 304             for( x = 0; x < etalon_size.width; x++, i++ )
 305             {
 306                 CvPoint pt;
 307                 pt.x = cvRound(corners[i].x);
 308                 pt.y = cvRound(corners[i].y);
 309
 310                 if( i != 0 )
 311                     cvLine( rgb_img, prev_pt, pt, color, 1, CV_AA, 0 );
 312
 313                 cvLine( rgb_img, cvPoint( pt.x - r, pt.y - r ),
 314                         cvPoint( pt.x + r, pt.y + r ), color, 1, CV_AA, 0 );
 315                 cvLine( rgb_img, cvPoint( pt.x - r, pt.y + r),
 316                         cvPoint( pt.x + r, pt.y - r), color, 1, CV_AA, 0 );
 317                 cvCircle( rgb_img, pt, r+1, color, 1, CV_AA, 0 );
 318                 prev_pt = pt;
 319             }
 320         }
 321     }
 322     else
 323     {
 324         CvScalar color = CV_RGB(255,0,0);
 325         for( i = 0; i < corner_count; i++ )
 326         {
 327             CvPoint pt;
 328             pt.x = cvRound(corners[i].x);
 329             pt.y = cvRound(corners[i].y);
 330             cvLine( rgb_img, cvPoint( pt.x - r, pt.y - r ),
 331                     cvPoint( pt.x + r, pt.y + r ), color, 1, CV_AA, 0 );
 332             cvLine( rgb_img, cvPoint( pt.x - r, pt.y + r),
 333                     cvPoint( pt.x + r, pt.y - r), color, 1, CV_AA, 0 );
 334             cvCircle( rgb_img, pt, r+1, color, 1, CV_AA, 0 );
 335         }
 336     }
 337 }
 338
 339
 340 void  CCalibFilter::FillEtalonObjPoints( CvPoint3D32f* obj_points,
 341                                          CvSize etalon_size,
 342                                          float square_size )
 343 {
 344     int x, y, i;
 345
 346     for( y = 0, i = 0; y < etalon_size.height; y++ )
 347     {
 348         for( x = 0; x < etalon_size.width; x++, i++ )
 349         {
 350             obj_points[i].x = square_size * x;
 351             obj_points[i].y = square_size * y;
 352             obj_points[i].z = 0;
 353         }
 354     }
 355 }
 356
 357
 358 #ifdef MIRROR_POINTS
 359 void MirrorPoints( CvPoint2D32f* points, int frames, CvSize etalon_size, CvSize imgSize )
 360 {
 361     int i, j, k;
 362     for( i = 0; i < frames; i++ )
 363     {
 364         int start = i*etalon_size.width*etalon_size.height;
 365
 366         for( j = 0; j < etalon_size.height; j++ )
 367         {
 368             for( k = 0; k < etalon_size.width/2; k++ )
 369             {
 370                 CvPoint2D32f temp;
 371                 CV_SWAP( points[start + j*etalon_size.width + k],
 372                          points[start + (j+1)*etalon_size.width - k - 1], temp );
 373             }
 374
 375             for( k = 0; k < etalon_size.width; k++ )
 376             {
 377                 points[start + j*etalon_size.width + k].y = imgSize.height -
 378                     points[start + j*etalon_size.width + k].y;
 379             }
 380         }
 381     }
 382 }
 383 #else
 384 void MirrorPoints( CvPoint2D32f*, int, CvSize )
 385 {
 386 }
 387 #endif
 388
 389 /* ProcessFrame */
 390 void  CCalibFilter::ProcessFrame( IplImage* rgb_img, double frame_time )
 391 {
 392     bool find_corners = m_initial_params.show_3d_window ||
 393                         m_params.calib_state == CalibState_CalibrationProcess;
 394     bool chess_found = false;
 395     CvSize etalon_size = GetEtalonSize();
 396     int   etalon_points = etalon_size.width * etalon_size.height;
 397     CvSize size;
 398     cvGetImageRawData(rgb_img, 0, 0, &size);
 399         CheckReallocBuffers( rgb_img );
 400
 401     CvPoint2D32f* pt_ptr = m_imagePoints + m_params.frames_collected * etalon_points;
 402
 403     if( find_corners )
 404     {
 405         /* Begin of find etalon points */
 406         int count = etalon_points;// + 10;
 407
 408         cvCvtColor( rgb_img, m_gray_img, CV_BGR2GRAY );
 409
 410         /*********************************************************/
 411         //////////////   FIND CHECKERBOARD CORNERS   //////////////
 412         ///////////////////////////////////////////////////////////
 413
 414         chess_found = cvFindChessBoardCornerGuesses( m_gray_img, m_thresh_img, 0,
 415                                                      etalon_size, pt_ptr, &count ) != 0;
 416         if( count != 0 )
 417         {
 418             cvFindCornerSubPix(
 419                 m_gray_img, pt_ptr, count, cvSize(5,5), cvSize(-1,-1),
 420                 cvTermCriteria( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f ));
 421         }
 422
 423         DrawEtalon( rgb_img, pt_ptr, count, etalon_size, chess_found );
 424     }
 425
 426     if( m_params.calib_state == CalibState_Calibrated )
 427     {
 428         /* Calibration finished */
 429         if( m_initial_params.show_3d_window && chess_found )
 430         {/* We must show 3D etalon and compute extrinsic parameters */
 431             float  rotVect[3];
 432             //float  Jacobian[27];
 433
 434             /* Collect object points */
 435             FillEtalonObjPoints( m_objectPoints, etalon_size,
 436                                  m_params.etalon_params[2] );
 437
 438             MirrorPoints( pt_ptr, 1, etalon_size, size );
 439
 440             cvFindExtrinsicCameraParams( etalon_points,
 441                                          size,
 442                                          pt_ptr,
 443                                          m_objectPoints,
 444                                          m_camera.focalLength,
 445                                          (CvPoint2D32f&)m_camera.principalPoint,
 446                                          m_camera.distortion,
 447                                          rotVect,
 448                                          m_camera.transVect );
 449
 450             {
 451                 CvMat rmat = cvMat( 3, 3, CV_32FC1, m_camera.rotMatr );
 452                 CvMat rvec = cvMat( 3, 1, CV_32FC1, rotVect );
 453                 //CvMat jacob = cvMat( 3, 9, CV_32FC1, Jacobian );
 454
 455                 /* Calc rotation matrix by via Rodrigues Transform */
 456                 cvRodrigues( &rmat, &rvec, 0, CV_RODRIGUES_V2M );
 457             }
 458             Update3DWindow();
 459         }
 460
 461         if( m_initial_params.enable_undistortion )
 462         {
 463             /* Apply undistortion */
 464             if( memcmp( m_camera.matrix, m_undistort_params.matrix, sizeof(m_camera.matrix)) != 0 ||
 465                 memcmp( m_camera.distortion, m_undistort_params.distortion, sizeof(m_camera.distortion)) != 0 )
 466             {
 467                 memcpy( &m_undistort_params, &m_camera, sizeof(m_camera));
 468
 469                 if( !m_undistort_data || m_undistort_data->width != rgb_img->width ||
 470                     m_undistort_data->height != rgb_img->height )
 471                 {
 472                     cvReleaseImage( &m_undistort_data );
 473                 }
 474                 m_undistort_data = cvCreateImage( cvSize( rgb_img->width, rgb_img->height ),
 475                                                   IPL_DEPTH_32S, 3 );
 476             }
 477
 478             {
 479             CvMat a = cvMat( 3, 3, CV_32F, m_undistort_params.matrix );
 480             CvMat dist = cvMat( 1, 4, CV_32F, m_undistort_params.distortion );
 481             cvUndistort2( rgb_img, m_undist_img, &a, &dist );
 482             }
 483             cvCopyImage(m_undist_img, rgb_img);
 484         }
 485     } /* Check if Calibration not finished and the etalon is recognized */
 486
 487     if( m_params.calib_state == CalibState_CalibrationProcess && chess_found &&
 488         frame_time >= m_params.last_frame_time + m_params.frame_interval )
 489     {
 490         m_params.last_frame_time = frame_time;
 491         m_params.frames_collected++;
 492
 493         cvXorS( rgb_img, cvScalarAll( 255 ), rgb_img );
 494
 495         if( m_params.frames_collected == m_params.frames_to_collect )
 496         {
 497             /* all frames are collected. Now will calibrate */
 498             CalculateCameraParams( size );
 499             m_params.calib_state = CalibState_Calibrated;
 500
 501             SetDirty(TRUE);
 502
 503         }/* End calibration */
 504     } /* Else point accumulation */
 505 }
 506
 507
 508
 509 void  CCalibFilter::CalculateCameraParams( CvSize size )
 510 {
 511     int frame;
 512     CvSize etalon_size = GetEtalonSize();
 513     int   etalon_points = etalon_size.width * etalon_size.height;
 514
 515     FillEtalonObjPoints( m_objectPoints, etalon_size,
 516                          m_params.etalon_params[2] );
 517
 518     for( frame = 1; frame < m_params.frames_collected; frame++ )
 519     {
 520         memcpy( m_objectPoints + etalon_points*frame, m_objectPoints,
 521                 etalon_points * sizeof(m_objectPoints[0]));
 522     }
 523
 524     /* Set etalon points counters */
 525     for( frame = 0; frame < m_params.frames_collected; frame++ )
 526     {
 527         m_numsPoints[frame] = etalon_points;
 528     }
 529
 530     MirrorPoints( m_imagePoints, m_params.frames_collected, etalon_size, size );
 531
 532     /* Calirate camera */
 533     cvCalibrateCamera( m_params.frames_collected, m_numsPoints,
 534                        size, m_imagePoints, m_objectPoints,
 535                        m_camera.distortion, m_camera.matrix,
 536                        (float*)m_transVects, m_rotMatrs, 0 );
 537
 538     /* Copy some camera parameters */
 539     m_camera.focalLength[0] = m_camera.matrix[0];
 540     m_camera.focalLength[1] = m_camera.matrix[4];
 541
 542     m_camera.principalPoint[0] = m_camera.matrix[2];
 543     m_camera.principalPoint[1] = m_camera.matrix[5];
 544 }
 545
 546
 547 double CCalibFilter::GetFrameTime( IMediaSample* pSample )
 548 {
 549     double result = GetTickCount();
 550     return result;
 551 }
 552
 553
 554 /* Transform */
 555 HRESULT CCalibFilter::Transform(IMediaSample *pSample)
 556 {
 557     CalibState  state = m_params.calib_state;
 558
 559     switch( state )
 560     {
 561     case  CalibState_Initial:
 562         {
 563
 564             CAutoLock cAutoLock(&m_CCalibFilterLock);
 565             m_params = m_initial_params;
 566             m_params.calib_state = CalibState_NotCalibrated;
 567         }
 568     case  CalibState_NotCalibrated:
 569     case  CalibState_Calibrated:
 570     case  CalibState_CalibrationProcess:
 571         {
 572             AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType();
 573             VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat;
 574
 575             uchar*  rgb_data;
 576             CvSize size = cvSize( pvi->bmiHeader.biWidth, abs(pvi->bmiHeader.biHeight) );
 577             int  step = (size.width*3 + 3) & -4;
 578             pSample->GetPointer(&rgb_data);
 579             assert( pvi->bmiHeader.biBitCount == 24 );
 580
 581             cvInitImageHeader(  m_rgb_img, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 582             cvSetImageData( m_rgb_img, rgb_data, step );
 583
 584             ProcessFrame( m_rgb_img, GetFrameTime( pSample ));
 585             ++m_params.frames_passed;
 586         }
 587         break;
 588     default:
 589         assert(0);
 590     }
 591
 592     return NOERROR;
 593 }
 594
 595
 596 /* CheckInputType */
 597 HRESULT CCalibFilter::CheckInputType(const CMediaType *mtIn)
 598 {
 599     // Check this is a VIDEOINFO type
 600     if( *mtIn->FormatType() != FORMAT_VideoInfo )
 601     {
 602         return E_INVALIDARG;
 603     }
 604
 605     if( !IsEqualGUID(*mtIn->Subtype(), MEDIASUBTYPE_RGB24 ))
 606     {
 607         return E_FAIL;
 608     }
 609     return NOERROR;
 610
 611 }
 612
 613
 614 /* CheckTransform */
 615 HRESULT CCalibFilter::CheckTransform(const CMediaType *mtIn,const CMediaType *mtOut)
 616 {
 617     HRESULT hr;
 618     if (FAILED(hr = CheckInputType(mtIn))) return hr;
 619
 620     // format must be a VIDEOINFOHEADER
 621     if (*mtOut->FormatType() != FORMAT_VideoInfo) return E_INVALIDARG;
 622
 623     // formats must be big enough
 624     if (mtIn->FormatLength() < sizeof(VIDEOINFOHEADER) ||
 625     mtOut->FormatLength() < sizeof(VIDEOINFOHEADER))
 626     return E_INVALIDARG;
 627
 628     VIDEOINFO *pInput = (VIDEOINFO *) mtIn->Format();
 629     VIDEOINFO *pOutput = (VIDEOINFO *) mtOut->Format();
 630
 631     if( pInput->bmiHeader.biBitCount != 24 )
 632     {
 633         return E_FAIL;
 634     }
 635
 636     if (memcmp(&pInput->bmiHeader,&pOutput->bmiHeader,sizeof(BITMAPINFOHEADER)) == 0)
 637         return NOERROR;
 638
 639     return E_INVALIDARG;
 640 }
 641
 642
 643 /* DecideBufferSize */
 644 HRESULT CCalibFilter::DecideBufferSize(IMemAllocator *pAlloc,ALLOCATOR_PROPERTIES *pProperties)
 645 {
 646     if( !m_pInput->IsConnected()) return E_UNEXPECTED;
 647
 648     ASSERT(pAlloc);
 649     ASSERT(pProperties);
 650     return NOERROR;
 651 }
 652
 653
 654 /* GetMediaType */
 655 HRESULT CCalibFilter::GetMediaType(int iPosition, CMediaType *pMediaType)
 656 {
 657     if( !m_pInput->IsConnected()) return E_UNEXPECTED;
 658     if( iPosition < 0 ) return E_INVALIDARG;
 659
 660     /* Do we have more items to offer */
 661     if( iPosition > 0 ) return VFW_S_NO_MORE_ITEMS;
 662
 663     *pMediaType = m_pInput->CurrentMediaType();
 664     return NOERROR;
 665
 666 }
 667
 668 /****************************************************************************************\
 669 *                               Interface methods                                        *
 670 \****************************************************************************************/
 671
 672 STDMETHODIMP CCalibFilter::get_EtalonParams(
 673     CalibEtalonType*   etalon_type,
 674     float*  etalon_params,
 675     long*   etalon_params_count)
 676 {
 677     int params_to_copy = m_initial_params.etalon_params_count;
 678     *etalon_type = m_initial_params.etalon_type;
 679
 680     if( etalon_params )
 681     {
 682         if( params_to_copy > *etalon_params_count )
 683             params_to_copy = *etalon_params_count;
 684
 685         memcpy( etalon_params, m_initial_params.etalon_params,
 686                 params_to_copy*sizeof(float) );
 687         *etalon_params_count = params_to_copy;
 688     }
 689
 690     return NOERROR;
 691 }
 692
 693
 694 STDMETHODIMP CCalibFilter::set_EtalonParams(
 695     CalibEtalonType   etalon_type,
 696     float*  etalon_params,
 697     long    etalon_params_count)
 698 {
 699     CAutoLock cAutoLock(&m_CCalibFilterLock);
 700
 701     if( etalon_type != CalibEtalon_ChessBoard ||
 702         etalon_params_count != 3 ) return E_INVALIDARG;
 703
 704     m_initial_params.etalon_type = etalon_type;
 705     m_initial_params.etalon_params_count = etalon_params_count;
 706     memcpy( m_initial_params.etalon_params, etalon_params,
 707             etalon_params_count*sizeof(float) );
 708
 709     SetDirty(TRUE);
 710
 711     return NOERROR;
 712 }
 713
 714 STDMETHODIMP CCalibFilter::get_FrameInterval( long*  count )
 715 {
 716     *count = m_params.frame_interval;
 717     return NOERROR;
 718 }
 719
 720 STDMETHODIMP CCalibFilter::set_FrameInterval( long  count )
 721 {
 722     CAutoLock cAutoLock(&m_CCalibFilterLock);
 723     if( count < 1 ) count = 1;
 724     m_initial_params.frame_interval = m_params.frame_interval = count;
 725
 726     SetDirty(TRUE);
 727
 728     return NOERROR;
 729 }
 730
 731
 732 STDMETHODIMP CCalibFilter::get_FramesToCollect(
 733     long*   frames)
 734 {
 735     *frames = m_params.frames_to_collect;
 736     return  NOERROR;
 737 }
 738
 739 /************************************/
 740
 741 STDMETHODIMP CCalibFilter::get_EnableUndistortion(
 742     long*   enable)
 743 {
 744     *enable = m_params.enable_undistortion;
 745     return  NOERROR;
 746 }
 747
 748 STDMETHODIMP CCalibFilter::set_EnableUndistortion(
 749     long    enable)
 750 {
 751     CAutoLock cAutoLock(&m_CCalibFilterLock);
 752
 753     m_initial_params.enable_undistortion = m_params.enable_undistortion = enable;
 754
 755     SetDirty(TRUE);
 756
 757     return  NOERROR;
 758 }
 759
 760
 761 STDMETHODIMP CCalibFilter::get_Show3DWindow(
 762     long*   enable)
 763 {
 764     *enable = m_params.show_3d_window;
 765     return  NOERROR;
 766 }
 767
 768 STDMETHODIMP CCalibFilter::set_Show3DWindow(
 769     long    enable)
 770 {
 771     CAutoLock cAutoLock(&m_CCalibFilterLock);
 772
 773     m_initial_params.show_3d_window = m_params.show_3d_window = enable;
 774
 775     m_window3D->Show( enable != 0 );
 776
 777     SetDirty(TRUE);
 778
 779     return  NOERROR;
 780 }
 781
 782 STDMETHODIMP CCalibFilter::SaveCameraParams()
 783 {
 784     if( m_params.calib_state == CalibState_Calibrated)
 785     {
 786         OPENFILENAME lpofn;
 787         char fileName[MAX_PATH] = "";
 788
 789         lpofn.lStructSize       = sizeof(OPENFILENAME);
 790         lpofn.hwndOwner         = 0;
 791         lpofn.hInstance         = 0;
 792         lpofn.lpstrFilter       = "Camera parameters (*.txt)\0*.txt\0";
 793         lpofn.lpstrCustomFilter = 0;
 794         lpofn.nMaxCustFilter    = 0;
 795         lpofn.nFilterIndex      = 1;
 796         lpofn.lpstrFile         = fileName;
 797         lpofn.nMaxFile          = MAX_PATH;
 798         lpofn.lpstrFileTitle    = 0;
 799         lpofn.nMaxFileTitle     = 0;
 800         lpofn.lpstrInitialDir   = 0;
 801         lpofn.lpstrTitle        = 0;
 802         lpofn.Flags             = OFN_OVERWRITEPROMPT;
 803         lpofn.nFileOffset       = 0;
 804         lpofn.nFileExtension    = 0;
 805         lpofn.lpstrDefExt       = "txt";
 806         lpofn.lCustData         = 0;
 807         lpofn.lpfnHook          = 0;
 808         lpofn.lpTemplateName    = 0;
 809
 810         GetSaveFileName(&lpofn);
 811         if( strlen(fileName) != 0)
 812         {
 813             FILE *file = fopen(fileName,"wt");
 814             if( file != 0)
 815             {
 816                 int i, j;
 817                 fprintf(file,"Camera Matrix:\n");
 818                 for( i = 0; i < 3; i++ )
 819                 {
 820                     for( j = 0; j < 3; j++ )
 821                     {
 822                         fprintf( file,"M[%d.%d]=%20.7f", i, j, m_camera.matrix[i*3+j]);
 823                     }
 824                     fprintf(file,"\n");
 825                 }
 826
 827                 fprintf(file,"\n\nDistortion:\n");
 828                 for( i = 0; i < 4; i++ )
 829                     fprintf(file,"D[%d]=%f\n", i, m_camera.distortion[i]);
 830
 831                 fclose(file);
 832             }
 833             else
 834             {
 835                 MessageBox(0,"Can't open file","Save camera parameters",MB_OK|MB_ICONERROR);
 836             }
 837         }
 838     }
 839     else
 840     {
 841         MessageBox(0,"Camera was not calibrated","Save camera parameters",MB_OK);
 842     }
 843
 844     return  NOERROR;
 845 }
 846
 847 STDMETHODIMP CCalibFilter::LoadCameraParams()
 848 {
 849     OPENFILENAME lpofn;
 850 #define BUF_SIZE 10000
 851     char buffer[BUF_SIZE + 100];
 852     char fileName[MAX_PATH] = "";
 853
 854     lpofn.lStructSize       = sizeof(OPENFILENAME);
 855     lpofn.hwndOwner         = 0;
 856     lpofn.hInstance         = 0;
 857     lpofn.lpstrFilter       = "Camera parameters (*.txt)\0*.txt\0";
 858     lpofn.lpstrCustomFilter = 0;
 859     lpofn.nMaxCustFilter    = 0;
 860     lpofn.nFilterIndex      = 1;
 861     lpofn.lpstrFile         = fileName;
 862     lpofn.nMaxFile          = MAX_PATH;
 863     lpofn.lpstrFileTitle    = 0;
 864     lpofn.nMaxFileTitle     = 0;
 865     lpofn.lpstrInitialDir   = 0;
 866     lpofn.lpstrTitle        = 0;
 867     lpofn.Flags             = OFN_FILEMUSTEXIST;
 868     lpofn.nFileOffset       = 0;
 869     lpofn.nFileExtension    = 0;
 870     lpofn.lpstrDefExt       = "txt";
 871     lpofn.lCustData         = 0;
 872     lpofn.lpfnHook          = 0;
 873     lpofn.lpTemplateName    = 0;
 874
 875     GetOpenFileName(&lpofn);
 876
 877     if( strlen(fileName) != 0)
 878     {
 879         FILE *file = fopen(fileName,"rb");
 880
 881         if( file != 0)
 882         {
 883             int i, j, k;
 884             float cameraMatrix[9];
 885             float distortion[4];
 886
 887             int sz = fread( buffer, 1, BUF_SIZE, file );
 888             char* ptr = buffer;
 889             buffer[sz] = '\0';
 890
 891             /* read matrix */
 892             for( k = 0; k < 9; k++ )
 893             {
 894                 ptr = strstr( ptr, "M[" );
 895                 if( ptr )
 896                 {
 897                     int s = 0;
 898                     ptr += 2;
 899                     if( sscanf( ptr, "%d%*[.,]%d%n", &i, &j, &s ) == 2 && i == k/3 && j == k%3 )
 900                     {
 901                         ptr += s;
 902                         ptr = strstr( ptr, "=" );
 903                         if( ptr )
 904                         {
 905                             s = 0;
 906                             ptr++;
 907                             if( sscanf( ptr, "%f%n", cameraMatrix + k, &s ) == 1 )
 908                             {
 909                                 ptr += s;
 910                                 continue;
 911                             }
 912                         }
 913                     }
 914                 }
 915
 916                 /* else report a bug */
 917                 MessageBox(0,"Invalid file format","Load camera parameters",MB_OK|MB_ICONERROR);
 918                 return E_FAIL;
 919             }
 920
 921             /* read distortion */
 922             for( k = 0; k < 4; k++ )
 923             {
 924                 ptr = strstr( ptr, "D[" );
 925                 if( ptr )
 926                 {
 927                     int s = 0;
 928                     ptr += 2;
 929                     if( sscanf( ptr, "%d%n", &i, &s ) == 1 && i == k )
 930                     {
 931                         ptr += s;
 932                         ptr = strstr( ptr, "=" );
 933                         if( ptr )
 934                         {
 935                             s = 0;
 936                             ptr++;
 937                             if( sscanf( ptr, "%f%n", distortion + k, &s ) == 1 )
 938                             {
 939                                 ptr += s;
 940                                 continue;
 941                             }
 942                         }
 943                     }
 944                 }
 945
 946                 /* else report a bug */
 947                 MessageBox(0,"Invalid file format","Load camera parameters",MB_OK|MB_ICONERROR);
 948                 return E_FAIL;
 949             }
 950
 951             memcpy( m_camera.matrix, cameraMatrix, sizeof( cameraMatrix ));
 952             memcpy( m_camera.distortion, distortion, sizeof( distortion ));
 953
 954             m_camera.focalLength[0] = m_camera.matrix[0];
 955             m_camera.focalLength[1] = m_camera.matrix[4];
 956
 957             m_camera.principalPoint[0] = m_camera.matrix[2];
 958             m_camera.principalPoint[1] = m_camera.matrix[5];
 959
 960             m_params.calib_state = CalibState_Calibrated;
 961
 962             fclose(file);
 963         }
 964         else
 965         {
 966             MessageBox(0,"Can't open file","Load camera parameters",MB_OK|MB_ICONERROR);
 967         }
 968     }
 969
 970     SetDirty(TRUE);
 971
 972     return  NOERROR;
 973 }
 974
 975 //////////////////////////////////////
 976 //////////////////////////////////////
 977 //////////////////////////////////////
 978
 979 STDMETHODIMP CCalibFilter::set_FramesToCollect(
 980     long    frames)
 981 {
 982     CAutoLock cAutoLock(&m_CCalibFilterLock);
 983
 984     m_initial_params.frames_to_collect = m_params.frames_to_collect = frames;
 985     SetDirty(TRUE);
 986     return  NOERROR;
 987 }
 988
 989
 990 STDMETHODIMP CCalibFilter::StartCalibrate()
 991 {
 992     CAutoLock cAutoLock(&m_CCalibFilterLock);
 993
 994     m_params = m_initial_params;
 995     m_params.calib_state = CalibState_CalibrationProcess;
 996     m_params.frames_collected = 0;
 997
 998     return  NOERROR;
 999 }
1000
1001 STDMETHODIMP CCalibFilter::GetCameraParams( CvCameraParams* camera )
1002 {
1003     if( m_params.calib_state != CalibState_Calibrated ) return E_PENDING;
1004
1005     *camera = m_camera;
1006     return  NOERROR;
1007 }
1008
1009
1010 STDMETHODIMP CCalibFilter::GetState(
1011    CalibState*  calib_state,
1012    long*   frames_collected,
1013    long*   frames_passed,
1014    double* last_frame_time)
1015 {
1016     *calib_state = m_params.calib_state;
1017     *frames_collected = m_params.frames_collected;
1018     *frames_passed = m_params.frames_passed;
1019     *last_frame_time = m_params.last_frame_time;
1020
1021     return NOERROR;
1022 }
1023
1024
1025 STDMETHODIMP CCalibFilter::GetPages(CAUUID *pPages)
1026 {
1027     pPages->cElems = 1;
1028     pPages->pElems = (GUID *) CoTaskMemAlloc(sizeof(GUID));
1029     if( !pPages->pElems ) return E_OUTOFMEMORY;
1030     *(pPages->pElems) = CLSID_CCalibFilterPropertyPage;
1031     return NOERROR;
1032 }
1033
1034 STDMETHODIMP CCalibFilter::Update3DWindow()
1035 {
1036     CvSize etalon_size = cvSize( (int)m_params.etalon_params[0],
1037                                       (int)m_params.etalon_params[1] );
1038     float square_size = m_params.etalon_params[2];
1039
1040     m_window3D->SetParams( &m_camera, etalon_size, square_size );
1041     return  NOERROR;
1042 }
1043
1044
1045 // IPersistStream implementation
1046 STDMETHODIMP CCalibFilter::GetClassID( CLSID *pClsID )
1047 {
1048     return CBaseFilter::GetClassID( pClsID );
1049 }
1050
1051
1052 DWORD CCalibFilter::GetSoftwareVersion()
1053 {
1054     return 0x11;
1055 }
1056
1057 int CCalibFilter::SizeMax()
1058 {
1059     return 1024;
1060 }
1061
1062
1063 #define WRITEOUT(var)  hr = pStream->Write(&var, sizeof(var), NULL); \
1064                if (FAILED(hr)) return hr;
1065
1066 #define READIN(var)    hr = pStream->Read(&var, sizeof(var), NULL); \
1067                if (FAILED(hr)) return hr;
1068
1069 HRESULT CCalibFilter::ReadFromStream( IStream* pStream )
1070 {
1071     HRESULT hr;
1072     READIN( m_initial_params );
1073
1074     m_params = m_initial_params;
1075
1076     m_params.calib_state = m_params.calib_state == CalibState_Calibrated ?
1077                            CalibState_Calibrated : CalibState_NotCalibrated;
1078
1079     if( m_params.calib_state == CalibState_Calibrated )
1080     {
1081         READIN( m_camera );
1082     }
1083     return NOERROR;
1084 }
1085
1086 HRESULT CCalibFilter::WriteToStream( IStream* pStream )
1087 {
1088     HRESULT hr;
1089     m_initial_params.calib_state = m_params.calib_state;
1090     WRITEOUT( m_initial_params );
1091
1092     if( m_params.calib_state == CalibState_Calibrated )
1093     {
1094         WRITEOUT( m_camera );
1095     }
1096
1097     /*SetDirty(FALSE);*/
1098
1099     return NOERROR;
1100 }
1101
1102
1103 STDAPI DllRegisterServer()
1104 {
1105     return AMovieDllRegisterServer2( TRUE );
1106 }
1107
1108 STDAPI DllUnregisterServer()
1109 {
1110    return AMovieDllRegisterServer2( FALSE );
1111 }
1112
1113 /* End of file. */
1114