calibfilter.cpp

微软的基于HMM的人脸识别原代码, 非常经典的说

        int start = i*etalon_size.width*etalon_size.height;

        for( j = 0; j < etalon_size.height; j++ )
        {
            for( k = 0; k < etalon_size.width/2; k++ )
            {
                CvPoint2D32f temp;
                CV_SWAP( points[start + j*etalon_size.width + k],
                         points[start + (j+1)*etalon_size.width - k - 1], temp );
            }

            for( k = 0; k < etalon_size.width; k++ )
            {
                points[start + j*etalon_size.width + k].y = imgSize.height - 
                    points[start + j*etalon_size.width + k].y;
            }
        }
    }
}
#else
void MirrorPoints( CvPoint2D32f*, int, CvSize )
{
}
#endif

/* ProcessFrame */
void  CCalibFilter::ProcessFrame( IplImage* rgb_img, double frame_time )
{
    bool find_corners = m_initial_params.show_3d_window ||
                        m_params.calib_state == CalibState_CalibrationProcess;
    bool chess_found = false; 
    CvSize etalon_size = GetEtalonSize();
    int   etalon_points = etalon_size.width * etalon_size.height;
    CvSize size;
    cvGetImageRawData(rgb_img, 0, 0, &size);
        CheckReallocBuffers( rgb_img );

    CvPoint2D32f* pt_ptr = m_imagePoints + m_params.frames_collected * etalon_points;

    if( find_corners )
    {  
        /* Begin of find etalon points */
        int count = etalon_points;// + 10;

        iplColorToGray( rgb_img, m_gray_img );

        /*********************************************************/    
        //////////////   FIND CHECKERBOARD CORNERS   //////////////    
        ///////////////////////////////////////////////////////////    

        chess_found = cvFindChessBoardCornerGuesses( m_gray_img, m_thresh_img, 0,
                                                     etalon_size, pt_ptr, &count ) != 0;
        if( count != 0 )
        {
            cvFindCornerSubPix( 
                m_gray_img, pt_ptr, count, cvSize(5,5), cvSize(-1,-1),
                cvTermCriteria( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f ));
        }
  
        DrawEtalon( rgb_img, pt_ptr, count, etalon_size, chess_found );
    }

    if( m_params.calib_state == CalibState_Calibrated )
    {
        /* Calibration finished */
        if( m_initial_params.show_3d_window && chess_found )
        {/* We must show 3D etalon and compute extrinsic parameters */
            float  rotVect[3];
            float  Jacobian[27];

            /* Collect object points */
            FillEtalonObjPoints( m_objectPoints, etalon_size,
                                 m_params.etalon_params[2] );
    
            MirrorPoints( pt_ptr, 1, etalon_size, size );

            cvFindExtrinsicCameraParams( etalon_points,
                                         size,
                                         pt_ptr,
                                         m_objectPoints,
                                         m_camera.focalLength,
                                         (CvPoint2D32f&)m_camera.principalPoint,
                                         m_camera.distortion,
                                         rotVect,
                                         m_camera.transVect );
    
            /* Calc rotation matrix by via Rodrigues Transform */
            cvRodrigues( m_camera.rotMatr, rotVect, Jacobian, CV_RODRIGUES_V2M );
            Update3DWindow();
        }

        if( m_initial_params.enable_undistortion )
        {
            /* Apply undistortion */
            cvUnDistortOnce( rgb_img, m_undist_img,
                             m_camera.matrix, m_camera.distortion, 1 );

            cvCopyImage(m_undist_img, rgb_img);
        }
    } /* Check if Calibration not finished and the etalon is recognized */

    if( m_params.calib_state == CalibState_CalibrationProcess && chess_found &&
        frame_time >= m_params.last_frame_time + m_params.frame_interval )
    {
        m_params.last_frame_time = frame_time;
        m_params.frames_collected++;

        iplXorS( rgb_img, rgb_img, 255 );

        if( m_params.frames_collected == m_params.frames_to_collect )
        {
            /* all frames are collected. Now will calibrate */
            CalculateCameraParams( size );
            m_params.calib_state = CalibState_Calibrated;

            SetDirty(TRUE);

        }/* End calibration */
    } /* Else point accumulation */
}



void  CCalibFilter::CalculateCameraParams( CvSize size )
{
    int frame;
    CvSize etalon_size = GetEtalonSize();
    int   etalon_points = etalon_size.width * etalon_size.height;
    
    FillEtalonObjPoints( m_objectPoints, etalon_size,
                         m_params.etalon_params[2] );
    
    for( frame = 1; frame < m_params.frames_collected; frame++ )
    {
        memcpy( m_objectPoints + etalon_points*frame, m_objectPoints,
                etalon_points * sizeof(m_objectPoints[0]));
    }

    /* Set etalon points counters */
    for( frame = 0; frame < m_params.frames_collected; frame++ )
    {
        m_numsPoints[frame] = etalon_points;
    }

    MirrorPoints( m_imagePoints, m_params.frames_collected, etalon_size, size );

    /* Calirate camera */
    cvCalibrateCamera( m_params.frames_collected, m_numsPoints,
                       size, m_imagePoints, m_objectPoints,
                       m_camera.distortion, m_camera.matrix,
                       (float*)m_transVects, m_rotMatrs, 0 );

    /* Copy some camera parameters */
    m_camera.focalLength[0] = m_camera.matrix[0];
    m_camera.focalLength[1] = m_camera.matrix[4];

    m_camera.principalPoint[0] = m_camera.matrix[2];
    m_camera.principalPoint[1] = m_camera.matrix[5];
}


double CCalibFilter::GetFrameTime( IMediaSample* pSample )
{
    double result = GetTickCount();
    return result;
}


/* Transform */
HRESULT CCalibFilter::Transform(IMediaSample *pSample)
{
    CalibState  state = m_params.calib_state;
                   
    switch( state )
    {
    case  CalibState_Initial:
        {
        
            CAutoLock cAutoLock(&m_CCalibFilterLock);
            m_params = m_initial_params;
            m_params.calib_state = CalibState_NotCalibrated;
        }
    case  CalibState_NotCalibrated:
    case  CalibState_Calibrated:
    case  CalibState_CalibrationProcess:
        {
            AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType();
            VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat;
            
            uchar*  rgb_data;
            CvSize size = cvSize( pvi->bmiHeader.biWidth, pvi->bmiHeader.biHeight );
            int  step = (size.width*3 + 3) & -4;
            pSample->GetPointer(&rgb_data);
            assert( pvi->bmiHeader.biBitCount == 24 );

            cvInitImageHeader(  m_rgb_img, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4, 1 );
            cvSetImageData( m_rgb_img, rgb_data, step ); 

            ProcessFrame( m_rgb_img, GetFrameTime( pSample ));
            ++m_params.frames_passed;
        }
        break;
    default:
        assert(0);
    }

    return NOERROR;
}


/* CheckInputType */
HRESULT CCalibFilter::CheckInputType(const CMediaType *mtIn)
{
    // Check this is a VIDEOINFO type
    if( *mtIn->FormatType() != FORMAT_VideoInfo )
    {
        return E_INVALIDARG;
    }

    if( !IsEqualGUID(*mtIn->Subtype(), MEDIASUBTYPE_RGB24 ))
    {
        return E_FAIL;
    }
    return NOERROR;

}


/* CheckTransform */
HRESULT CCalibFilter::CheckTransform(const CMediaType *mtIn,const CMediaType *mtOut)
{
    HRESULT hr;
    if (FAILED(hr = CheckInputType(mtIn))) return hr;

    // format must be a VIDEOINFOHEADER
    if (*mtOut->FormatType() != FORMAT_VideoInfo) return E_INVALIDARG;
    
    // formats must be big enough 
    if (mtIn->FormatLength() < sizeof(VIDEOINFOHEADER) ||
    mtOut->FormatLength() < sizeof(VIDEOINFOHEADER))
    return E_INVALIDARG;
    
    VIDEOINFO *pInput = (VIDEOINFO *) mtIn->Format();
    VIDEOINFO *pOutput = (VIDEOINFO *) mtOut->Format();
    
    if( pInput->bmiHeader.biBitCount != 24 )
    {
        return E_FAIL;
    }

    if (memcmp(&pInput->bmiHeader,&pOutput->bmiHeader,sizeof(BITMAPINFOHEADER)) == 0)
        return NOERROR;

    return E_INVALIDARG;
}


/* DecideBufferSize */
HRESULT CCalibFilter::DecideBufferSize(IMemAllocator *pAlloc,ALLOCATOR_PROPERTIES *pProperties)
{
    if( !m_pInput->IsConnected()) return E_UNEXPECTED;

    ASSERT(pAlloc);
    ASSERT(pProperties);
    return NOERROR;
}


/* GetMediaType */
HRESULT CCalibFilter::GetMediaType(int iPosition, CMediaType *pMediaType)
{
    if( !m_pInput->IsConnected()) return E_UNEXPECTED;
    if( iPosition < 0 ) return E_INVALIDARG;

    /* Do we have more items to offer */
    if( iPosition > 0 ) return VFW_S_NO_MORE_ITEMS;

    *pMediaType = m_pInput->CurrentMediaType();
    return NOERROR;

}

/****************************************************************************************\
*                               Interface methods                                        *
\****************************************************************************************/

STDMETHODIMP CCalibFilter::get_EtalonParams(
    CalibEtalonType*   etalon_type,
    float*  etalon_params,
    long*   etalon_params_count)
{
    int params_to_copy = m_initial_params.etalon_params_count;
    *etalon_type = m_initial_params.etalon_type;
    
    if( etalon_params )
    {
        if( params_to_copy > *etalon_params_count )
            params_to_copy = *etalon_params_count;

        memcpy( etalon_params, m_initial_params.etalon_params,
                params_to_copy*sizeof(float) );
        *etalon_params_count = params_to_copy;
    }

    return NOERROR;
}


STDMETHODIMP CCalibFilter::set_EtalonParams(
    CalibEtalonType   etalon_type,
    float*  etalon_params,
    long    etalon_params_count)
{
    CAutoLock cAutoLock(&m_CCalibFilterLock);

    if( etalon_type != CalibEtalon_ChessBoard || 
        etalon_params_count != 3 ) return E_INVALIDARG;

    m_initial_params.etalon_type = etalon_type;
    m_initial_params.etalon_params_count = etalon_params_count;
    memcpy( m_initial_params.etalon_params, etalon_params,
            etalon_params_count*sizeof(float) );

    SetDirty(TRUE);

    return NOERROR;
}

STDMETHODIMP CCalibFilter::get_FrameInterval( long*  count )
{
    *count = m_params.frame_interval;
    return NOERROR;
}

STDMETHODIMP CCalibFilter::set_FrameInterval( long  count )
{
    CAutoLock cAutoLock(&m_CCalibFilterLock);
    if( count < 1 ) count = 1;
    m_initial_params.frame_interval = m_params.frame_interval = count;

    SetDirty(TRUE);

    return NOERROR;
}


STDMETHODIMP CCalibFilter::get_FramesToCollect(
    long*   frames)
{
    *frames = m_params.frames_to_collect;
    return  NOERROR;
}

/************************************/

STDMETHODIMP CCalibFilter::get_EnableUndistortion(
    long*   enable)
{
    *enable = m_params.enable_undistortion;
    return  NOERROR;
}

STDMETHODIMP CCalibFilter::set_EnableUndistortion(
    long    enable)
{