基于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 ));
}