opencv 摄像机标定 实时标定

#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
#include "cvcam.h"
#include<windows.h>

//#pragma comment(lib,"cv210.lib")
using namespace cv;
//图像的像素直接提取

#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);

int main()
{
    CvCapture* capture1 = cvCreateCameraCapture(0);

	//IplImage* frame1=cvLoadImage("D:\\棋盘图.png",1);

	//double w=640,h=480;

	//cvShowImage("image",frame1);

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

	//callback(frame1);

	int key = cvWaitKey();
	//if(key == 27)

	//cvSetCaptureProperty(capture1,CV_CAP_PROP_FPS,5);

	IplImage* frame1;
 	while(1)
	{
		frame1=cvQueryFrame(capture1);
		if(!frame1)
			break;
		cvShowImage("camera_1",frame1);

		callback(frame1);

		int key = cvWaitKey(30);
		if(key == 27)
			break;

	}
	cvReleaseCapture(&capture1);

	//cvReleaseImage(&frame1);

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

	return 0;

}

void callback(IplImage* image)
{
// 	Mat m1(image,0);
// 	Mat ss;

	IplImage* Show1 = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	IplImage* Show2 = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	IplImage* ImageC1 = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);

	//转换为灰度图
	cvCvtColor(image, ImageC1, CV_RGB2GRAY);
	//imshow("ss",ss);
	//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<430; 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 );    

}