Opencv 摄像头矫正

摄像机有6个外参数（3个旋转，3个平移），5个内参数（fx，fy,cx,cy,θ）,摄像机的内参数在不同的视场，分辨率中是一样的，但是不同的视角下6个外参数是变化的，一个平面物体可以固定8个参数，（为了表明平面投影视场中的所有目标只需要4个点，无论在一个平面中我们检测到多少个角点，只有4个有用的角点，每个角点有X,Y两个坐标，一共有8个方程）

摄像头标定其实就是把三维坐标的点首先经过平移，旋转将世界坐标系变到摄像机坐标系，然后根据三角几何变换得到图像物理坐标系，最后根据像素和公制单位的比率得到图像像素坐标系。

这里就是一系列的方程式求解，可以参考《学习Opencv》和 !(标定参考资料)[https://files.cnblogs.com/files/shhu1993/Maintex.pdf]

下面有一个代码实例

#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>

using namespace std;

int n_boards = 1;
const int board_dt = 10;
int board_w = 10;
int board_h = 8;
float measure = 20;// 棋盘格单元大小

int x_coordinate[10] = { 4,3,2,1,0,0,0,0,0,0 };//x,y 的坐标（4，0）（3，0）...(0,1)(0,2)...
int y_coordinate[10] = { 0,0,0,0,0,1,2,3,4,5 };

int main(int argc, char* argv[])
{
int board_n = board_w * board_h; //总的conner 点数
CvSize board_sz = cvSize(board_w, board_h);
cvNamedWindow("Calibration");
//ALLOCATE STORAGE
CvMat* image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);

CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1); 
CvMat* distortion_coeffs = cvCreateMat(4, 1, CV_32FC1);


IplImage* image = 0;
IplImage* gray_image = 0;	//for subpixel
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count;

image = cvLoadImage("A.jpg", 1);
//求角点
if (gray_image == 0 && image) // need this for subpixel accurate stuff
gray_image = cvCreateImage(cvGetSize(image), 8, 1);

int found = cvFindChessboardCorners(
image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);

//Get Subpixel accuracy on those corners
cvCvtColor(image, gray_image, CV_BGR2GRAY);
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
//Draw it
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
cvShowImage("Calibration", image);
//存储角点信息
// If we got a good board, add it to our data
if (corner_count == board_n) {
for (int i = 0; i < board_h; ++i)
for (int j = 0; j < board_w; ++j) {
int k = i*board_w + j;
CV_MAT_ELEM(*image_points, float, k, 0) = corners[k].x;
CV_MAT_ELEM(*image_points, float, k, 1) = corners[k].y;
CV_MAT_ELEM(*object_points, float, k, 0) = x_coordinate[j]*measure;
CV_MAT_ELEM(*object_points, float, k, 1) = y_coordinate[j]* measure;
CV_MAT_ELEM(*object_points, float, k, 2) = (8 - i)*measure;
}
}
//构造P矩阵
CvMat* U_matrix = cvCreateMat(2 * board_n, 12, CV_32FC1);
float Even_line[12];
float Odd_line[12];
memset(Even_line, 0.0f, sizeof(float) * 12);
memset(Odd_line, 0.0f, sizeof(float) * 12);
Even_line[3] = Odd_line[7] = 1;
for (int i = 0, j = 0; i < 2 * board_n; i += 2, j++) {
Even_line[0] = Odd_line[4] = CV_MAT_ELEM(*object_points, float, j, 0);//Px
Even_line[1] = Odd_line[5] = CV_MAT_ELEM(*object_points, float, j, 1);//Py
Even_line[2] = Odd_line[6] = CV_MAT_ELEM(*object_points, float, j, 2);//Pz
Even_line[11] = -CV_MAT_ELEM(*image_points, float, j, 0);//u1
Odd_line[11] = -CV_MAT_ELEM(*image_points, float, j, 1);//v1
Even_line[8] = Even_line[11] * Even_line[0];//
Even_line[9] = Even_line[11] * Even_line[1];
Even_line[10] = Even_line[11] * Even_line[2];
Odd_line[8] = Odd_line[11] * Odd_line[4];
Odd_line[9] = Odd_line[11] * Odd_line[5];
Odd_line[10] = Odd_line[11] * Odd_line[6];

for (int j = 0; j < 12; j++) CV_MAT_ELEM(*U_matrix, float, i, j) = Even_line[j];
for (int j = 0; j < 12; j++) CV_MAT_ELEM(*U_matrix, float, i + 1, j) = Odd_line[j];
}
//计算内参、外参
CvMat* UtU = cvCreateMat(12, 12, CV_32FC1);
cvGEMM(U_matrix, U_matrix, 1, NULL, 1, UtU, CV_GEMM_A_T);//||U_matrix||
CvMat* UtU_evects = cvCreateMat(12, 12, CV_32FC1);
CvMat* UtU_evals = cvCreateMat(1, 12, CV_32FC1);
cvEigenVV(UtU, UtU_evects, UtU_evals, 0);//特征向量，特征值

double UtU_evals_min_val;
CvPoint min_loc;
cvMinMaxLoc(UtU_evals, &UtU_evals_min_val, NULL, &min_loc, NULL, NULL);
//cout << UtU_evals_min_val << endl;
CvMat* M = cvCreateMat(12, 1, CV_32FC1);
for (int i = 0; i < 12; i++) CV_MAT_ELEM(*M, float, i, 0) = CV_MAT_ELEM(*UtU_evects, float, min_loc.x, i);//M的值得到
CvMat* A1 = cvCreateMat(3, 1, CV_32FC1);
CvMat* A2 = cvCreateMat(3, 1, CV_32FC1);
CvMat* A3 = cvCreateMat(3, 1, CV_32FC1);
CvMat* b = cvCreateMat(3, 1, CV_32FC1);
for (int i = 0; i < 3; i++) CV_MAT_ELEM(*A1, float, i, 0) = CV_MAT_ELEM(*M, float, i, 0);
for (int i = 4; i < 7; i++) CV_MAT_ELEM(*A2, float, i - 4, 0) = CV_MAT_ELEM(*M, float, i, 0);
for (int i = 8; i < 11; i++) CV_MAT_ELEM(*A3, float, i - 8, 0) = CV_MAT_ELEM(*M, float, i, 0);
for (int i = 3, j = 0; i < 12; i += 4, j++) CV_MAT_ELEM(*b, float, j, 0) = CV_MAT_ELEM(*M, float, i, 0);
float a3 = cvNorm(A3, NULL, CV_L2, NULL);
float Rho = -1 / a3;

CvMat* r1 = cvCreateMat(3, 1, CV_32FC1);
CvMat* r2 = cvCreateMat(3, 1, CV_32FC1);
CvMat* r3 = cvCreateMat(3, 1, CV_32FC1);
for (int i = 0; i < 3; i++)	CV_MAT_ELEM(*r3, float, i, 0) = Rho*CV_MAT_ELEM(*A3, float, i, 0);
float u0 = pow(Rho, 2)*(cvDotProduct(A1, A3));
float v0 = pow(Rho, 2)*(cvDotProduct(A2, A3));

CvMat* Cross_a1_a3 = cvCreateMat(3, 1, CV_32FC1);
CvMat* Cross_a2_a3 = cvCreateMat(3, 1, CV_32FC1);
cvCrossProduct(A1, A3, Cross_a1_a3);
cvCrossProduct(A2, A3, Cross_a2_a3);

float cos_theta = -((cvDotProduct(Cross_a1_a3, Cross_a2_a3)) / (cvNorm(Cross_a1_a3, NULL, CV_L2, NULL)*cvNorm(Cross_a2_a3, NULL, CV_L2, NULL)));
float theta = acos(cos_theta);
float alpha = pow(Rho, 2)*cvNorm(Cross_a1_a3, NULL, CV_L2, NULL)*sin(theta);
float beta = pow(Rho, 2)*cvNorm(Cross_a2_a3, NULL, CV_L2, NULL)*sin(theta);

for (int i = 0; i < 3; i++)CV_MAT_ELEM(*r1, float, i, 0) = (1 / (cvNorm(Cross_a2_a3, NULL, CV_L2, NULL)))*CV_MAT_ELEM(*Cross_a2_a3, float, i, 0);
cvCrossProduct(r3, r1, r2);
float K_element[9] = { alpha,-alpha*cos_theta,u0,0,beta / sin(theta),v0,0,0,1 };
CvMat* K = cvCreateMat(3, 3, CV_32FC1);

for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++) CV_MAT_ELEM(*K, float, i, j) = K_element[i * 3 + j];

CvMat* t = cvCreateMat(3, 1, CV_32FC1);
CvMat* K_invert = cvCreateMat(3, 3, CV_32FC1);

cvInvert(K, K_invert, CV_LU);
cvGEMM(K_invert, b, 1, NULL, NULL, t, 0);
for (int i = 0; i < 3; i++) CV_MAT_ELEM(*t, float, i, 0) = Rho*CV_MAT_ELEM(*t, float, i, 0);
//显示内参、外参
printf("Intrinsic Parameters:\n");
printf("alpha =%15.8f\n", alpha);
printf("beta =%15.8f\n", beta);
printf("theta =%15.8f\n", theta*57.29577951);
printf("u0 =%15.8f\n", u0);
printf("v0 =%15.8f\n", v0);
printf("Extrinsic Parameters:\n");
printf("r1 = [%15.8f %15.8f %15.8f]'\n", CV_MAT_ELEM(*r1, float, 0, 0), CV_MAT_ELEM(*r1, float, 1, 0), CV_MAT_ELEM(*r1, float, 2, 0));
printf("r2 = [%15.8f %15.8f %15.8f]'\n", CV_MAT_ELEM(*r2, float, 0, 0), CV_MAT_ELEM(*r2, float, 1, 0), CV_MAT_ELEM(*r2, float, 2, 0));
printf("r3 = [%15.8f %15.8f %15.8f]'\n", CV_MAT_ELEM(*r3, float, 0, 0), CV_MAT_ELEM(*r3, float, 1, 0), CV_MAT_ELEM(*r3, float, 2, 0));
printf("t = [%15.8f %15.8f %15.8f]'\n", CV_MAT_ELEM(*t, float, 0, 0), CV_MAT_ELEM(*t, float, 1, 0), CV_MAT_ELEM(*t, float, 2, 0));
cvWaitKey(0);
return 0;
}

还有一个常用的（有Bug，堆溢出）

#include < stdio.h>
#include < opencv2\opencv.hpp>
#include < opencv2\features2d\features2d.hpp>
#include < opencv2\nonfree\nonfree.hpp>
#include < opencv2\nonfree\features2d.hpp>

#include < vector>

 

using namespace std;
using namespace cv;

struct cornerInformation {
float x;
float y;
float x3;
float y3;
float z3;
};

void fprintMatrix(Mat matrix, string name);
void fprintfVectorMat(vector< Mat> matrix, string name);
void fprintf2Point(vector< vector< Point2f> > Points, string name);
void fprintf3Point(vector< vector< Point3f> > Points, string name);

void main()
{
//////////////////////////////////////////////////////////////////////////////////////////////////
//Set input params..
int board_w, board_h;
int n_boards;
float measure = 25;
Size imageSize;

vector< vector< Point2f> > imagePoints;
vector< vector< Point3f> > objectPoints;

board_w = 6;
board_h = 9;
n_boards = 2;

printf("w=%d h=%d n=%d %lfmm\n", board_w, board_h, n_boards, measure);
//////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////
//image load
//extraction image point and object point
char str[100];
for (int i = 0; i< n_boards; ++i)
{
//image load
sprintf(str, "%d.jpg", i + 1);
printf("%s\n", str);
Mat img = imread(str);
imageSize = Size(img.cols, img.rows);
Mat gray;
cvtColor(img, gray, CV_RGB2GRAY);
vector< Point2f> corners;

//find chessboard corners
bool sCorner = findChessboardCorners(gray, Size(board_w, board_h), corners);

//if find corner success, then
if (sCorner)
{
//corner point refine
cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
//draw corner
drawChessboardCorners(img, Size(board_w, board_h), corners, sCorner);
if (corners.size() == board_w*board_h)
{
vector< Point2f> v_tImgPT;
vector< Point3f> v_tObjPT;
//save 2d coordenate and world coordinate
for (int j = 0; j< corners.size(); ++j)
{
Point2f tImgPT;
Point3f tObjPT;

tImgPT.x = corners[j].x;
tImgPT.y = corners[j].y;

tObjPT.x = j%board_w*measure;
tObjPT.y = j / board_w*measure;
tObjPT.z = 0;

v_tImgPT.push_back(tImgPT);
v_tObjPT.push_back(tObjPT);
}
imagePoints.push_back(v_tImgPT);
objectPoints.push_back(v_tObjPT);
}
}
sprintf(str, "Detected%d.jpg", i + 1);
imwrite(str, img);
printf("cacsc");
//imshow("pattern", img);
//cvWaitKey(0);
//cvDestroyAllWindows();
}
//////////////////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////////////////
//claibration part
vector< Mat> rvecs, tvecs;
Mat intrinsic_Matrix(3, 3, CV_64F);
Mat distortion_coeffs(8, 1, CV_64F);

calibrateCamera(objectPoints, imagePoints, imageSize, intrinsic_Matrix, distortion_coeffs, rvecs, tvecs);


for (int i = 0; i< distortion_coeffs.rows; ++i)
{
for (int j = 0; j< distortion_coeffs.cols; ++j)
{
printf("%lf ", distortion_coeffs.at< double>(i, j)); //cvmGet(matrix,i,j));
}
printf("\n");
}
printf("\n");
//////////////////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////////////////
//save part
fprintMatrix(intrinsic_Matrix, "intrinsic.txt");
fprintMatrix(distortion_coeffs, "distortion_coeffs.txt");

fprintfVectorMat(rvecs, "rotation.txt");
fprintfVectorMat(tvecs, "translation.txt");

fprintf3Point(objectPoints, "objectpt.txt");
fprintf2Point(imagePoints, "imagept.txt");

FILE* fp = fopen("ptSize.txt", "w");
fprintf(fp, "%d %d\n", board_w, board_h);
fclose(fp);
//////////////////////////////////////////////////////////////////////////////////////////////////
}

void fprintf3Point(vector< vector< Point3f> > Points, string name)
{
FILE * fp;
fp = fopen(name.c_str(), "w");
for (int i = 0; i< Points.size(); ++i)
{
for (int j = 0; j< Points[i].size(); ++j)
{
fprintf(fp, "%lf %lf %lf\n", Points[i][j].x, Points[i][j].y, Points[i][j].z);
}
fprintf(fp, "\n");
}
fclose(fp);
}


void fprintf2Point(vector< vector< Point2f> > Points, string name)
{
FILE * fp;
fp = fopen(name.c_str(), "w");
for (int i = 0; i< Points.size(); ++i)
{
for (int j = 0; j< Points[i].size(); ++j)
{
fprintf(fp, "%lf %lf\n", Points[i][j].x, Points[i][j].y);
}
fprintf(fp, "\n");
}
fclose(fp);
}


void fprintfVectorMat(vector< Mat> matrix, string name)
{
FILE * fp;
fp = fopen(name.c_str(), "w");
int i, j;
printf("%s size %d, %d\n", name.c_str(), matrix.size(), matrix[0].cols, matrix[0].rows);
for (i = 0; i< matrix.size(); ++i)
{
for (int j = 0; j< matrix[i].rows; ++j)
{
for (int k = 0; k< matrix[i].cols; ++k)
{
fprintf(fp, "%lf ", matrix[i].at< double >(j, k));
}
fprintf(fp, "\n");
}
fprintf(fp, "\n");
}


fclose(fp);
}

void fprintMatrix(Mat matrix, string name)
{
FILE * fp;
fp = fopen(name.c_str(), "w");
int i, j;
printf("%s size %d %d\n", name.c_str(), matrix.cols, matrix.rows);
for (i = 0; i< matrix.rows; ++i)
{
for (j = 0; j< matrix.cols; ++j)
{
fprintf(fp, "%lf ", matrix.at< double >(i, j));
}
fprintf(fp, "\n");
}

fclose(fp);
}