摄像机镜头标定
/*
基于OpenCV的自标定技术实现
----------各个矩阵的作用--------------
intrinsics; //内参矩阵[fx,0,cx;0,fy,cy;0,0,1]
distortion_coeff; //形变参数向量4*1[k1,k2,p1,p2]
rotation_vectors; //旋转向量 3*1
translation_vectors; //平移变量 3*1
R,T为从世界坐标系到图像坐标系的旋转和平移变换矩阵
*/
#include "stdafx.h"
#include
#include
#include
// OpenCV
#include
#include
#include
#include
//生成角点三维空间的坐标
void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);
void makeChessBoard();
//函数myFindChessboardCorners寻找棋盘图的内角点位置
int myFindChessboardCorners( const void* image, CvSize pattern_size,
CvPoint2D32f* corners, int* corner_count=NULL,
int flags=CV_CALIB_CB_ADAPTIVE_THRESH );
//绘制一个十字标记
inline int drawCorssMark(IplImage *dst,CvPoint pt)
/*************************************************
Function: main_loop
Description: 绘制一个十字标记
Calls:
Called By:
Input: RGB image, pt
Output:
Return:
Others: 需要检查坐标是否越界 to do list
*************************************************/
{
const int cross_len = 4;
CvPoint pt1,pt2,pt3,pt4;
pt1.x = pt.x;
pt1.y = pt.y - cross_len;
pt2.x = pt.x;
pt2.y = pt.y + cross_len;
pt3.x = pt.x - cross_len;
pt3.y = pt.y;
pt4.x = pt.x + cross_len;
pt4.y = pt.y;
cvLine(dst,pt1,pt2,CV_RGB(0,255,0),2,CV_AA, 0 );
cvLine(dst,pt3,pt4,CV_RGB(0,255,0),2,CV_AA, 0 );
return 0;
}
/* declarations for OpenCV */
IplImage *current_frame_rgb,grid; //RGB图
IplImage *current_frame_gray; //灰度图
IplImage *chessBoard_Img; //棋盘图
//int Thresholdness = 120; //阈值， 二值化
int Thresholdness = 1;
int image_width = 320; //定义图像大小
int image_height = 240;
//bool verbose = false;
const int ChessBoardSize_w = 8; //
const int ChessBoardSize_h = 6; //图片中可标定的标角数
// Calibration stuff
bool calibration_done = false;
const CvSize ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h); //以像素为单位 定义棋盘大小
//float SquareWidth = 21.6f; //实际距离 毫米单位 在A4纸上为两厘米
float SquareWidth = 17; //投影实际距离 毫米单位 200;
//棋盘格子的边长，可任意设定，不影响内参数，只影响内参数
const int NPoints = ChessBoardSize_w*ChessBoardSize_h;
const int NImages = 24; // 捕获图片数
CvPoint2D32f corners[NPoints*NImages]; //三维浮点坐标
int corner_count[NImages] = {0};
int captured_frames = 0;
CvMat *intrinsics; //内参矩阵[fx,0,cx;0,fy,cy;0,0,1]
CvMat *distortion_coeff; //形变参数向量4*1[k1,k2,p1,p2]
CvMat *rotation_vectors; //旋转向量 3*1
CvMat *translation_vectors; //平移变量 3*1
CvMat *object_points; //物体点的坐标--定标点的世界坐标
CvMat *point_counts; //向量数目
CvMat *image_points; //输出数组 存储图像点的坐标---定标点的图象坐标
int find_corners_result =0 ;
/*
void on_mouse( int event, int x, int y, int flags, void* param )
{ //
if( event == CV_EVENT_LBUTTONDOWN )
{
//calibration_done = true;
}
}
*/
/* 主函数 */
int main(int argc, char *argv[])
{
CvFont font;
cvInitFont( &font, CV_FONT_VECTOR0,5, 5, 0, 7, 8);
intrinsics = cvCreateMat(3,3,CV_32FC1); //为新矩阵非配头和数据
// distortion_coeff = cvCreateMat(1,4,CV_32FC1);
distortion_coeff = cvCreateMat(4,1,CV_32FC1);
rotation_vectors = cvCreateMat(NImages,3,CV_32FC1);
translation_vectors = cvCreateMat(NImages,3,CV_32FC1);
point_counts = cvCreateMat(NImages,1,CV_32SC1);
object_points = cvCreateMat(NImages*NPoints,3,CV_32FC1);
image_points = cvCreateMat(NImages*NPoints,2,CV_32FC1);
// Function to fill in the real-world points of the checkerboard
//生成角点三维空间的坐标
InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth);
CvCapture* capture = 0;
//摄像头
if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
//视频流
else if( argc == 2 )
capture = cvCaptureFromAVI( argv[1] );
//无摄像头
if( !capture )
{
fprintf(stderr,"Could not initialize capturing...\n");
return -1;
}
// Initialize all of the IplImage structures
current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
IplImage *current_frame_rgb2 = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
chessBoard_Img = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
current_frame_rgb2->origin = chessBoard_Img->origin = current_frame_gray->origin = current_frame_rgb->origin = 1;
makeChessBoard();
cvNamedWindow( "result", 0);
cvNamedWindow( "Window 0", 0);
cvNamedWindow( "grid", 0);
cvNamedWindow("src",0);
cvMoveWindow("src",300,450);
cvMoveWindow( "grid", 20,20);
/* 设定阈值 二值化
cvSetMouseCallback( "Window 0", on_mous
e, 0 ); /
cvCreateTrackbar("Thresholdness","Window 0",&Thresholdness, 255,0);
*/
cvCreateTrackbar("Thresholdness","Window 0",&Thresholdness, 50,0);
while (!calibration_done)
{
while (captured_frames < NImages)
{
current_frame_rgb = cvQueryFrame( capture );
// cvShowImage("Window 0",current_frame_rgb); //+++++
//current_frame_rgb = cvLoadImage( "c:\\BoardStereoL3.jpg" );
//cvCopy(chessBoard_Img,current_frame_rgb);
/* char filename[]="00.jpg"; //说明：我把待标定的图片的名子依次命名为：01.jpg, 02.jpg, 03.jpg, 04.jpg,……
filename[1]=(char)(captured_frames+49);
*/
if( !current_frame_rgb )
break;
/*
////////////////////////////////////////////////////////////
int captured_frames=0;
for(captured_frames=0;captured_frames
char filename[]="00.jpg"; //说明：我把待标定的图片的名子依次命名为：01.jpg, 02.jpg, 03.jpg, 04.jpg,……
if(captured_frames<9)
filename[1]=(char)(captured_frames+49);
else printf("error, too many images.......\n"); //load images end
current_frame_rgb=cvLoadImage( filename, CV_LOAD_IMAGE_COLOR );
////////////////////////////////////////////////////////////
*/
// current_frame_rgb=cvLoadImage( filename, CV_LOAD_IMAGE_COLOR );
cvCopy(current_frame_rgb,current_frame_rgb2); //current_frame_rgb-->current_frame_rgb2
cvCvtColor(current_frame_rgb, current_frame_gray, CV_BGR2GRAY);//current_frame_rgb-->current_frame_gray
/* 二值化
//cvThreshold(current_frame_gray,current_frame_gray,Thresholdness,255,CV_THRESH_BINARY);
//cvThreshold(current_frame_gray,current_frame_gray,150,255,CV_THRESH_BINARY_INV);
*/
/*
int pos = 1;
IplConvKernel* element = 0;
const int element_shape = CV_SHAPE_ELLIPSE;
element = cvCreateStructuringElementEx( pos*2+1, pos*2+1, pos, pos, element_shape, 0 );
cvDilate(current_frame_gray,current_frame_gray,element,1);
cvErode(current_frame_gray,current_frame_gray,element,1);
cvReleaseStructuringElement(&element);
*/
/* 寻找棋盘图的内角点位置 */
find_corners_result = cvFindChessboardCorners(current_frame_gray,//image 输入的其盘图
ChessBoardSize, //其盘图中每行和每列角点的个数
&corners[captured_frames*NPoints],
//检测到的角点
&corner_count[captured_frames], //输出变量，角点的个数
0);
/* 精确查找角点 */
cvFindCornerSubPix( current_frame_gray,
&corners[captured_frames*NPoints],
NPoints, cvSize(2,2),cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03) );
/*
绘制检测到的棋盘角点
当棋盘没有完全被检测出来,以红色圆圈绘制
如果整个棋盘都被检测到，则用直线连接所有的角点
*/
cvDrawChessboardCorners(current_frame_rgb2, //结果图像 ，彩色
ChessBoardSize, //每行和每列角点的个数
&corners[captured_frames*NPoints], //检测到的角点数组
NPoints, //角点数目
find_corners_result); //指示完整棋盘被发现与否(非0 or 0)
cvShowImage("Window 0",current_frame_rgb2);
cvShowImage("grid",chessBoard_Img);
cvWaitKey(500);
/*
if(captured_frames>NImages-1)
break;
*/
if(find_corners_result==1) //完整棋盘被发现
{
cvWaitKey(2000);
/* write picture */
cvSaveImage("c:\\hardyinCV.jpg",current_frame_rgb2);
//cvSaveImage("c:\\hardyinCV.jpg",current_frame_gray);
captured_frames++;
cvSetTrackbarPos("Thresholdness","Window 0",++Thresholdness);
printf("-----Captured_Number=%d------\n",captured_frames);
}
cvShowImage("result",current_frame_gray);
cvShowImage("src",current_frame_rgb);
intrinsics->data.fl[0] = float(256.8093262); //fx
intrinsics->data.fl[2] = float(160.2826538); //cx
intrinsics->data.fl[4] = float(254.7511139); //fy
intrinsics->data.fl[5] = float(127.6264572); //cy
intrinsics->data.fl[1] = float(0.0);
intrinsics->data.fl[3] = float(0.0);
intrinsics->data.fl[6] = float(0.0);
intrinsics->data.fl[7] = float(0.0);
intrinsics->data.fl[8] = float(1.0);
distortion_coeff->data.fl[0] = float(-0.193740); //k1
distortion_coeff->data.fl[1] = float(-0.378588); //k2
distortion_coeff->data.fl[2] = float(0.028980); //p1
distortion_coeff->data.fl[3] = float(0.008136)
; //p2
cvWaitKey(40);
// find_corners_result = 0;
}
//if (find_corners_result !=0)
{
printf("\n");
cvSetData( image_points, corners, sizeof(CvPoint2D32f));
cvSetData( point_counts, &corner_count, sizeof(int));
cvCalibrateCamera2( object_points, //定标点的坐标 N*3
image_points, //定标点在图像内的坐标N*2
point_counts, //向量，指定不同视图里点的数目
cvSize(image_width,image_height), //image_size图像大小
intrinsics, //输出内参矩阵A [fx,0,cx; 0,fy,cy; 0,0,1]
distortion_coeff, //输出大小为4*1 [k1,k2,p1,p2]
rotation_vectors, //输出大小为3*1 旋转向量
translation_vectors, //输出大小为3*1 平移向量
0);
// [fx 0 cx; 0 fy cy; 0 0 1].
cvUndistort2(current_frame_rgb,current_frame_rgb2,intrinsics,distortion_coeff);//校正图像因摄像机镜头引起的变形
// cvShowImage("result",current_frame_rgb);
cvShowImage("dst",current_frame_rgb);
float intr[3][3] = {0.0};
float dist[4] = {0.0};
float tranv[3] = {0.0};
float rotv[3] = {0.0};
for ( int i = 0; i < 3; i++)
{//?
for ( int j = 0; j < 3; j++)
{
intr[i][j] = ((float*)(intrinsics->data.ptr + intrinsics->step*i))[j];
}
dist[i] = ((float*)(distortion_coeff->data.ptr))[i];
tranv[i] = ((float*)(translation_vectors->data.ptr))[i];
rotv[i] = ((float*)(rotation_vectors->data.ptr))[i];
}
dist[3] = ((float*)(distortion_coeff->data.ptr))[3];
//******************************
/*
float RotaVec[9]={0.0};
CvMat *rotation=cvCreateMat(1,3,CV_32FC1);
for(int m=0;i<3;m++)
(rotation->data.fl)[m]=rotv[m];
rotation->type=rotation_vectors->type;
rotation->step=rotation_vectors->step;
rotation->refcount=rotation_vectors->refcount;
rotation->data.ptr=rotation_vectors->data.ptr;
CvMat *RotationVec= cvCreateMat(3,3,CV_32FC1);
cvRodrigues2(rotation,RotationVec,0);
for(int k=0;k<9;k++)
RotaVec[k]=((float*)(RotationVec->data.ptr))[k];
*/
//******************************
printf("-----------------------------------------\n");
printf("INTRINSIC MATRIX: \n");
printf("[ %6.4f %6.4f %6.4f ] \n", intr[0][0], intr[0][1], intr[0][2]);
printf("[ %6.4f %6.4f %6.4f ] \n", intr[1][0], intr[1][1], intr[1][2]);
printf("[ %6.4f %6.4f %6.4f ] \n", intr[2][0], intr[2][1], intr[2][2]);
printf("-----------------------------------------\n");
printf("DISTORTION VECTOR: \n");
printf("[ %6.4f %6.4f %6.4f %6.4f ] \n", dist[0], dist[1], dist[2], dist[3]);
printf("-----------------------------------------\n");
printf("ROTATION VECTOR: \n");
printf("[ %6.4f %6.4f %6.4f ] \n", rotv[0], rotv[1], rotv[2]);
printf("TRANSLATION VECTOR: \n");
printf("[ %6.4f %6.4f %6.4f ] \n", tranv[0], tranv[1], tranv[2]);
printf("-----------------------------------------\n");
printf("-----------------Good Luck!--------------\n");
cvWaitKey(0);
calibration_done = true;
}
cvReleaseCapture(&capture);
}
exit(0);
cvReleaseCapture(&capture);
cvDestroyAllWindows();
}
//生成角点三维空间的坐标
void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int NImages, float SquareSize)
{
int CurrentImage = 0;
int CurrentRow = 0;
int CurrentColumn = 0;
int NPoints = ChessBoardSize.height*ChessBoardSize.width;
float * temppoints = new float[NImages*NPoints*3]; //坐标数组
// for now, assuming we're row-scanning
for (CurrentImage = 0 ; CurrentImage < NImages ; CurrentImage++)
{
for (CurrentRow = 0; CurrentRow < ChessBoardSize.height; CurrentRow++) //行
{
for (CurrentColumn = 0; CurrentColumn < ChessBoardSize.width; CurrentColumn++) //列
{
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3]=(float)CurrentRow*SquareSize;
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+1]=(float)CurrentColumn*SquareSize;
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+2]=0.0;
}
}
}
(*Corners3D) = cvMat(NImages*NPoints,3,CV_32FC1, temppoints);
}
//实现 角点检测
int myFindChessboardCorners( const void* image, CvSize pattern_size,
CvPoint2D32f* corners, int* corner_count,
int flags )
{
IplImage* eig = cvCreateImage( cvGetSize(image), 32, 1 );//创建图像头并分配空间
IplImage* temp = cvCreateImage( cvGetSize(image), 32, 1 );
double quality = 0.01;
double min_distance = 5;
int win_size =10;
int count = pattern_size.width * pattern_size.height; //角点数
cvGoodFeaturesToTrack( image, eig, temp, corners, &count, //在图像中找寻具有最大特征点的角点
quality, min_distance, 0, 3, 0, 0.04 );
cvFindCornerSubPix( image, corners, count, //寻找精确的角点位置
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
cvReleaseImage( &eig );
cvReleaseImage( &temp );
return 1;
}
//画 棋盘图
void makeChessBoard()
{
CvScalar e;
e.val[0] =255;
e.val[1] =255;
e.val[2] =255;
cvSet(chessBoard_Img,e,0);
for(int i = 0;i
int w =(image_width)/2/(ChessBoardSize.width);
int h = w; //(image_height)/2/(ChessBoardSize.height);
int ii = i+1;
int iii = ii+1;
int jj =j+1;
int jjj =jj+1;
int s_x = image_width/6;
if((i+j)%2==1)
cvRectangle( chessBoard_Img,
cvPoint(w*i+s_x,h*j+s_x),
cvPoint(w*ii-1+s_x,h*jj-1+s_x),
CV_RGB(0,0,0),CV_FILLED,
8,
0 );
}
}