图像畸变校正OPENCV

#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
#include "cvcam.h"

//图像的像素直接提取
#define        _I(img,x,y) ((unsigned char*)((img)->imageData + (img)->widthStep*(y)))[(x)]
//亚像素级灰度值
#define        _IF(image,x,y)    ( ((int)(x+1)-(x))*((int)(y+1)-(y))*_I((image),(int)(x),(int)(y)) + ((int)(x+1)-(x))*((y)-(int)(y))*_I((image),(int)(x),(int)(y+1)) + ((x)-(int)(x))*((int)(y+1)-(y))*_I((image),(int)(x+1),(int)(y)) + ((x)-(int)(x))*((y)-(int)(y))*_I((image),(int)(x+1),(int)(y+1)) )//插值后的像素值(IN表示interpolation),x、y可以为小数

void callback(IplImage* image);

void main()
{

int ncams = cvcamGetCamerasCount( );//返回可以访问的摄像头数目
HWND mywin;
cvcamSetProperty(0, CVCAM_PROP_ENABLE, CVCAMTRUE);
cvcamSetProperty(0, CVCAM_PROP_RENDER, CVCAMTRUE);
mywin = (HWND)cvGetWindowHandle("cvcam window");
cvcamSetProperty(0, CVCAM_PROP_WINDOW, &mywin);
cvcamSetProperty(0, CVCAM_PROP_CALLBACK, callback);

//cvcamGetProperty(0, CVCAM_VIDEOFORMAT,NULL);
cvNamedWindow( "径向矫正1", 1 );//创建窗口
cvNamedWindow( "径向矫正2", 1 );//创建窗口

cvcamInit( );
cvcamStart( );

cvWaitKey(0);

cvcamStop( );
cvcamExit( );
cvDestroyWindow( "径向矫正1" );//销毁窗口
cvDestroyWindow( "径向矫正2" );//销毁窗口

}

void callback(IplImage* image)
{

IplImage* Show1 = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1);
IplImage* Show2 = cvCreateImage( cvSize(420,340), IPL_DEPTH_8U, 1);
IplImage* ImageC1 = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1);

//转换为灰度图
cvCvtColor( image, ImageC1, CV_RGB2GRAY);
cvFlip( ImageC1, NULL, 0);

double *mi;
double *md;

mi = new double[3*3];
md = new double[4];

CvMat intrinsic_matrix,distortion_coeffs;

//摄像机内参数
cvInitMatHeader(&intrinsic_matrix,3,3,CV_64FC1,mi);

//镜头畸变参数
cvInitMatHeader(&distortion_coeffs,1,4,CV_64FC1,md);

/////////////////////////////////////////////////
////////////////////////////320*240 120度广角镜头
//参数由matlab获得
double fc1,fc2,cc1,cc2,kc1,kc2,kc3,kc4;
fc1 = 667.23923/2.5;
fc2 = 669.78156/2.5;
cc1 = 429.96933/2.5;
cc2 = 351.48350/2.5;
kc1 = -0.40100;
kc2 = 0.19463;
kc3 = 0.00508;
kc4 = -0.00051;

cvmSet(&intrinsic_matrix, 0, 0, fc1);
cvmSet(&intrinsic_matrix, 0, 1, 0);
cvmSet(&intrinsic_matrix, 0, 2, cc1);
cvmSet(&intrinsic_matrix, 1, 0, 0);
cvmSet(&intrinsic_matrix, 1, 1, fc2);
cvmSet(&intrinsic_matrix, 1, 2, cc2);
cvmSet(&intrinsic_matrix, 2, 0, 0);
cvmSet(&intrinsic_matrix, 2, 1, 0);
cvmSet(&intrinsic_matrix, 2, 2, 1);

cvmSet(&distortion_coeffs, 0, 0, kc1);
cvmSet(&distortion_coeffs, 0, 1, kc2);
cvmSet(&distortion_coeffs, 0, 2, kc3);
cvmSet(&distortion_coeffs, 0, 3, kc4);
////////////////////////////320*240 120度广角镜头
/////////////////////////////////////////////////

//矫正畸变(opencv)
cvUndistort2( ImageC1, Show1, &intrinsic_matrix, &distortion_coeffs);

//矫正畸变
for (int nx=0; nx<420; nx++)
{
for (int ny=0; ny<340; ny++)
{
double x=nx-50;
double y=ny-50;
double xx=(x-cc1)/fc1;
double yy=(y-cc2)/fc2;
double r2=pow(xx,2)+pow(yy,2);
double r4=pow(r2,2);
double xxx=xx*(1+kc1*r2+kc2*r4)+2*kc3*xx*yy+kc4*(r2+2*xx*xx);
double yyy=yy*(1+kc1*r2+kc2*r4)+2*kc4*xx*yy+kc3*(r2+2*yy*yy);
double xxxx = xxx*fc1+cc1;
double yyyy = yyy*fc2+cc2;
if (xxxx>0 && xxxx<320 && yyyy>0 && yyyy<240)
{
_I(Show2,nx,ny) = (int)_IF(ImageC1,xxxx,yyyy);
}
else
{
_I(Show2,nx,ny) = 0;
}

}
}

//画线
cvLine( Show1, cvPoint(0,10), cvPoint(320,10), cvScalar(255,255,255),3 );
cvLine( Show1, cvPoint(0,230), cvPoint(320,230), cvScalar(255,255,255),3 );
cvLine( Show1, cvPoint(10,0), cvPoint(10,240), cvScalar(255,255,255),3 );
cvLine( Show1, cvPoint(310,0), cvPoint(310,240), cvScalar(255,255,255),3 );
cvLine( Show1, cvPoint(0,0), cvPoint(320,240), cvScalar(255,255,255),3 );
cvLine( Show1, cvPoint(0,240), cvPoint(320,0), cvScalar(255,255,255),3 );

cvLine( Show1, cvPoint(0,10), cvPoint(320,10), cvScalar(0,0,0) );
cvLine( Show1, cvPoint(0,230), cvPoint(320,230), cvScalar(0,0,0) );
cvLine( Show1, cvPoint(10,0), cvPoint(10,240), cvScalar(0,0,0) );
cvLine( Show1, cvPoint(310,0), cvPoint(310,240), cvScalar(0,0,0) );
cvLine( Show1, cvPoint(0,0), cvPoint(320,240), cvScalar(0,0,0) );
cvLine( Show1, cvPoint(0,240), cvPoint(320,0), cvScalar(0,0,0) );

//显示
cvShowImage("径向矫正1", Show1);
cvShowImage("径向矫正2", Show2);
cvWaitKey(1);
cvReleaseImage( &Show1 );
cvReleaseImage( &Show2 );
cvReleaseImage( &ImageC1 );

}