单目摄像机标定程序（using openCV functions）

//转载来源 http://blog.csdn.net/guvcolie/article/details/7454632

我自己写了一个摄像机标定程序，核心算法参照learning opencv，但是那个程序要从命令行预先输入参数，且标定图片要预先准备好，我觉得不太好，我就自己写了一个，跟大家分享下。

若有纰漏，希望大家指正！

[cpp]  view plain  copy

1. #include "stdafx.h"  
2. #include "cv.h"  
3. #include "highgui.h"  
4. #include <string>  
5. #include <iostream>  
6.   
7. using namespace std;  
8.   
9. int main()  
10. {  
11.     int cube_length=7;  
12.   
13.     CvCapture* capture;  
14.   
15.     capture=cvCreateCameraCapture(0);  
16.   
17.     if(capture==0){  
18.         printf("无法捕获摄像头设备！\n\n");  
19.         return 0;  
20.     }else{  
21.         printf("捕获摄像头设备成功！！\n\n");  
22.     }  
23.   
24.     IplImage* frame;  
25.   
26.     cvNamedWindow("摄像机帧截取窗口",1);  
27.   
28.     printf("按“C”键截取当前帧并保存为标定图片...\n按“Q”键退出截取帧过程...\n\n");  
29.   
30.     int number_image=1;  
31.     char *str1;  
32.     str1=".jpg";  
33.     char filename[20]="";  
34.   
35.     while(true)  
36.     {  
37.         frame=cvQueryFrame(capture);  
38.         if(!frame)  
39.              break;  
40.         cvShowImage("摄像机帧截取窗口",frame);  
41.           
42.         if(cvWaitKey(10)=='c'){  
43.             sprintf_s (filename,"%d.jpg",number_image);  
44.             cvSaveImage(filename,frame);  
45.             cout<<"成功获取当前帧，并以文件名"<<filename<<"保存...\n\n";  
46.             printf("按“C”键截取当前帧并保存为标定图片...\n按“Q”键退出截取帧过程...\n\n");  
47.             number_image++;  
48.         }else if(cvWaitKey(10)=='q'){  
49.             printf("截取图像帧过程完成...\n\n");  
50.             cout<<"共成功截取"<<--number_image<<"帧图像！！\n\n";  
51.             break;  
52.         }  
53.     }  
54.   
55.     cvReleaseImage(&frame);  
56.     cvDestroyWindow("摄像机帧截取窗口");  
57.   
58.     IplImage * show;  
59.     cvNamedWindow("RePlay",1);  
60.   
61.     int a=1;  
62.     int number_image_copy=number_image;  
63.   
64.     CvSize board_size=cvSize(7,7);  
65.     int board_width=board_size.width;  
66.     int board_height=board_size.height;  
67.     int total_per_image=board_width*board_height;  
68.     CvPoint2D32f * image_points_buf = new CvPoint2D32f[total_per_image];  
69.     CvMat * image_points=cvCreateMat(number_image*total_per_image,2,CV_32FC1);  
70.     CvMat * object_points=cvCreateMat(number_image*total_per_image,3,CV_32FC1);  
71.     CvMat * point_counts=cvCreateMat(number_image,1,CV_32SC1);  
72.     CvMat * intrinsic_matrix=cvCreateMat(3,3,CV_32FC1);  
73.     CvMat * distortion_coeffs=cvCreateMat(5,1,CV_32FC1);  
74.   
75.     int count;  
76.     int found;  
77.     int step;  
78.     int successes=0;  
79.   
80.     while(a<=number_image_copy){  
81.         sprintf_s (filename,"%d.jpg",a);  
82.         show=cvLoadImage(filename,-1);  
83.   
84.         found=cvFindChessboardCorners(show,board_size,image_points_buf,&count,  
85.                                      CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);  
86.         if(found==0){         
87.             cout<<"第"<<a<<"帧图片无法找到棋盘格所有角点!\n\n";  
88.             cvNamedWindow("RePlay",1);  
89.             cvShowImage("RePlay",show);  
90.             cvWaitKey(0);  
91.   
92.         }else{  
93.             cout<<"第"<<a<<"帧图像成功获得"<<count<<"个角点...\n";  
94.           
95.             cvNamedWindow("RePlay",1);  
96.   
97.             IplImage * gray_image= cvCreateImage(cvGetSize(show),8,1);  
98.             cvCvtColor(show,gray_image,CV_BGR2GRAY);  
99.             cout<<"获取源图像灰度图过程完成...\n";  
100.             cvFindCornerSubPix(gray_image,image_points_buf,count,cvSize(11,11),cvSize(-1,-1),  
101.                                 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));  
102.             cout<<"灰度图亚像素化过程完成...\n";  
103.             cvDrawChessboardCorners(show,board_size,image_points_buf,count,found);  
104.             cout<<"在源图像上绘制角点过程完成...\n\n";  
105.             cvShowImage("RePlay",show);  
106.   
107.               
108.   
109.             cvWaitKey(0);  
110.         }  
111.   
112.         if(total_per_image==count){  
113.             step=successes*total_per_image;  
114.             for(int i=step,j=0;j<total_per_image;++i,++j){  
115.                 CV_MAT_ELEM(*image_points,float,i,0)=image_points_buf[j].x;  
116.                 CV_MAT_ELEM(*image_points,float,i,1)=image_points_buf[j].y;  
117.                 CV_MAT_ELEM(*object_points,float,i,0)=(float)(j/cube_length);  
118.                 CV_MAT_ELEM(*object_points,float,i,1)=(float)(j%cube_length);  
119.                 CV_MAT_ELEM(*object_points,float,i,2)=0.0f;  
120.             }  
121.             CV_MAT_ELEM(*point_counts,int,successes,0)=total_per_image;  
122.             successes++;  
123.         }  
124.         a++;  
125.     }  
126.   
127.     cvReleaseImage(&show);  
128.     cvDestroyWindow("RePlay");  
129.   
130.   
131.     cout<<"*********************************************\n";  
132.     cout<<number_image<<"帧图片中，标定成功的图片为"<<successes<<"帧...\n";  
133.     cout<<number_image<<"帧图片中，标定失败的图片为"<<number_image-successes<<"帧...\n\n";  
134.     cout<<"*********************************************\n\n";  
135.   
136.     cout<<"按任意键开始计算摄像机内参数...\n\n";  
137.   
138.   
139.     CvCapture* capture1;  
140.     capture1=cvCreateCameraCapture(0);  
141.     IplImage * show_colie;  
142.     show_colie=cvQueryFrame(capture1);  
143.       
144.       
145.     CvMat * object_points2=cvCreateMat(successes*total_per_image,3,CV_32FC1);  
146.     CvMat * image_points2=cvCreateMat(successes*total_per_image,2,CV_32FC1);  
147.     CvMat * point_counts2=cvCreateMat(successes,1,CV_32SC1);  
148.   
149.   
150.     for(int i=0;i<successes*total_per_image;++i){  
151.         CV_MAT_ELEM(*image_points2,float,i,0)=CV_MAT_ELEM(*image_points,float,i,0);  
152.         CV_MAT_ELEM(*image_points2,float,i,1)=CV_MAT_ELEM(*image_points,float,i,1);  
153.         CV_MAT_ELEM(*object_points2,float,i,0)=CV_MAT_ELEM(*object_points,float,i,0);  
154.         CV_MAT_ELEM(*object_points2,float,i,1)=CV_MAT_ELEM(*object_points,float,i,1);  
155.         CV_MAT_ELEM(*object_points2,float,i,2)=CV_MAT_ELEM(*object_points,float,i,2);  
156.     }  
157.   
158.     for(int i=0;i<successes;++i){  
159.         CV_MAT_ELEM(*point_counts2,int,i,0)=CV_MAT_ELEM(*point_counts,int,i,0);  
160.     }  
161.   
162.       
163.     cvReleaseMat(&object_points);  
164.     cvReleaseMat(&image_points);  
165.     cvReleaseMat(&point_counts);  
166.   
167.   
168.     CV_MAT_ELEM(*intrinsic_matrix,float,0,0)=1.0f;  
169.     CV_MAT_ELEM(*intrinsic_matrix,float,1,1)=1.0f;  
170.       
171.   
172.     cvCalibrateCamera2(object_points2,image_points2,point_counts2,cvGetSize(show_colie),  
173.                         intrinsic_matrix,distortion_coeffs,NULL,NULL,0);  
174.       
175.     cout<<"摄像机内参数矩阵为：\n";  
176.     cout<<CV_MAT_ELEM(*intrinsic_matrix,float,0,0)<<"    "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,1)  
177.                                                             <<"    "<<CV_MAT_ELEM(*intrinsic_matrix,float,0,2)  
178.                                                             <<"\n\n";  
179.     cout<<CV_MAT_ELEM(*intrinsic_matrix,float,1,0)<<"    "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,1)  
180.                                                             <<"    "<<CV_MAT_ELEM(*intrinsic_matrix,float,1,2)  
181.                                                             <<"\n\n";  
182.     cout<<CV_MAT_ELEM(*intrinsic_matrix,float,2,0)<<"    "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,1)  
183.                                                             <<"          "<<CV_MAT_ELEM(*intrinsic_matrix,float,2,2)  
184.                                                             <<"\n\n";  
185.   
186.     cout<<"畸变系数矩阵为：\n";  
187.     cout<<CV_MAT_ELEM(*distortion_coeffs,float,0,0)<<"    "<<CV_MAT_ELEM(*distortion_coeffs,float,1,0)  
188.                                                             <<"    "<<CV_MAT_ELEM(*distortion_coeffs,float,2,0)  
189.                                                             <<"    "<<CV_MAT_ELEM(*distortion_coeffs,float,3,0)  
190.                                                             <<"    "<<CV_MAT_ELEM(*distortion_coeffs,float,4,0)  
191.                                                             <<"\n\n";  
192.   
193.     cvSave("Intrinsics.xml",intrinsic_matrix);  
194.     cvSave("Distortion.xml",distortion_coeffs);  
195.   
196.     cout<<"摄像机矩阵、畸变系数向量已经分别存储在名为Intrinsics.xml、Distortion.xml文档中\n\n";  
197.   
198.     CvMat * intrinsic=(CvMat *)cvLoad("Intrinsics.xml");  
199.     CvMat * distortion=(CvMat *)cvLoad("Distortion.xml");  
200.   
201.     IplImage * mapx=cvCreateImage(cvGetSize(show_colie),IPL_DEPTH_32F,1);  
202.     IplImage * mapy=cvCreateImage(cvGetSize(show_colie),IPL_DEPTH_32F,1);  
203.       
204.     cvInitUndistortMap(intrinsic,distortion,mapx,mapy);  
205.   
206.     cvNamedWindow("原始图像",1);  
207.     cvNamedWindow("非畸变图像",1);  
208.   
209.     cout<<"按‘E’键退出显示...\n\n";  
210.   
211.     while(show_colie){  
212.         IplImage * clone=cvCloneImage(show_colie);  
213.         cvShowImage("原始图像",show_colie);  
214.         cvRemap(clone,show_colie,mapx,mapy);  
215.         cvReleaseImage(&clone);  
216.         cvShowImage("非畸变图像",show_colie);  
217.   
218.         if(cvWaitKey(10)=='e'){  
219.             break;  
220.         }  
221.   
222.         show_colie=cvQueryFrame(capture1);  
223.     }  
224.   
225.     return 0;  
226.       
227. }