直接线性变换法DLT详解（张正友标定法）

相机2D坐标到3D坐标的变换是一个SLAM、三维重建、相机标定的经典问题，这个过程必定要求解相机的内参矩阵、外参矩阵。

虽然这是一个slam最基础的入门问题，似乎只要调一个函数就能完成，但实际上，该问题是一个涉及最优化、矩阵论、代数等多个领域的较复杂数学问题。

参考资料：

PnP算法详解（超详细公式推导） - 简书 (jianshu.com)

立体视觉入门指南（4）：相机标定之DLT直接线性变换 - 知乎 (zhihu.com)

相机标定之张正友标定法数学原理详解（含python源码） - leoking01 - 博客园 (cnblogs.com)

https://blog.csdn.net/weixin_43562948/article/details/105758043

相机坐标系到世界坐标系的转换如下所示（如果看不懂，建议补充一些基本知识）：

将内参矩阵和外参合并为单应矩阵，注意旋转矩阵R是正交矩阵 （这在张正友标定法中，还原外参参数有很好的运用）：

对于张正友标定法，有Z=0的情况，也就是平面的情况：

这是一个特例，我们暂时只考虑DLT方法，DLT和张正友标定法有一些不同，不同的地方我会说明。

接着对此式进行展开，得到如下方程，在张正友标定法中会使tz=1。

消去Zc，得到如下齐次线性方程：

也就是：

DLT中F有12个参数，而张中F有8个参数，所以张需要4个A这样的矩阵排列成一个更大的A矩阵来求解F，而DLT需要6个。

如果给出了大于4个或6个的情况，需要使用最小二乘法求解，为了保证解不为0，还要加上约束，使其解的2范数为1：

显然，AtA的归一化特征向量就是解，此解应该是特征值最小的特征向量，注意特征值必大于0，读者可以自行证明。在此作一个小提示：

我们可以通过QR分解H来得到一个上三角矩阵以及一个正交矩阵。上三角矩阵就是相机内参矩阵，正交矩阵是带比例的R矩阵。（这里QR分解不属于我们探讨范围，不赘述）

无论是齐次非线性方程，还是最小二乘法，都有多个带比例的解。（最小二乘法可以通过调节约束条件获得不同比例的解）但是求解内外参问题是有唯一解的，也就是说比例是唯一的。

 因此，引出另一个优化问题，上划线H就是我们刚刚求出的R矩阵（以下所有H、h都应该是旋转矩阵R、r，和上面的H单应矩阵不是一个东西，真是尴尬，回过头来才发现全写错了，要改的话就太麻烦了）：

 展开此式：

只考虑与H有关的项，因为H是待求项，其他项都是固定的参数，结合奇异值分解，将问题转变为：

 上式中运用了矩阵迹的性质，所以可以化为最后的形式。

 接着我们设一个M，这个M有可能不是方阵，当使用张正友标定法时，n为2，此时不为方阵。不过这对我们求解H，没有任何影响。

M的各个列向量是正交且标准化的，所以不难得到上式。至于为什么正交且标准，读者不妨自己思考一下。

 使，此时可取到最大值。那么。

解得：

我们观察一下上式，和之前求得的H相比，只不过是中间的对角矩阵被换成了M。其实就是比例的问题，我们把比例调整一下就好了。

 最后，我们求一下位移t，把t的比例调整一下。下面给出了一个平均调整法，大多时候都是准确的，读者不妨思考一下，还可以如何调整。

（大多时候为正）

DLT问题我们已经求解出来了，但是张正友标定法并没有结束。

 还需要求出旋转矩阵中的h3，也就是h1和h2的叉积，之后还要标准化一下。