本科阶段学习计算机视觉的时候也写过相机检校的程序，里面求解相机变换矩阵的时候使用的就是DLT算法，但这一次的作业只是要求计算投影矩阵（即P矩阵）
讲解DLT算法的原理的帖子已经很多了，推荐下面这个链接：
这里的C++ 代码中用到了一个常用的矩阵运算库Eigen,这个库只需要把头文件放到 include 文件夹中就可以用了，非常方便。
Eigen的参考文档当中已经说得很清楚了，不过如果不想看英文的话，可以参考这个链接：
C++ 开源矩阵 运算工具——Eigen
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;
}
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都没达到这个精度>_<||
只能先这样了。。。