Using opencv to achieve single target positioning
The purpose of camera calibration : to obtain the internal and external parameter matrix of the camera (at the same time, the selection and translation matrix of each calibration image will also be obtained). The internal and external parameter coefficients can correct the images captured by the camera later, and the distortion is relatively small. small image.
Input of camera calibration : the image coordinates of all inner corner points on the calibration image, and the three-dimensional space coordinates of all inner corner points on the calibration plate image (generally, it is assumed that the image is located on the Z=0 plane).
Output of camera calibration : internal and external parameters of the camera.
These three basic problems determine the complete process of using Opencv to realize the calibration process of Zhang Zhengyou's calibration camera, the evaluation of calibration results, and the correction of the original image using the calibration results:
-
1. Prepare the calibration image
-
2. For each calibration picture, extract corner information
-
3. For each calibration picture, further extract sub-pixel corner information
-
4. Draw the found interior corner points on the checkerboard calibration map (not necessary, just for display)
-
5. Camera Calibration
-
6. Evaluate the calibration results
-
7. Check the calibration effect - use the calibration results to correct the checkerboard diagram
Prepare calibration image
Calibration pictures need to be taken at different positions, different angles, and different postures using the calibration board. At least 3 pictures are required, and 10-20 pictures are appropriate. The calibration board needs to be a checkerboard diagram composed of black and white rectangles, and the production accuracy is required to be high.
Here we use the calibration picture in the sample program provided by OpenCV. The picture is located in the installation path of opencv (C++ version): opencv\sources\samples\data:
We first create a C++ console project, and store the calibration image in the following format:
There are two folders left and right under the sample folder, corresponding to the calibration board pictures captured by the left camera and the right camera respectively:
filename.txt stores the path of the calibration image, the content is as follows:
For the API functions provided by OpenCV for camera calibration, you can refer to the blog binocular vision calibration program explanation. The code for single target calibration is as follows:
/************************************************************************************** Description：相机标定，张氏标定法 单目标定* Author ：JNU* Data ：2018.7.22*************************************************************************************/#include <opencv2/core/core.hpp>#include <opencv2/imgproc/imgproc.hpp>#include <opencv2/calib3d/calib3d.hpp>#include <opencv2/highgui/highgui.hpp>#include <iostream>#include <fstream>#include <vector>using namespace cv;using namespace std;void main(char *args){//保存文件名称std::vector<std::string> filenames;//需要更改的参数//左相机标定，指定左相机图片路径，以及标定结果保存文件string infilename = "sample/left/filename.txt"; //如果是右相机把left改为rightstring outfilename = "sample/left/caliberation_result.txt";//标定所用图片文件的路径,每一行保存一个标定图片的路径 ifstream 是从硬盘读到内存ifstream fin(infilename);//保存标定的结果 ofstream 是从内存写到硬盘ofstream fout(outfilename);/*1.读取毎一幅图像，从中提取出角点，然后对角点进行亚像素精确化、获取每个角点在像素坐标系中的坐标像素坐标系的原点位于图像的左上角*/std::cout << "开始提取角点......" << std::endl;;//图像数量int imageCount = 0;//图像尺寸cv::Size imageSize;//标定板上每行每列的角点数cv::Size boardSize = cv::Size(9, 6);//缓存每幅图像上检测到的角点std::vector<Point2f> imagePointsBuf;//保存检测到的所有角点std::vector<std::vector<Point2f>> imagePointsSeq;char filename[100];if (fin.is_open()){//读取完毕？while (!fin.eof()){//一次读取一行fin.getline(filename, sizeof(filename) / sizeof(char));//保存文件名filenames.push_back(filename);//读取图片Mat imageInput = cv::imread(filename);//读入第一张图片时获取图宽高信息if (imageCount == 0){imageSize.width = imageInput.cols;imageSize.height = imageInput.rows;std::cout << "imageSize.width = " << imageSize.width << std::endl;std::cout << "imageSize.height = " << imageSize.height << std::endl;}std::cout << "imageCount = " << imageCount << std::endl;imageCount++;//提取每一张图片的角点if (cv::findChessboardCorners(imageInput, boardSize, imagePointsBuf) == 0){//找不到角点std::cout << "Can not find chessboard corners!" << std::endl;exit(1);}else{Mat viewGray;//转换为灰度图片cv::cvtColor(imageInput, viewGray, cv::COLOR_BGR2GRAY);//亚像素精确化 对粗提取的角点进行精确化cv::find4QuadCornerSubpix(viewGray, imagePointsBuf, cv::Size(5, 5));//保存亚像素点imagePointsSeq.push_back(imagePointsBuf);//在图像上显示角点位置cv::drawChessboardCorners(viewGray, boardSize, imagePointsBuf, true);//显示图片//cv::imshow("Camera Calibration", viewGray);cv::imwrite("test.jpg", viewGray);//等待0.5s//waitKey(500);}}//计算每张图片上的角点数 54int cornerNum = boardSize.width * boardSize.height;//角点总数int total = imagePointsSeq.size()*cornerNum;std::cout << "total = " << total << std::endl;for (int i = 0; i < total; i++){int num = i / cornerNum;int p = i%cornerNum;//cornerNum是每幅图片的角点个数，此判断语句是为了输出，便于调试if (p == 0){std::cout << "\n第 " << num+1 << "张图片的数据 -->: " << std::endl;}//输出所有的角点std::cout<<p+1<<":("<< imagePointsSeq[num][p].x;std::cout << imagePointsSeq[num][p].y<<")\t";if ((p+1) % 3 == 0){std::cout << std::endl;}}std::cout << "角点提取完成!" << std::endl;/*2.摄像机标定 世界坐标系原点位于标定板左上角(第一个方格的左上角)*/std::cout << "开始标定" << std::endl;//棋盘三维信息，设置棋盘在世界坐标系的坐标//实际测量得到标定板上每个棋盘格的大小cv::Size squareSize = cv::Size(26, 26);//毎幅图片角点数量std::vector<int> pointCounts;//保存标定板上角点的三维坐标std::vector<std::vector<cv::Point3f>> objectPoints;//摄像机内参数矩阵 M=[fx γ u0,0 fy v0,0 0 1]cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));//摄像机的5个畸变系数k1,k2,p1,p2,k3cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));//每幅图片的旋转向量std::vector<cv::Mat> tvecsMat;//每幅图片的平移向量std::vector<cv::Mat> rvecsMat;//初始化标定板上角点的三维坐标int i, j, t;for (t = 0; t < imageCount; t++){std::vector<cv::Point3f> tempPointSet;//行数for (i = 0; i < boardSize.height; i++){//列数for (j = 0; j < boardSize.width; j++){cv::Point3f realPoint;//假设标定板放在世界坐标系中z=0的平面上。realPoint.x = i*squareSize.width;realPoint.y = j*squareSize.height;realPoint.z = 0;tempPointSet.push_back(realPoint);}}objectPoints.push_back(tempPointSet);}//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板for (i = 0; i < imageCount; i++){pointCounts.push_back(boardSize.width*boardSize.height);}//开始标定cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);std::cout << "标定完成" << std::endl;//对标定结果进行评价std::cout << "开始评价标定结果......" << std::endl;//所有图像的平均误差的总和double totalErr = 0.0;//每幅图像的平均误差double err = 0.0;//保存重新计算得到的投影点std::vector<cv::Point2f> imagePoints2;std::cout << "每幅图像的标定误差:" << std::endl;fout << "每幅图像的标定误差:" << std::endl;for (i = 0; i < imageCount; i++){std::vector<cv::Point3f> tempPointSet = objectPoints[i];//通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点imagePoints2（在像素坐标系下的点坐标）cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);//计算新的投影点和旧的投影点之间的误差std::vector<cv::Point2f> tempImagePoint = imagePointsSeq[i];cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);cv::Mat imagePoints2Mat = cv::Mat(1, imagePoints2.size(), CV_32FC2);for (int j = 0; j < tempImagePoint.size(); j++){imagePoints2Mat.at<cv::Vec2f>(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);tempImagePointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);}//Calculates an absolute difference norm or a relative difference norm.err = cv::norm(imagePoints2Mat, tempImagePointMat, NORM_L2);totalErr += err /= pointCounts[i];std::cout << " 第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;fout<< "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;}//每张图像的平均总误差std::cout << " 总体平均误差:" << totalErr / imageCount << "像素" << std::endl;fout << "总体平均误差:" << totalErr / imageCount << "像素" << std::endl;std::cout << "评价完成!" << std::endl;//保存标定结果std::cout << "开始保存标定结果....." << std::endl;//保存每张图像的旋转矩阵cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));fout << "相机内参数矩阵:" << std::endl;fout << cameraMatrix << std::endl << std::endl;fout << "畸变系数:" << std::endl;fout << distCoeffs << std::endl << std::endl;for (int i = 0; i < imageCount; i++){fout << "第" << i + 1 << "幅图像的旋转向量:" << std::endl;fout << tvecsMat[i] << std::endl;//将旋转向量转换为相对应的旋转矩阵cv::Rodrigues(tvecsMat[i], rotationMatrix);fout << "第" << i + 1 << "幅图像的旋转矩阵:" << std::endl;fout << rotationMatrix << std::endl;fout << "第" << i + 1 << "幅图像的平移向量:" << std::endl;fout << rvecsMat[i] << std::endl;}std::cout << "保存完成" << std::endl;/************************************************************************显示定标结果*************************************************************************/cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);cv::Mat R = cv::Mat::eye(3, 3, CV_32F);std::cout << "显示矫正图像" << endl;for (int i = 0; i != imageCount; i++){std::cout << "Frame #" << i + 1 << "..." << endl;//计算图片畸变矫正的映射矩阵mapx、mapy(不进行立体校正、立体校正需要使用双摄)initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);//读取一张图片Mat imageSource = imread(filenames[i]);Mat newimage = imageSource.clone();//另一种不需要转换矩阵的方式//undistort(imageSource,newimage,cameraMatrix,distCoeffs);//进行校正remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);imshow("原始图像", imageSource);imshow("矫正后图像", newimage);waitKey();}//释放资源fin.close();fout.close();system("pause");}}
There are two functions above that need to be introduced separately:
CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,InputArray R, InputArray newCameraMatrix,Size size, int m1type, OutputArray map1, OutputArray map2 );
Function: This function is to calculate the mapping transformation matrix of distortion correction and camera stereo correction. For remapping, the result is expressed in the form of a map. An undistorted image looks the same as the original image, as if the image was newCameraMatrixcaptured with an undistorted camera. This function actually builds the map for the reverse mapping algorithm to be used by the reverse mapping. That is, for each pixel $(u,v)$ in the corrected distortion image, the function calculates the corresponding coordinate system in the original image (the original image obtained from the camera).
Parameter Description:
-
cameraMatrix: input camera internal parameter matrix
-
distCoeffs: input parameters, the distortion coefficient of the camera
There are 4, 5, 8, 12 or 14 elements. If this vector is empty, it is assumed to have zero distortion coefficients.
-
RR: optional stereo correction transformation matrix, which is a 3×33×3 matrix.
In the example of a monocular camera, $R$ is set to the identity matrix cv::Mat R = cv::Mat::eye(3, 3, CV_32F), indicating that no stereo correction is performed.
In the case of a binocular camera, newCameraMatrix is generally calculated with cv::stereoRectify() and set to R1R1 or R2R2 (the corrected rotation matrix for the alignment of the left and right camera planes). Furthermore, the orientation of the new camera in the coordinate space is different according to RR. For example, it helps to register the two camera orientations of a stereo camera so that the epipolar lines of the two images are horizontal and the yy coordinates are the same (in the case of a stereo camera where both cameras are placed horizontally).
-
newCameraMatrix: new camera internal parameter matrix
In the monocular camera example, newCameraMatrix is generally equal to cameraMatrix, or can be calculated with cv::getOptimalNewCameraMatrix() to obtain a better scaled control result.
In the binocular camera example, newCameraMatrix is generally calculated by cv::stereoRectify() and set to P1 or P2 (the projection matrix for the left and right cameras to convert the coordinates of 3D points in space to the coordinates of 2D points in the image).
-
size: The undistorted image size.
-
m1type: The type of the first output map, which can be CV_32FC1, CV_32FC2 or CV_16SC2, see cv::convertMaps.
-
map1: The first output map.
-
map2: The second output map.
void remap(InputArray src, OutputArray dst, InputArray map1, InputArraymap2, int interpolation,int borderMode=BORDER_CONSTANT, const Scalar&borderValue=Scalar())
Function: Remapping: It is the process of placing a pixel at a certain position in one image to a specified position in another picture.
Parameter Description:
-
src: The input image, that is, the original image, requires a single-channel 8-bit or floating-point type image
-
dst: The output image, that is, the target image, needs to be the same size and type as the original image
-
map1: It has two possible objects: (1) represents the first mapping of points (x, y); (2) represents CV_16SC2, CV_32FC1, etc.
-
map2: It has two possible objects: (1) If map1 represents a point (x, y), this parameter does not represent any value; (2) represents the Y value of CV_16UC1, CV_32FC1 type
-
interpolation: interpolation method, there are four interpolation methods:
(1) INTER_NEAREST - nearest neighbor interpolation
(2) INTER_LINEAR - bilinear interpolation (default)
(3) INTER_CUBIC - double triple spline interpolation (default)
(4) INTER_LANCZOS4 - lanczos interpolation (default)
-
borderMode: border mode, default BORDER_CONSTANT
-
borderValue: border color, default Scalar() black
After the program runs, save the internal parameters and external parameters of the camera in the calibration_result.txt file, the content is as follows
Calibration error per image:
Average error for image 1: 0.0644823 pixels
Average error for image 2: 0.0769712 pixels
Average error for image 3: 0.057877 pixels
Average error for image 4: 0.0596713 pixels Image
5 Average error of images: 0.0625956 pixels
Average error of 6th image: 0.0658863 pixels
Average error of 7th image: 0.0568134 pixels Average error of
8th image: 0.0643699 pixels
Average error of 9th image: 0.058048 pixels
10th image Average error of images: 0.0565483 pixels
Average error of 11th image: 0.0590138 pixels
Average error of 12th image: 0.0569968 pixels Average error of 13th image: 0.0698826 pixels
Overall average error: 0.0622428 pixels
Camera
intrinsic parameters matrix:
[530.5277314196954 , 0, 338.8371277433631;
0, 530.5883296858968, 231.5390118666163;
0, 0, 1]
Distortion coefficient:
[-0.2581406917163123, -0.11124480187392, 0.00 04630258905514519, -0.0009475605555950018, 0.413646790569884]
Rotation vector for the 1st image:
[-75.22204622827574;
-109.7328226714255;
412.7511174854986]
第1幅图像的旋转矩阵:
[0.9927105083879407, -0.1161407096490343, -0.03220531164846807;
0.1168004495051158, 0.9929655913965856, 0.01941621224214358;
0.02972375365863362, -0.02303627280285992, 0.999292664139887]
第1幅图像的平移向量:
[-1.985720132175791 ;
-2.010141521348128;
0.1175016759367312]
第2幅图像的旋转向量:
[-57.88571684656549;
88.73102475029921;
365.4767680110305]
第2幅图像的旋转矩阵:
[-0.880518198944593, 0.2965025784551226, -0.36982958548071;
-0.4330747951156081, -0.8203927789645991, 0.3733656519530371;
-0.192701642865192, 0.4889191233652108, 0.8507785655767596]
Translation vector of 2nd image:
[-2.431974050326802;
-0.2015324617416875 ;
0.2103186188188722]
Rotation vector of 3rd image:
[-38.96229403649615;
-101.619482335263;
328.79 91741655258]
The rotation matrix of the 3rd image:
[0.7229826652152683, -0.6501194230369263, -0.2337537199455046 ;
0.6686409526220074, 0.7435854196067706, -1.49985835111166e-05;
0.1738256088007802, -0.1562864662674188, 0.9722958388199968]
第3幅图像的平移向量:
[1.726707502757928;
2.49410066154742;
-0.5169212442744683]
第4幅图像的旋转向量:
[-99.94408740929534;
-67.11904896100746;
341.7035262057663 ]
The rotation matrix of the 4th image:
[-0.4166240767662854, 0.8762113538151707, -0.2422355095852507;
-0.7194830230098562, -0.4806860756468779, -0.5012834290895748;
-0.5556694685325433, -0.03456240912595265, 0.8306845861192869]
第4幅图像的平移向量:
[-2.144507828065959;
-2.137658756455213;
0.3861555312888436]
第5幅图像的旋转向量:
[63.1817601794685;
-117.2855578733511;
327.5340459209377]
第5幅图像的旋转矩阵:
[-0.1237680939389874, -0.9830519969136794, -0.1352413778646805;
0.8454470843144938, -0.03311262698003439, -0.5330316890754268;
0.5195196690663707, -0.1803117447603135, 0.8352167312468426]
第5幅图像的平移向量:
[-0.3394208745634724;
-2.941274925899604;
0.7239987875443074]
The rotation vector of the 6th image:
[176.6380486063267;
-65.02048705679623;
345.2669628180993]
第6幅图像的旋转矩阵:
[-0.4823787195065527, 0.3144101256594393, 0.8175922234525194;
-0.5902636261183672, -0.8063068742380883, -0.03818476447485269;
0.6472245534965549, -0.5010144682933011, 0.5745301383843724]
第6幅图像的平移向量:
[0.144403698794371;
-2.686413562533621 ;
-0.08279238304814077]
第7幅图像的旋转向量:
[23.37912628758978;
-71.28708027930361;
401.7783087659996]
第7幅图像的旋转矩阵:
[0.950756682549477, -0.3056521783663705, -0.05136610212392408;
0.3046663933949521, 0.9520979509442887, -0.02622747687825021;
0.05692204602107398, 0.009286423831555549, 0.9983354361181394]
The translation vector for the 7th image:
[0.4433620069430767;
-2.778035766165631;
0.1565310822654871]
第8幅图像的旋转向量:
[84.53413910746443;
-88.75268154189268;
326.4489757550855]
第8幅图像的旋转矩阵:
[-0.882333219506006, -0.1387045774185431, 0.4497211691251699;
-0.1080922696912742, -0.870309912144045, -0.4804963247068739;
0.4580438308602738, -0.4725692510383723, 0.7529104541603049]
Translation vector for image 8:
[0.3026042878663719;
-2.832559861959414;
0.51976000788 74884]
Rotation vector for 9th image: [
-66.87955552666558;
-81.79728232518671;
287.3798612501427]
Rotation matrix for 9th image:
[- 0.06408698919457989, 0.997286705569611, 0.03622270986668297;
-0.8668814706204128, -0.03765202403427882, -0.4970903750638435;
-0.4943777641752957, -0.06325782149453277, 0.8669423708118097]
第9幅图像的平移向量:
[1.918018245182696;
2.198445482038513;
0.6398190872020209]
第10幅图像的旋转向量:
[51.38889872566385;
-112.4792732922813;
348.8614284720838]
第10幅图像的旋转矩阵:
[0.8410751829508221, 0.5075468667660225, 0.1870527055678015;
-0.521221221444936, 0.852916565973049, 0.0293559159998552;
-0.1446408481020841, -0.1221863720908967, 0.9819111546039054]
第10幅图像的平移向量:
[0.2388869800501047;
2.534868757127185;
0.05816455567725017]
第11幅图像的旋转向量:
[55.25157597573984;
-103.974863603741;
332.3331998859927]
第11幅图像的旋转矩阵:
[0.7603104175748064, -0.6302201082550355, -0.1573235013538499;
0.6075084686586226, 0.7756458925501082, -0.1711926104661106;
0.2299163531271294, 0.0345841657577196, 0.9725957053388442]
第11幅图像的平移向量:
[-0.02801590475009446;
-3.011578659457537;
0.5796308944847007]
第12幅图像的旋转向量:
[37.20265745451167;
-92.46700742075161;
299.3885458741333]
第12幅图像的旋转矩阵:
[0.1968247409885918, -0.9604756585987335, -0.1968413843024444;
0.9041946443200382, 0.2554459280495449, -0.3423148010616344;
0.3790673640894628, -0.1106069034112951, 0.9187350251296783]
第Translation vectors for 12 images:
[-0.4442257873668548;
-2.891665626351126;
-0.7306268697464358]
第13幅图像的旋转向量:
[49.15686896201693;
-109.7597615043953;
322.2472823512488]
第13幅图像的旋转矩阵:
[-0.02527960043733595, 0.888126856668879, 0.4589026348422781;
-0.9835935284565535, 0.05992383782219021, -0.170155530145356;
-0.1786189031992861, -0.4556751256368033, 0.8720409779911538]
Translation vector for image 13:
[0.2685697410235677;
2.70549028727733;
0.25750202686 14151]
Attached below is a source code from another blog:
/*************************************************************************************** Description：相机标定，张氏标定法 单目标定，一次只能标定一个相机OPENCV3.0 单目摄像头标定(使用官方自带的标定图片)https://blog.csdn.net/zc850463390zc/article/details/48946855* Author ：JNU* Data ：2018.7.22*************************************************************************************/#include <opencv2/opencv.hpp>#include <highgui.hpp>#include "cv.h"#include <cv.hpp>#include <iostream>using namespace std;using namespace cv;//程序运行之前需要更改的参数//使用官方标定图片集？//#define SAMPLE#define MY_DATA#ifdef SAMPLE/* 官方数据集 */const int imageWidth = 640; //摄像头的分辨率const int imageHeight = 480;const int boardWidth = 9; //横向的角点数目const int boardHeight = 6; //纵向的角点数据const int boardCorner = boardWidth * boardHeight; //总的角点数据const int frameNumber = 13; //相机标定时需要采用的图像帧数const int squareSize = 20; //标定板黑白格子的大小 单位mmconst Size boardSize = Size(boardWidth, boardHeight);const char imageFilePathFormat[] = "sample/right%02d.jpg"; //用于标定的图片路径，格式化字符串sample/left%02d.bmp表明图片路径为 sample/left01.bmp - sample/leftxx.bmp#elif defined MY_DATA//自己的数据const int imageWidth = 1600; //摄像头的分辨率const int imageHeight = 1200;const int boardWidth = 9; //横向的角点数目const int boardHeight = 6; //纵向的角点数据const int boardCorner = boardWidth * boardHeight; //总的角点数据const int frameNumber = 10; //相机标定时需要采用的图像帧数const int squareSize = 30; //标定板黑白格子的大小 单位mmconst Size boardSize = Size(boardWidth, boardHeight);Size imageSize = Size(imageWidth, imageHeight);const char imageFilePathFormat[] = "image/right/%d.bmp";#endif // SAMPLEMat intrinsic; //相机内参数Mat distortion_coeff; //相机畸变参数vector<Mat> rvecs; //旋转向量vector<Mat> tvecs; //平移向量vector<vector<Point2f>> corners; //各个图像找到的角点的集合 和objRealPoint 一一对应vector<vector<Point3f>> objRealPoint; //各副图像的角点的实际物理坐标集合vector<Point2f> corner; //某一副图像找到的角点Mat rgbImage, grayImage;/*计算标定板上模块的实际物理坐标*/void calRealPoint(vector<vector<Point3f>>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize){// Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));vector<Point3f> imgpoint;for (int rowIndex = 0; rowIndex < boardheight; rowIndex++){for (int colIndex = 0; colIndex < boardwidth; colIndex++){// imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);imgpoint.push_back(Point3f(rowIndex * squaresize, colIndex * squaresize, 0));}}for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++){obj.push_back(imgpoint);}}/*设置相机的初始参数 也可以不估计*/void guessCameraParam(void){/*分配内存*/intrinsic.create(3, 3, CV_64FC1);distortion_coeff.create(5, 1, CV_64FC1);/*fx 0 cx0 fy cy0 0 1*/intrinsic.at<double>(0, 0) = 256.8093262; //fxintrinsic.at<double>(0, 2) = 160.2826538; //cxintrinsic.at<double>(1, 1) = 254.7511139; //fyintrinsic.at<double>(1, 2) = 127.6264572; //cyintrinsic.at<double>(0, 1) = 0;intrinsic.at<double>(1, 0) = 0;intrinsic.at<double>(2, 0) = 0;intrinsic.at<double>(2, 1) = 0;intrinsic.at<double>(2, 2) = 1;/*k1 k2 p1 p2 p3*/distortion_coeff.at<double>(0, 0) = -0.193740; //k1distortion_coeff.at<double>(1, 0) = -0.378588; //k2distortion_coeff.at<double>(2, 0) = 0.028980; //p1distortion_coeff.at<double>(3, 0) = 0.008136; //p2distortion_coeff.at<double>(4, 0) = 0; //p3}void outputCameraParam(void){/*保存数据*///cvSave("cameraMatrix.xml", &intrinsic);//cvSave("cameraDistoration.xml", &distortion_coeff);//cvSave("rotatoVector.xml", &rvecs);//cvSave("translationVector.xml", &tvecs);/*保存数据*//*输出数据*/FileStorage fs("intrinsics.yml", FileStorage::WRITE);if (fs.isOpened()){fs << "intrinsic" << intrinsic << "distortion_coeff" << distortion_coeff ;fs.release();}else{cout << "Error: can not save the intrinsics!!!!!" << endl;}fs.open("extrinsics.yml", FileStorage::WRITE);if (fs.isOpened()){fs << "rvecs" << rvecs << "tvecs" << tvecs;fs.release();}else{cout << "Error: can not save the extrinsics parameters\n";}/*输出数据*/cout << "fx :" << intrinsic.at<double>(0, 0) << endl << "fy :" << intrinsic.at<double>(1, 1) << endl;cout << "cx :" << intrinsic.at<double>(0, 2) << endl << "cy :" << intrinsic.at<double>(1, 2) << endl;cout << "k1 :" << distortion_coeff.at<double>(0, 0) << endl;cout << "k2 :" << distortion_coeff.at<double>(1, 0) << endl;cout << "p1 :" << distortion_coeff.at<double>(2, 0) << endl;cout << "p2 :" << distortion_coeff.at<double>(3, 0) << endl;cout << "p3 :" << distortion_coeff.at<double>(4, 0) << endl;}void main(char *args){Mat img;int goodFrameCount = 0;namedWindow("chessboard");cout << "按Q退出 ..." << endl;while (goodFrameCount < frameNumber){char filename[100];//sprintf_s(filename, "image/right/%d.bmp", goodFrameCount + 1);sprintf_s(filename, imageFilePathFormat, goodFrameCount + 1);// cout << filename << endl;rgbImage = imread(filename, CV_LOAD_IMAGE_COLOR);cvtColor(rgbImage, grayImage, CV_BGR2GRAY);imshow("Camera", grayImage);bool isFind = findChessboardCorners(rgbImage, boardSize, corner, 0);if (isFind == true) //所有角点都被找到 说明这幅图像是可行的{/*Size(5,5) 搜索窗口的一半大小Size(-1,-1) 死区的一半尺寸TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)迭代终止条件*/cornerSubPix(grayImage, corner, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));drawChessboardCorners(rgbImage, boardSize, corner, isFind);imshow("chessboard", rgbImage);corners.push_back(corner);//string filename = "res\\image\\calibration";//filename += goodFrameCount + ".jpg";//cvSaveImage(filename.c_str(), &IplImage(rgbImage)); //把合格的图片保存起来goodFrameCount++;cout << "The image is good" << endl;}else{cout << goodFrameCount+1 <<" The image is bad please try again" << endl;}// cout << "Press any key to continue..." << endl;// waitKey(0);if (waitKey(10) == 'q'){break;}// imshow("chessboard", rgbImage);}/*图像采集完毕 接下来开始摄像头的校正calibrateCamera()输入参数 objectPoints 角点的实际物理坐标imagePoints 角点的图像坐标imageSize 图像的大小输出参数cameraMatrix 相机的内参矩阵distCoeffs 相机的畸变参数rvecs 旋转矢量(外参数)tvecs 平移矢量(外参数）*//*设置实际初始参数 根据calibrateCamera来 如果flag = 0 也可以不进行设置*/guessCameraParam();cout << "guess successful" << endl;/*计算实际的校正点的三维坐标*/calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);cout << "cal real successful" << endl;/*标定摄像头*/calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);cout << "calibration successful" << endl;/*保存并输出参数*/outputCameraParam();cout << "out successful" << endl;/*显示畸变校正效果*/Mat cImage;undistort(rgbImage, cImage, intrinsic, distortion_coeff);imshow("Corret Image", cImage);cout << "Correct Image" << endl;cout << "Wait for Key" << endl;waitKey(0);system("pause");}