Developing an Algorithm for Undistorting an Image

Background

Any real world point $\mathit{P}\left(\mathit{X},\mathit{Y},\mathit{Z}\right)$ can be defined with respect to some 3-D world origin.

Relative to a camera lens, this 3-D point can be defined as ${\mathit{p}}_0$, which is obtained by rotating and translating $\mathit{P}$.

$$p_0=(x_0,y_0,z_0)$$ = $$RP+t$$

The 3-D point ${\mathit{p}}_0$ is then projected into the camera's image plane as a 2D point, (${\mathit{x}}_1$,${\mathit{y}}_1$).

$$x_1=\frac{x_0}{z_0}$$ , $$y_1=\frac{y_0}{z_0}$$

When a camera captures an image, it does not precisely capture the real points, but rather a slightly distorted version of the real points which can be denoted (${\mathit{x}}_2$,${\mathit{y}}_2$). The distorted points can be described using the following function:

where:

${\mathit{k}}_1$, ${\mathit{k}}_2$ = radial distortion coefficients of the lens

${\mathit{p}}_1$, ${\mathit{p}}_2$ = tangential distortion coefficients of the lens

Distortion

An example of lens distortion is shown below; original distorted image (left) and undistorted image (right).

Note the curvature of the lines toward the edges of the first image. For applications such as image reconstruction and tracking, it is important to know the real world location of points. When we have a distorted image, we know the distorted pixel locations (${\mathit{x}}_2$,${\mathit{y}}_2$). It's our goal to determine the undistorted pixel locations (${\mathit{x}}_1$,${\mathit{y}}_1$) given (${\mathit{x}}_2$,${\mathit{y}}_2$) and the distortion coefficients of the particular lens.

While otherwise straightforward, the nonlinear nature of the lens distortion makes the problem challenging.

Define Distortion Model

We begin by defining our distortion model:

% Parameters
syms k_1 k_2 p_1 p_2 real
syms r x y
distortionX = subs(x * (1 + k_1 * r^2 + k_2 * r^4) + 2 * p_1 * x * y + p_2 * (r^2 + 2 * x^2), r, sqrt(x^2 + y^2))

distortionX = $$p_2\hspace{0.17em}\left(3\hspace{0.17em}{x}^2+y^2\right)+x\hspace{0.17em}\left(k_2\hspace{0.17em}{\left(x^2+y^2\right)}^2+k_1\hspace{0.17em}\left(x^2+y^2\right)+1\right)+2\hspace{0.17em}{p}_1\hspace{0.17em}x\hspace{0.17em}y$$

distortionY = subs(y * (1 + k_1 * r^2 + k_2 * r^4) + 2 * p_2 * x * y + p_1 * (r^2 + 2 * y^2), r, sqrt(x^2 + y^2))

distortionY = $$p_1\hspace{0.17em}\left(x^2+3\hspace{0.17em}{y}^2\right)+y\hspace{0.17em}\left(k_2\hspace{0.17em}{\left(x^2+y^2\right)}^2+k_1\hspace{0.17em}\left(x^2+y^2\right)+1\right)+2\hspace{0.17em}{p}_2\hspace{0.17em}x\hspace{0.17em}y$$

Radial Distortion ${\mathit{k}}_1=0$

We plot a grid of pixel locations assuming our lens has a radial distortion coefficient ${\mathit{k}}_1=0$. Note that distortion is smallest near the center of the image and largest near the edges.

% Set Parameters
parameters = [k_1 k_2 p_1 p_2];
parameterValues = [0 0 0 0];
plotLensDistortion(distortionX,distortionY,parameters,parameterValues)

spacing = 
0.2000

distortionX = $$x$$

distortionY = $$y$$

Radial Distortion ${\mathit{k}}_1=0.15$

Explore the sensitivity to changes in ${\mathit{k}}_1$.

% Set Parameters
parameters = [k_1 k_2 p_1 p_2];
parameterValues = [0.15 0 0 0];
plotLensDistortion(distortionX,distortionY,parameters,parameterValues)

spacing = 
0.2000

distortionX = 
$$x\hspace{0.17em}\left(\frac{3\hspace{0.17em}{x}^2}{20}+\frac{3\hspace{0.17em}{y}^2}{20}+1\right)$$

distortionY = 
$$y\hspace{0.17em}\left(\frac{3\hspace{0.17em}{x}^2}{20}+\frac{3\hspace{0.17em}{y}^2}{20}+1\right)$$

Calculate Inverse Distortion Model

Given a camera's lens distortion coefficients and a set of distorted pixel locations (${\mathit{x}}_2$,${\mathit{y}}_2$), we want to be able to calculate the undistorted pixel locations (${\mathit{x}}_1$,${\mathit{y}}_1$). We will look at the specific case where all distortion coefficients are zero except for ${\mathit{k}}_1$ which equals 0.2.

We begin by defining the distortion coefficients.

syms X Y positive
eq1 = X == distortionX

eq1 = $$X=p_2\hspace{0.17em}\left(3\hspace{0.17em}{x}^2+y^2\right)+x\hspace{0.17em}\left(k_2\hspace{0.17em}{\left(x^2+y^2\right)}^2+k_1\hspace{0.17em}\left(x^2+y^2\right)+1\right)+2\hspace{0.17em}{p}_1\hspace{0.17em}x\hspace{0.17em}y$$

eq2 = Y == distortionY

eq2 = $$Y=p_1\hspace{0.17em}\left(x^2+3\hspace{0.17em}{y}^2\right)+y\hspace{0.17em}\left(k_2\hspace{0.17em}{\left(x^2+y^2\right)}^2+k_1\hspace{0.17em}\left(x^2+y^2\right)+1\right)+2\hspace{0.17em}{p}_2\hspace{0.17em}x\hspace{0.17em}y$$

We define the distortion equations for given distortion coefficients, and solve for the undistorted pixel locations (${\mathit{x}}_1$,${\mathit{y}}_1$).

parameters = [k_1 k_2 p_1 p_2];
parameterValues = [0.2 0 0 0];
eq1 = expand(subs(eq1, parameters, parameterValues))

eq1 = 
$$X=\frac{x^3}{5}+\frac{x\hspace{0.17em}{y}^2}{5}+x$$

eq2 = expand(subs(eq2, parameters, parameterValues))

eq2 = 
$$Y=\frac{x^2\hspace{0.17em}y}{5}+\frac{y^3}{5}+y$$

Result = solve([eq1, eq2], [x,y], 'MaxDegree', 3,'Real',true)

Result = struct with fields:
    x: (X*(((5*Y^3)/(2*(X^2 + Y^2)) + ((25*Y^6)/(4*(X^2 + Y^2)^2) + (125*Y^6)/(27*(X^2 + Y^2)^3))^(1/2))^(1/3) - (5*Y^2)/(3*(X^2 + Y^2)*((5*Y^3)/(2*X^2 + 2*Y^2) + ((25*Y^6)/(4*(X^2 + Y^2)^2) + (125*Y^6)/(27*(X^2 + Y^2)^3))^(1/2))^(1/3))))/Y
    y: ((5*Y^3)/(2*(X^2 + Y^2)) + ((25*Y^6)/(4*(X^2 + Y^2)^2) + (125*Y^6)/(27*(X^2 + Y^2)^3))^(1/2))^(1/3) - (5*Y^2)/(3*(X^2 + Y^2)*((5*Y^3)/(2*X^2 + 2*Y^2) + ((25*Y^6)/(4*(X^2 + Y^2)^2) + (125*Y^6)/(27*(X^2 + Y^2)^3))^(1/2))^(1/3))

Since element 1 is the only real solution, we will extract that expression into its own variable.

[Result.x Result.y]

ans = 
$$\begin{array}{l}\left(\begin{array}{cc}\frac{X\hspace{0.17em}{\sigma }_1}{Y}& {\sigma }_1\end{array}\right)\\ \\ \mathrm{where}\\ \\ \mathrm{ }{\sigma }_1={\sigma }_2-\frac{5\hspace{0.17em}{Y}^2}{3\hspace{0.17em}\left(X^2+Y^2\right)\hspace{0.17em}{\sigma }_2}\\ \\ \mathrm{ }{\sigma }_2={\left(\frac{5\hspace{0.17em}{Y}^3}{2\hspace{0.17em}\left(X^2+Y^2\right)}+\sqrt{\frac{25\hspace{0.17em}{Y}^6}{4\hspace{0.17em}{\left(X^2+Y^2\right)}^2}+\frac{125\hspace{0.17em}{Y}^6}{27\hspace{0.17em}{\left(X^2+Y^2\right)}^3}}\right)}^{1/3}\end{array}$$

Now we have analytical expressions for the pixel locations X and Y which we can use to undistort our images.

Function for Drawing the Lens Distortion

function plotLensDistortion(distortionX,distortionY,parameters,parameterValues)
% distortionX is the expression describing the distorted x coordinate
% distortionY is the expression describing the distorted y coordinate
% k1 and k2 are the radial distortion coefficients 
% p1 and p2 are the tangential distortion coefficients 

syms x y

% This is the grid spacing over the image
spacing = 0.2

% Inspect and parametrically substitute in the values for k_1 k_2 p_1 p_2
distortionX = subs(distortionX,parameters,parameterValues)
distortionY = subs(distortionY,parameters,parameterValues)

% Loop over the grid
for x_i = -1:spacing:1
    for y_j = -1:spacing:1
        
        % Compute the distorted location 
        xout = subs(distortionX, {x,y}, {x_i,y_j});
        yout = subs(distortionY, {x,y}, {x_i,y_j});
        
        % Plot the original point
        plot(x_i,y_j, 'o', 'Color', [1.0, 0.0, 0.0])
        hold on
       
        % Draw the distortion direction with Quiver
        p1 = [x_i,y_j];                     % First Point
        p2 = [xout,yout];                   % Second Point
        dp = p2-p1;                         % Difference
        quiver(p1(1),p1(2),dp(1),dp(2),'AutoScale','off','MaxHeadSize',1,'Color',[0 0 1])
        
    end
end
hold off
grid on

end