[备忘，无新意] undistort （求反函数对某个值的映射）使用迭代优化方法

相机内参数优化中构建的模型需要使用到 distort 过程，是将相机归一化平面直角坐标系上的点（即相机坐标系 Z = 1 上的坐标）进行畸变的过程。之后使用相机内参矩阵变换为 uv 坐标。

undistort 过程一般应用中难以用到，所以一直没有注意。distort 过程的逆映射就是 undistort 过程。从数学公式上，这个函数是不可逆的，所以处理起来有些麻烦。

最近关注到这个问题，参考 kalibr 的代码 RadialTangentialDistortion::undistort，突然就看不懂这个代码了。数学基础不够扎实，方向错误，从 Inverse function theorem 处思考。从公式 (\ref{inv_deri}) 着手思考，参考迭代计算平方根的代码，发现如何都不能得到结果。

\[\begin{align} {\text{d} x \over \text{d} y} = {1 \over {\text{d} y \over \text{d} x}} \label{inv_deri} \end{align}\]

于是转换方向。代码中的 (F.transpose() * F).inverse() * F.transpose() * e; 像是在优化，毕竟这个函数最明显的特征是迭代，这就是优化。于是参考以前用于理解优化理论的 MVG 书的 Appendix 6 Iterative Estimation Methods，得到结果。

最优化理论，就是一个通用的反函数方法，不求反函数的 closed-form，而求值域中一点对应的定义域中的点。

现在描述一遍 MVG 数 A6.1 Newton iteration，以为我自己加深印象，备查。

假设有观察值 \(\mathbf{X}\) 是由某些变量的值 \(\mathbf{P}\) 经过映射 \(f\) 得到的，可以写作 \(\mathbf{X} = f(\mathbf{P})\)，要对 \(\mathbf{P}\) 进行估计，估计值为 \(\hat{\mathbf{P}}\)，标准是误差 \(f(\hat{\mathbf{P}}) - \mathbf{X} = \mathbf{\epsilon}\) 的模 \(\|\mathbf{\epsilon}\|\) 最小化。

用迭代的方法做，选取一个初始值 \(\mathbf{P}_0\)，此时的误差 \(\mathbf{\epsilon}_0 = f(\mathbf{P}_0) - \mathbf{X}\) 最小化。接下来我们要在 \(\mathbf{P}_0\) 的附近选取一个新的值 \(\mathbf{P}_1 = \mathbf{P}_0 + \mathbf{\Delta}\)，使得 \(\|\mathbf{\epsilon}_1\|\) 最小化。\(\mathbf{P}_1\) 在 \(\mathbf{P}_0\) 的附近，所以可以用泰勒展开将 \(f(\mathbf{P}_1)\) 与 \(f(\mathbf{P}_0)\) 联系起来，\(f(\mathbf{P}_1) = f(\mathbf{P}_0 + \mathbf{\Delta}) = f(\mathbf{P}_0) + \mathbf{J}\mathbf{\Delta}\)。所以有 \(\mathbf{\epsilon}_1 = f(\mathbf{P}_1) - \mathbf{X} = f(\mathbf{P}_0) + \mathbf{J}\mathbf{\Delta} - \mathbf{X} = \mathbf{\epsilon}_0 + \mathbf{J}\mathbf{\Delta}\)，通过选择 \(\mathbf{\Delta}\) 最小化 \(\|\mathbf{\epsilon}_0 + \mathbf{J}\mathbf{\Delta}\|\)，也就是最小化 \((\mathbf{\epsilon}_0 + \mathbf{J}\mathbf{\Delta})^T(\mathbf{\epsilon}_0 + \mathbf{J}\mathbf{\Delta})\)，接下来就是最小二乘法，通过求导使得导数等于 0，可以解算得到 \(\mathbf{\Delta}\)。每次都使得 \(\mathbf{\epsilon}\) 降低，最终收敛，可以得到 \(\hat{\mathbf{P}}\)。求导参考 矩阵求导的思考。