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

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

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

参考资料：

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

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

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

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

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

对于张正友标定法，有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，读者可以自行证明。在此作一个小提示：

齐次非线性方程有无数个带比例的解，但是求解内外参问题是有唯一解的。同时，求出来的解有一个问题，H矩阵是个正交矩阵，求出来的解能否保证H为正交呢？

这是一个较麻烦的优化问题，如果把正交作为约束条件代入到优化中，是很难解的。

 因此，引出另一个优化问题，上划线H就是我们刚刚求出的H：

 展开此式：

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

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

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

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

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

解得

 最后，我们求一下位移，上划线t是我们之前求的位移：