Background

Radial tangential distortion model is the most commonly used model in pinhole camera calibration. Its mathematical representation is shown as formula (1-1). By observing the formula, it is fair to say that the coordinate mapping from undistorted to distorted is natural. But the reverse of the process is difficult because the inverse function is in implicit form.

\[ \left[ \begin{matrix} x_d \\ y_d \\ \end{matrix} \right] = \left[ \begin{matrix} x \\ y \\ \end{matrix} \right]+ \underbrace{ \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} \left[ \begin{matrix} x \\ y \\ \end{matrix} \right] }_{\text{radial}} + \underbrace{ \left[ \begin{matrix} 2xy & r^2 + 2x^2 \\ r^2 + 2 y^2 & 2xy \\ \end{matrix} \right] \left[ \begin{matrix} p_1 \\ p_2 \\ \end{matrix} \right] }_{\text{tangential}} \tag{1-1} \]

Here \(r^2 = x^2 + y^2\)

In engineering application, both directions of the mapping are used. Therefore, the implementation in OpenCV may have some indications. Here is a description that copied from OpenCV specification:

where undistort is an approximate iterative algorithm that estimates the normalized original point coordinates out of the normalized distorted point coordinates ("normalized" means that the coordinates do not depend on the camera matrix) ^[1].

An approximate iterative algorithm proves the difficulty to find the close form of the undistort mapping. The implementation ^[2] in OpenCV seems to call the distortion function several times and finally get the solution of undistort mapping. The workflow looks like a Newton Method but without any derivative calculation.

Mathematical Representation

Given vector-valued function \(y = f(x)\) and vector \(y_0\), find vector \(x_0\) that satisfies \(f(x_0) = y_0\) .

The solution would be quite clear if \(x = f^{-1}(y)\) is an explicit function. However, for the radial tangential model that mentioned above, it is an implicit function. Therefore, an numerical method is required to find the solution.

\[ x_{n+1} = x_{n} + (x_0 - f(x_{n})) \]

Denote \(g(x) = x + x_0 - f(x)\),

\[ x_{n+1} = g(x_n) \]

A Controller's Approach

Recall the the distort function moves a pixel a bit in a specific direction. So the undistort function is to find the original coordinate of a distorted point. The approach used in this blog is to add the error back to the original input and finally find the numerical solution. It sounds like an incremental PID controller.

Notations

\(P_d\): distorted point coordinate
\(P_n\): point coordinate in n-th iteration
\(K_I\): coefficient of the integral feedback
\(f(x)\): distort function
\(e_n\): error in n-th iteration, \(e_n = P_d - f(P_n)\)

Iterative Formula

Fig.1 - Controller Diagram of the Integral Controller

\[ P_{n+1} = P_n + K_I e_n \] \[ P_{n+1} = P_n + K_I(P_d - f(P_n)) \tag{1-2} \]

Set \(K_I = 1.0\) to simplify the problem. here is the C++ implementation in mxm/model_camera.h:

    virtual Matrix undistort(const Matrix& homo_pts) const override
    {
        Matrix ret(homo_pts);
        const size_t iter_max = 5;
        Matrix tmp;
        for(size_t i = 0; i <iter_max; i++)
        {
            tmp = distort(ret);
            ret = (homo_pts - tmp) + ret;
        }
        return ret;
    }

If the controller works, then \( \lim_{n \to \infty} e_n = 0\), i.e. \( \lim_{n \to \infty} P_d = f(P_n) \).

Test Result

The undistort accuracy turned out to be good.

Fig. 2-1: undistorted image

Fig. 2-2: distorted

Fig. 2-3: original image

Fig. 2-4: diff between original image and distorted image

Know-How and Know-Why

We have found out how the iteration method works, but still don't know why. There are several questions left to be answered.

What's the condition for convergence of the iteration method?
What's the convergence rate?
What can we do if the iteration diverged?

With a great amount of searching on the Internet I have found the standard name of the iteration method used by OpenCV is called: Fixed-Point Iteration.

Fixed-point Iteration

In general, an iterative system has the form

\[ u_{n+1} = g(u_n) \]

Where \(g: \mathbb{R}^{n} \rightarrow \mathbb{R}^{n} \) is a vector-valued function.

A fixed point or equilibrium of a discrete dynamical system is a vector \(u^{\star} \in \mathbb{R}^{n}\) such that^[3]

\[ g(u^\star ) = u^ \star \]

Convergence rate of the iteration: \(\rho(g'(x))\). The method is convergent only if \(\rho(g'(x)) < 1\). If \(\rho(g'(x)) > 1\), the procedure diverges. If \(\rho(g'(x)) < 1\) but is close to \(1.0\), convergence is quite slow ^[4]. Here \(\rho(\cdot)\) is the spectral radial of a matrix. \(g'(x)\) is the jacobian matrix of vector-valued function \(g(x)\).

References

OpenCV, undistortPoints
OpenCV, undistort implementation
Peter J. Olver, Numerical Solution of Scalar Equations, Numerical Analysis Lecture Notes
Peter J. Olver, Vector-Valued Iteration, Numerical Analysis Lecture Notes
Joe D. Hoffman, Fixed-Point Iteration Numerical Methods for Engineers and Scientists