图像畸变校正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 );    

}