【计算机视觉】直接线性变换（DLT）求解P矩阵（附C++和MATLAB代码）

引言

本科阶段学习计算机视觉的时候也写过相机检校的程序，里面求解相机变换矩阵的时候使用的就是DLT算法，但这一次的作业只是要求计算投影矩阵（即P矩阵）

原理

讲解DLT算法的原理的帖子已经很多了，推荐下面这个链接：

双目视觉算法研究（二）相机模型和直接线性法（DLT）

代码

这里的C++ 代码中用到了一个常用的矩阵运算库Eigen,这个库只需要把头文件放到 include 文件夹中就可以用了，非常方便。
Eigen的参考文档当中已经说得很清楚了，不过如果不想看英文的话，可以参考这个链接：
C++ 开源矩阵 运算工具——Eigen

C++代码

int  main()
{
	// 将点位数据保存到矩阵当中,6✖5 的矩阵，前三列为空间坐标，后两列表示图像坐标 
	MatrixXd Point_info_mtx = MatrixXd::Zero(6, 5);

	Point_info_mtx << 99.1517085791261, -0.892099762666786, 3.06159510601795, 163.209290388816, 182.199675950568,
		100.558334460204, 0.868021368458366, 2.25981241694746, 608.892566967667, 701.375883991155,
		100.137647321744, -0.0612187178835884, 2.02380413900248, 449.262241640596, 377.206100107244,
		99.6742452887978, -0.675635383613515, 3.58856908136781, 316.188427147902, 257.277246206713,
		99.6224300840896, 0.0570662710124255, 2.33129745899956, 267.575494465364, 430.054554508315,
		100.203963882803, -0.474057430919711, 3.30815819695356, 439.688462560083, 287.229433675101;

	cout << Point_info_mtx << endl;

	// 调用编写好的函数计算P矩阵
	MatrixXd P = MatrixXd::Zero(3, 4);

	bool flag = Compute_P_matrix(Point_info_mtx, P);

	if (flag == 0) { cout << "程序运行失败！！！" << endl; exit(0); }
	else { cout << "P矩阵解求如下：\n" << P << endl; }

	// 使用计算的P矩阵来进行验证
	MatrixXd Points3d = MatrixXd::Zero(4, 6); //前三行表示坐标值，最后一行为 1 ，表示为增广坐标
	MatrixXd Points2d = MatrixXd::Zero(2, 6);
	for (int i = 0; i < 6; i++)
	{
		Points3d.col(i) << Point_info_mtx(i, 0), Point_info_mtx(i, 1), Point_info_mtx(i, 2), 1;
		Points2d.col(i) << Point_info_mtx(i, 3), Point_info_mtx(i, 4);
	}
	MatrixXd Res2d_ = MatrixXd::Zero(3, 6);		// 保存计算得到的结果，这里得到的是增广后的图像坐标 
	MatrixXd Res2d = MatrixXd::Zero(2, 6);		// 图像坐标

	Res2d_ = P*Points3d;

	// 归一化到图像坐标
	for (int i = 0; i < 6; i++)
	{
		Res2d.col(i) << Res2d_(0, i) / Res2d_(2, i), Res2d_(1, i) / Res2d_(2, i);
	}

	// 将理论值和计算值相减
	MatrixXd d = Points2d - Res2d;

	cout << "Res_:\n" << Res2d_ << endl;
	cout << "Res:\n" << Res2d << endl;
	cout << "d:\n" << d << endl;;

	return 0;
}



bool Compute_P_matrix(MatrixXd &points, MatrixXd &P)
{
	// 要用到的临时变量
	double x, y, X, Y, Z;

	//C * L  = I -- > L = inv(Ct * C) * Ct * (I)
	//C为2N行11列，L为11行1列，I为2N行1列
	MatrixXd C = MatrixXd::Zero(2 * 6, 11);
	MatrixXd L = MatrixXd::Zero(11, 1);
	MatrixXd I = MatrixXd::Zero(2 * 6, 1);

	//给C赋值,points为6行5列，0/1/2 三列为 X/Y/Z, 3/4列为 x/y
	// X Y Z 1 0 0 0 0 -x*X -x*Y -x*Z
	// 0 0 0 0 X Y Z 1 -y*X -y*Y -y*Z
	for (int i = 0; i < 6; i++)
	{
		X = points(i, 0);
		Y = points(i, 1);
		Z = points(i, 2);
		x = points(i, 3);
		y = points(i, 4);
		C.row(2 * i) << X, Y, Z, 1, 0, 0, 0, 0, -x*X, -x*Y, -x*Z;
		C.row(2 * i + 1) << 0, 0, 0, 0, X, Y, Z, 1, -y*X, -y*Y, -y*Z;
		I(2 * i) = x;
		I(2 * i + 1) = y;
	}

	MatrixXd Ct = C.transpose();
	MatrixXd CtC = Ct*C;
	L = CtC.inverse()*Ct*I;

	// 给P矩阵赋值
	P << L(0) / L(10), L(1) / L(10), L(2) / L(10),  L(3) / L(10),
		 L(4) / L(10), L(5) / L(10), L(6) / L(10),  L(7) / L(10),
		 L(8) / L(10), L(9) / L(10), L(10) / L(10), 1 / L(10);

	return true;
}

Matlab代码

clc;clear;close all;
%% DLT 算法需要用到 6 对点 ，前三列是 X Y Z 表示空间坐标 ，后两列 x y 表示图像坐标,下面是测试数据
Points = [  99.1517085791261, -0.892099762666786, 3.06159510601795, 163.209290388816, 182.199675950568;
		100.558334460204, 0.868021368458366, 2.25981241694746, 608.892566967667, 701.375883991155;
		100.137647321744, -0.0612187178835884, 2.02380413900248, 449.262241640596, 377.206100107244;
		99.6742452887978, -0.675635383613515, 3.58856908136781, 316.188427147902, 257.277246206713;
		99.6224300840896, 0.0570662710124255, 2.33129745899956, 267.575494465364, 430.054554508315;
		100.203963882803, -0.474057430919711, 3.30815819695356, 439.688462560083, 287.229433675101];

%% 计算部分 C*L + I = 0
% 赋值 有6 个点，所以C的大小为 12×11,  I 为12×1
C = zeros(12,11);I = zeros(12,1);
for i = 1:6
    X = Points(i,1);Y = Points(i,2);Z = Points(i,3);x = Points(i,4);y = Points(i,5);
    C(i*2-1,:) = [X,Y,Z,1,0,0,0,0,-x*X,-x*Y,-x*Z];
    C(i*2,:)   = [0,0,0,0,X,Y,Z,1,-y*X,-y*Y,-y*Z];
    
    I(i*2-1) = x; I(i*2) = y;
end

L = (C'*C)^(-1) * C' * I ;

P(1:11) = L;P(12) = 1;

P = P/L(11);

P = reshape(P,[4,3])';

%% 验证
Point3d = ones(4,6);
Point3d(1:3,:) = Points(:,1:3)';

Point2d = Points(:,4:5)';

Res = P*Point3d;

Res2d(1,:) = Res(1,:)./Res(3,:);
Res2d(2,:) = Res(2,:)./Res(3,:);

disp(Point2d)
disp(Res2d)

% 计算结果和原本数据的差值，即重投影的误差
diff = Point2d - Res2d;
disp(diff);

补充

到此为止已经计算出了相机的投影矩阵，这里补充部分用于计算相机参数的代码，参考文献为 ：
利用二维 DLT 进行数字摄像机标定

注意，这篇文章中的公式和上面的链接中的公式中某些正负号是不同的，参考的时候需要注意，我当时编写的时候是按照老师PPT中的公式写的（下面的代码和上面的代码不是同一个时期写的 >_^ !!!）

内参数矩阵：

x0 = -(L(0)*L(8) + L(1)*L(9) + L(2)*L(10)) / (pow(L(8), 2) + pow(L(9), 2) + pow(L(10), 2));
y0 = -(L(4)*L(8) + L(5)*L(9) + L(6)*L(10)) / (pow(L(8), 2) + pow(L(9), 2) + pow(L(10), 2));

double fx = sqrt(-pow(x0, 2) + (pow(L(0), 2) + pow(L(1), 2) + pow(L(2), 2)) /
		(pow(L(8), 2) + pow(L(9), 2) + pow(L(10), 2)));
double fy = sqrt(-pow(y0, 2) + (pow(L(4), 2) + pow(L(5), 2) + pow(L(6), 2)) /
		(pow(L(8), 2) + pow(L(9), 2) + pow(L(10), 2)));

运行结果

C++

Matlab

结语

其实我们老师要求的是重投影误差要小于 0.01 ，悲剧的是无论是C++还是MATLAB都没达到这个精度>_<||
只能先这样了。。。

12月19日更新：【计算机视觉】直接线性变换（DLT）求解P矩阵（2 使用SVD分解）（附MATLAB代码）