学步园

基于opencv的标定程序，结果保存到文本文件。

VC6+OpenCV beta 3.1，对话框架构。

当时记录的开发日志：

感谢上帝，半个下午加一个晚上，完成本标定程序。
新建对话框框架，从CalibFilter改作并完善过来。
核心标定函数在ProcessFrame()中。
22:14 2005-4-7

贴两个函数代码：

void  CMyCalibrationDlg::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;

        cvCvtColor( rgb_img, m_gray_img, CV_BGR2GRAY );

        /*********************************************************/    
        //////////////   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.rotMatr里
                                         m_camera.transVect );
    
            {
                CvMat rmat = cvMat( 3, 3, CV_32FC1, m_camera.rotMatr );
                CvMat rvec = cvMat( 3, 1, CV_32FC1, rotVect );
                //CvMat jacob = cvMat( 3, 9, CV_32FC1, Jacobian );

                /* Calc rotation matrix by via Rodrigues Transform */
                cvRodrigues( &rmat, &rvec, 0, CV_RODRIGUES_V2M );
            }
            Update3DWindow();
        }

        if( m_initial_params.enable_undistortion )
        {
            /* Apply undistortion */
            if( memcmp( m_camera.matrix, m_undistort_params.matrix, sizeof(m_camera.matrix)) != 0 ||
                memcmp( m_camera.distortion, m_undistort_params.distortion, sizeof(m_camera.distortion)) != 0 )
            {
                memcpy( &m_undistort_params, &m_camera, sizeof(m_camera));
                
                if( !m_undistort_data || m_undistort_data->width != rgb_img->width ||
                    m_undistort_data->height != rgb_img->height )
                {
                    cvReleaseImage( &m_undistort_data );
                }
                m_undistort_data = cvCreateImage( cvSize( rgb_img->width, rgb_img->height ),
                                                  IPL_DEPTH_32S, 3 );
                cvUnDistortInit( rgb_img, m_undistort_data, m_undistort_params.matrix,
                                 m_undistort_params.distortion,1 );//,1 for version 5
            }

            cvUnDistort( rgb_img, m_undist_img, m_undistort_data,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++;

        cvXorS( rgb_img, cvScalarAll( 255 ), rgb_img );

        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);//qiansen

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


//全部采集后被调用，只一次，外参数也只是此刻的外参数。
void  CMyCalibrationDlg::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];

	//qiansen
	memcpy( m_camera.transVect, m_transVects, sizeof( m_transVects ));
	memcpy( m_camera.rotMatr, m_rotMatrs, sizeof( m_rotMatrs ));
}