C++/opencv 直接线性变换 相机标定

头文件

#pragma once
#include <iostream>
#include <math.h>
#include <vector>

#include <opencv2\opencv.hpp>

#include <Eigen/core>
#include <eigen/dense>

using namespace std;
using namespace Eigen;

cpp文件

#include "LinerCalibrate.h"


//计算旋转矩阵
MatrixXd calRotationMatrix(int Ax, int Ay, int Az)
{
	MatrixXd  R(3, 3);
	R(0, 0) = cos(Ay) * cos(Az) - sin(Ay)*sin(Ax)*sin(Az);
	R(0, 1) = cos(Ax)*sin(Az);
	R(0, 2) = sin(Ay)*cos(Az) + cos(Ay)*sin(Ax)*sin(Az);
	R(1, 0) = -cos(Ay)*sin(Az) - sin(Ay)*sin(Ax)*cos(Az);
	R(1, 1) = cos(Ax)*cos(Az);
	R(1, 2) = -sin(Ay)*sin(Az) + cos(Ay)*sin(Ax)*cos(Az);
	R(2, 0) = -sin(Ay)*cos(Ax);
	R(2, 1) = -sin(Ax);
	R(2, 2) = cos(Ay)*cos(Ax);
	return R;
}
//求解线性参数
MatrixXd solve_liner_parameters(MatrixXd &Points3D, MatrixXd &Points2D)
{
	MatrixXd U = Points2D.transpose();
	U.resize(12, 1);
	MatrixXd K(12, 11);
	for (int i = 0, j = 0; i < K.rows(); i++)
	{

		if (i % 2 == 0)
		{
			K(i, 0) = Points3D(j, 0);
			K(i, 1) = Points3D(j, 1);
			K(i, 2) = Points3D(j, 2);
			K(i, 3) = 1;

			K(i, 4) = 0;
			K(i, 5) = 0;
			K(i, 6) = 0;
			K(i, 7) = 0;

			K(i, 8) = (-1) *  Points3D(j, 0) * U(i, 0);
			K(i, 9) = (-1) *  Points3D(j, 1) * U(i, 0);
			K(i, 10) = (-1) *  Points3D(j, 2) * U(i, 0);

		}
		else if (i % 2 == 1)
		{
			K(i, 0) = 0;
			K(i, 1) = 0;
			K(i, 2) = 0;
			K(i, 3) = 0;

			K(i, 4) = Points3D(j, 0);
			K(i, 5) = Points3D(j, 1);
			K(i, 6) = Points3D(j, 2);
			K(i, 7) = 1;

			K(i, 8) = (-1) *  Points3D(j, 0) * U(i, 0);
			K(i, 9) = (-1) *  Points3D(j, 1) * U(i, 0);
			K(i, 10) = (-1) *  Points3D(j, 2) * U(i, 0);
			j++;
		}
	}

	//cout << K << endl;
	MatrixXd K_trans(11, 12);
	K_trans = K.transpose();
	MatrixXd Ktrans_k(11, 11);
	Ktrans_k = K_trans * K;
	MatrixXd Ktrans_KInverse(11, 11);
	Ktrans_KInverse = Ktrans_k.inverse();
	//cout << Ktrans_KInverse << endl;
	MatrixXd S(11, 1);
	S = Ktrans_KInverse * K_trans * U;
	//cout << S << endl;
	return S;
}



//求解相机内外参
void solve_exter_matrix(MatrixXd & S, MatrixXd & InterMatrix, MatrixXd & ExternalMatrix)
{
	MatrixXd S1(1, 3);
	S1 << S(0, 0), S(1, 0), S(2, 0);
	MatrixXd S2(1, 3);
	S2 << S(4, 0), S(5, 0), S(6, 0);
	MatrixXd S3(1, 3);
	S3 << S(8, 0), S(9, 0), S(10, 0);
	double s3 = sqrt(pow(S(8, 0), 2) + pow(S(9, 0), 2) + pow(S(10, 0), 2));
	double m34 = 1.0 / s3;
	cout << "m34的值为" << m34 << endl;
	MatrixXd M(11, 1);
	M = S / m34;
	double u0 = m34 * m34 * (S1(0, 0) * S3(0, 0) + S1(0, 1) * S3(0, 1) + S1(0, 2) * S3(0, 2));
	double v0 = m34 * m34 * (S2(0, 0) * S3(0, 0) + S2(0, 1) * S3(0, 1) + S2(0, 2) * S3(0, 2));
	double fu = m34 * m34 * sqrt(pow((S1(1) * S3(2) - S1(2) * S3(1)), 2)
		+ pow((S1(2) * S3(0) - S1(0) * S3(2)), 2)
		+ pow((S1(0) * S3(1) - S1(1) * S3(0)), 2));

	double fv = m34 * m34 * sqrt(pow((S2(1) * S3(2) - S2(2) * S3(1)), 2)
		+ pow((S2(2) * S3(0) - S2(0) * S3(2)), 2)
		+ pow((S2(0) * S3(1) - S2(1) * S3(0)), 2));

	MatrixXd R1(1, 3);
	R1 = (S1 - u0 * S3) * m34 / fu;
	MatrixXd R2(1, 3);
	R2 = (S2 - v0 * S3) * m34 / fv;
	MatrixXd R3(1, 3);
	R3 = m34 * S3;
	double tz = m34;
	double tx = (S(3, 0) - u0) * m34 / fu;
	double ty = (S(7, 0) - v0) * m34 / fv;
	
	InterMatrix << fu, 0, u0, 0,
		0, fv, v0, 0,
		0, 0, 1, 0;
	cout << "相机的内参矩阵为：" << endl << InterMatrix << endl;
	
	ExternalMatrix << R1(0), R1(1), R1(2), tx,
		R2(0), R2(1), R2(2), ty,
		R3(0), R3(1), R3(2), tz,
		0, 0, 0, 1;
	cout << "相机的外参矩阵为：" << endl << ExternalMatrix << endl;
}

//计算投影矩阵
MatrixXd cal_project_matrix(MatrixXd & InterMatrix, MatrixXd & ExternalMatrix)
{
	MatrixXd ProjectMatrix = InterMatrix * ExternalMatrix;
	cout << "投影矩阵" << ProjectMatrix << endl;
	return ProjectMatrix;
}

//计算理想像点
MatrixXd cal_reproject_points(MatrixXd & Points3D, MatrixXd & ProjectMatrix)
{
	MatrixXd CalPoint_2D(3, 6);
	//计算重投影坐标
	for (int i = 0; i < Points3D.rows(); i++)
	{
		MatrixXd Xw(4, 1);
		Xw << Points3D(i, 0),
			Points3D(i, 1),
			Points3D(i, 2),
			1;
		CalPoint_2D.col(i) = ProjectMatrix * Xw;
	}

	for (int i = 0; i < CalPoint_2D.cols(); i++)
	{
		double zc = CalPoint_2D.col(i)(2);
		CalPoint_2D.col(i) = CalPoint_2D.col(i) / zc;
	}
	MatrixXd IdealPoint_2D = (CalPoint_2D.transpose()).block<6, 2>(0, 0);
	cout << "重投影坐标点为：" << endl << IdealPoint_2D << endl;
	return IdealPoint_2D;
}

//计算理想像点和实际像点偏差
MatrixXd cal_deviation(MatrixXd & Points2D, MatrixXd &IdealPoint_2D)
{
	MatrixXd point_deviation = Points2D - IdealPoint_2D;
	cout << "理想点和实际点之间的偏差为：" << endl << point_deviation << endl;
	return point_deviation;
}


//求解像差系数
void solve_aberration_coefficients(MatrixXd & Points2D, MatrixXd & IdealPoint_2D, MatrixXd  & InterMatrix, MatrixXd & Coefficient, MatrixXd & D)
{
	MatrixXd  Error = Points2D - IdealPoint_2D;
	//cout << "投影偏差：" << endl << Error << endl;
	double u0 = InterMatrix(0, 2);
	double v0 = InterMatrix(1, 2);
	double fu = InterMatrix(0, 0);
	double fv = InterMatrix(1, 1);
	
	for (int i = 0, j = 0; i < 12; i++)
	{
		double xd = (IdealPoint_2D(j, 0) - u0) / fu;
		double yd = (IdealPoint_2D(j, 1) - v0) / fv;
		if (i % 2 == 0)
		{
			D(i, 0) = xd * (xd * xd + yd * yd);
			D(i, 1) = xd * xd + yd * yd;
			D(i, 2) = 0;
			D(i, 3) = xd * xd;
			D(i, 4) = xd * yd;
		}
		else
		{
			D(i, 0) = yd * (xd * xd + yd * yd);
			D(i, 1) = 0;
			D(i, 2) = xd * xd + yd * yd;
			D(i, 3) = xd * yd;
			D(i, 4) = yd * yd;
			j++;
		}
	}
	MatrixXd projectError = Error.transpose();
	projectError.resize(12, 1);
	MatrixXd D_trans(5, 12);
	D_trans = D.transpose();
	MatrixXd Dtrans_D(5, 5);
	Dtrans_D = D_trans * D;
	MatrixXd Dtrans_DInverse(5, 5);
	Dtrans_DInverse = Dtrans_D.inverse();
	Coefficient = Dtrans_DInverse * D_trans * projectError;
	cout << "像差系数：" << endl << Coefficient << endl;	
}

//修正像差
MatrixXd corrected_aberration(MatrixXd & Points2D, MatrixXd & Coefficient, MatrixXd & D)
{
	MatrixXd  calError = D * Coefficient;
	//cout << "计算的像差：" << endl << calError << endl;

	MatrixXd U = Points2D.transpose();
	U.resize(12, 1);
	MatrixXd correctPoint_2D = U - calError;
	correctPoint_2D.resize(2, 6);
	MatrixXd correct_Point_2D = correctPoint_2D.transpose();

	cout << "修正后的像点：" << endl << correct_Point_2D << endl;
	return correct_Point_2D;
}



int main()
{
	//数据仿真
	//定义像机内参
	double fx = 718.856;
	double fy = 718.856;
	double cx = 607.1928;
	double cy = 185.2157;
	MatrixXd intrinsicMatrix(3, 4);
	intrinsicMatrix << fx, 0, cx, 0,
		0, fy, cy, 0,
		0, 0, 1, 0;
	
	//定义平移矩阵	
	MatrixXd TransMatrix(3, 1);
	TransMatrix << 2, 3, 15;
	MatrixXd TransRight(3, 1);
	TransRight << 3, 4, 25;
	//计算旋转矩阵
	MatrixXd RotationMattix = calRotationMatrix(4, 3, 25);
	
	//得到像机外参
	MatrixXd LExternalMatrix(4, 4);
	LExternalMatrix << RotationMattix, TransMatrix,
		0, 0, 0, 1;

	//获取3D点坐标
	MatrixXd Points3D(6, 3);
	Points3D << 15.0805898011000, - 240.498051047000, -106.236122549000,
		30.6387916207000, - 239.755541086000, - 101.758614182000,
		-29.3889399618000, - 235.353872180000, - 103.475697339000,
		-13.9720272273000, - 232.428580523000, - 108.193732798000,
		-40.6951531768000, - 127.180248499000, - 93.5706794262000,
		-13.8775017112000, - 204.104274511000, - 95.9943234921000;
	cout << "空间点坐标" << endl << Points3D << endl;
	//计算投影矩阵
	MatrixXd LProjectMatrix(3, 4);
	LProjectMatrix = cal_project_matrix(intrinsicMatrix, LExternalMatrix);
	//计算对应的图像点坐标
	MatrixXd Points2D(6, 2);
	Points2D = cal_reproject_points(Points3D, LProjectMatrix);
	cout << "图像点坐标" << endl << Points2D << endl;
	//对应图像点坐标
	//MatrixXd Points2D(6, 2);
	//Points2D << 785,	759,
	//			855,	781,
	//			466,	781,
	//			588,	798,
	//			368,	1608,
	//			564,	1017;
	//
	
	
	double max_deviation = 10;
	int i = 0;//迭代次数
	while (max_deviation > 10e-8) //
	{

		//求解线性参数
		MatrixXd S = solve_liner_parameters(Points3D, Points2D);

		//求解相机内外参
		MatrixXd InterMatrix(3, 4);
		MatrixXd ExternalMatrix(4, 4);
		
		solve_exter_matrix(S, InterMatrix, ExternalMatrix);
		//计算投影矩阵
		MatrixXd ProjectMatrix(3, 4);
		ProjectMatrix = cal_project_matrix(InterMatrix, ExternalMatrix);

		//计算理想像点
		MatrixXd IdealPoint_2D(6, 2);
		IdealPoint_2D = cal_reproject_points(Points3D, ProjectMatrix);
		//计算理想像点和实际像点偏差
		MatrixXd point_deviation = cal_deviation(Points2D, IdealPoint_2D);
		max_deviation = point_deviation.maxCoeff();
		//cout << "最大的偏差：" << endl << max_deviation << endl;
		//求解像差系数
		MatrixXd Coefficient(5, 1);
		MatrixXd D(12, 5);
		solve_aberration_coefficients(Points2D, IdealPoint_2D, InterMatrix, Coefficient, D);


		//修正像点
		MatrixXd correct_Point_2D = corrected_aberration(Points2D, Coefficient, D);

		//作为下一步理想像点
		Points2D = correct_Point_2D;

		i++;

	}


	cout << "迭代次数：" << i << endl;
	system("pause");
	return 0;

}