Distance through the lens of a fisheye

Say you want a car to know where the curb is while it parks, from its cameras. An ordinary lens, one that keeps straight lines straight, has the field of view of a narrow cone in front of it. It can never see sideways, and as it is made wider, its stretched edges take up more and more of the pixels, leaving few for the view straight ahead. So it would take many ordinary cameras to see the ground all around the car. Cars use fisheye lenses instead, like the peephole in a door. A fisheye sees everything in front of it and a little more, and shares its pixels more evenly. So four cameras are enough, one in each direction.

The price is that a fisheye bends the image. Straight lines like the curb come out curved, and things aren’t where they look to be. Car mirrors make the same trade, which is why some warn that objects in them are closer than they appear.

Let’s explore this with an example in the figure below. Say the front camera sits half a meter above the ground. In its image, a curb 2 meters ahead then looks like it’s only 0.9 meters away. We’ll work out how to get the curb’s real position back, from the pixel’s direction, given by the lens’s calibration (intrinsics), and where the camera sits on the car (extrinsics). I faced a similar task before when working on self-driving trucks, where wide-angle cameras had to give exact positions, not only pictures.

The top row shows the car with ordinary lenses, the middle row with fisheyes, both with the same zoom, the focal length $f$ set by the slider. On the left is what the camera records. On the right is the view from above. Each camera’s image is undistorted and laid out on the ground, and the four are put together, each covering the ground on its own side of the car. The yellow mark is a point, starting on the curb, and how far away it really is. The red cross is where the fisheye distortion makes it look to be. At the bottom, two charts work out step by step which direction the fisheye pixel’s light came from, with the two methods the math covers later.

Ordinary lenses in the top row and fisheyes in the middle row. In a view from above, dragging the point past a dashed line hands it to the camera on that side. The sliders change the lenses' parameters in the equations below, and the charts replay their steps after each change.

The camera model

To know how far away the curb is, we have to undistort the image, that is flatten the bend the lens adds. The first thing to try is a one-liner from OpenCV’s fisheye module, given the camera’s calibration:

cv2.fisheye.undistortPoints(pixels, K=camera_matrix, D=distortion_coeffs)

It does model the fisheye’s bend, but it answers with the points where a straight lens would have put the same rays, on a flat image plane in front of the camera, and that limits the range. The answer can’t hold past 90 degrees from the axis, which is exactly where a fisheye earns its place. A ray from the side runs parallel to that plane and never meets it, and rays from further back point away from it, so OpenCV caps the angle at 90 degrees. So to make it work even for the widest fisheye cameras, let’s get our hands dirty with some math. We need a model of the lens that tells where the light landing on a pixel came from, and to undo it ourselves.

The ray from X reaches the lens at the angle θ from z, from the direction d. A straight lens lets it through unbent, and it lands on the image at u with a radius r from the principal point c. A fisheye bends it toward the axis, so it lands closer in at u_d, at the radius r_d = θ_d defined below. Both points lie at the angle φ from u_x. Pixels count from the corner (0, 0), the picture's top left, since the image is upside down.

Let a ray of light reach the camera at the angle $\theta$ from its optical axis, and turned by the angle $\varphi$ around the axis, counted from the image’s $u_x$ axis, as in the figure above. Its direction

\mathbf{d} = \begin{pmatrix} \sin\theta \cos\varphi \\\ \sin\theta \sin\varphi \\\ \cos\theta \end{pmatrix} \tag{1}

in the camera frame, where $z$ points forward along the optical axis, and $x$ points right and $y$ down as the camera sees it. For a world point $\mathbf{X}$ in the camera frame, $\mathbf{d} = \mathbf{X} / \|\mathbf{X}\|$ . The image has its own coordinates, $\mathbf{u} = (u_x, u_y)$ , counted from the principal point $\mathbf{c}$ . Behind the lens the image is upside down, so $u_x$ and $u_y$ point opposite to $x$ and $y$ . The picture then comes out the right way up, and the ray lands in the image at the same angle $\varphi$ .

The simplest model is the pinhole model, an ideal straight lens where all light passes through one point, the camera centre, without bending. A ray at the angle $\theta$ then lands on the normalized image, at the radius

r = \tan\theta

from the principal point $\mathbf{c}$ , in the direction $\varphi$ . The focal length $f$ , the distance from the camera centre to the image plane, is the camera’s zoom. The further back the image plane, the larger the image. The normalized image divides every distance in the image plane by $f$ , which puts it at the distance 1 behind the lens. That is why $r = \tan\theta$ , and why a ray lands at the same place in it whatever the zoom. Straight lines stay straight. But the radius grows without bound toward 90 degrees, so a straight lens can never see sideways, and it spends its pixels unevenly. A small angle near $\theta$ takes $1/\cos^2(\theta)$ times as many pixels as the same angle at the centre. On the figure’s 640 pixel wide sensor, a straight lens that sees out to 85 degrees from its axis gets half a pixel per degree at the centre and 64 at the edge.

A wide-angle lens has the opposite aim. It maps the angles more evenly onto its pixels, so it can see past 90 degrees while keeping detail in the middle. A fisheye does that by bending each ray toward the axis, more the further out it comes in. The simplest such lens, the equidistant fisheye, makes the radius equal to the angle itself, in radians:

r = \theta

The radius grows with the angle, from the centre all the way past 90 degrees, so a ray at twice the angle lands at twice the radius. Real lenses don’t follow this exactly. Each lens design has its own curve of radius against angle, some fisheyes are designed for other projections, and each lens differs a little from its design due to manufacturing errors. A fisheye model keeps the assumption that the lens is radially symmetric, so the radius depends only on the angle, but leaves open how it depends on it, and corrects the angle with a polynomial. The OpenCV fisheye model puts the ray at the radius $r_d = \theta_d$ , with

\theta_d = \theta \ (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8) \tag{2}

where $k_1, ..., k_4$ are radial distortion coefficients, found by calibrating the camera, and $\theta_d$ is the distorted angle, shown in the graph below. With the default parameters in the interactive figure at the top, a focal length of 220 pixels, $k_1 = -0.25$ and $k_2 = 0.04$ , the fisheye has 3.8 pixels per degree at the centre and still 1.7 at 95 degrees.

Where a ray lands for each lens. The radius r in the normalized image is plotted against the ray's angle θ from the optical axis. The straight lens runs off toward 90 degrees. The figure's fisheye squashes the outer angles. Its curve flattens, so each degree out there takes fewer pixels, and at 95 degrees it is still inside the image. The thin lines follow the curb's ray, at θ = 53 degrees. The fisheye puts it at the radius r_d, where the ideal fisheye, r = θ, puts a ray at 43 degrees. That angle is θ_d.

From a direction to a pixel

The lens model goes the way the light does, from a direction to a point in the image. We need the opposite. The curb shows up at a pixel, and we want the direction its light came from. So we first follow the light all the way to the pixel, and then go back.

The last step on the way there goes from a point in the normalized image to a pixel, counted in columns and rows from the image’s top left corner.

In the normalized image, the fisheye’s ray lands at the radius $r_d = \theta_d$ from the principal point, in the direction $\varphi$ , at the point

\mathbf{u}_d = \theta_d \begin{pmatrix} \cos\varphi \\\ \sin\varphi \end{pmatrix} \tag{3}

The focal lengths scale it to pixels, and the principal point moves it to its place in the image, the pixel

\mathbf{p}_d = \mathbf{f} \odot \mathbf{u}_d + \mathbf{c}

where $\mathbf{f} = (f_x, f_y)$ are the focal lengths in pixels, $\mathbf{c} = (c_x, c_y)$ is the principal point in pixels and $\odot$ is the elementwise product. With a straight lens the ray lands at the pinhole point $\mathbf{u} = \tan(\theta) \, (\cos\varphi, \sin\varphi)$ instead, the pixel $\mathbf{p} = \mathbf{f} \odot \mathbf{u} + \mathbf{c}$ .

From a pixel back to a direction

To undistort, we go in the reverse order. The normalized coordinates of the pixel

\mathbf{u}_d = \frac{\mathbf{p}_d - \mathbf{c}}{\mathbf{f}}

give the distorted angle and the turn directly from equation $(3)$

\Rightarrow \quad \theta_d = \|\mathbf{u}_d\|, \quad \begin{pmatrix} \cos\varphi \\\ \sin\varphi \end{pmatrix} = \frac{\mathbf{u}_d}{\theta_d}

What’s left is to invert the polynomial $(2)$ , which has no closed-form solution. Instead, by letting

\begin{align*} g(\theta) &\equiv \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8) - \theta_d = 0 \\ \Rightarrow g'(\theta) &= 1 + 3k_1\theta^2 + 5k_2\theta^4 + 7k_3\theta^6 + 9k_4\theta^8 \end{align*}

we can use the Newton-Raphson method

\theta_{n+1} = \theta_n - \frac{g(\theta_n)}{g'(\theta_n)} \tag{4}

to solve for $\theta$ numerically, starting from the approximation $\theta_0 = \theta_d$ as a first guess.

Or alternatively, solving equation $(2)$ for the $\theta$ in front of the bracket gives the fixed-point iteration

\theta_{n+1} = \frac{\theta_d}{1 + k_1 \theta_n^2 + k_2 \theta_n^4 + k_3 \theta_n^6 + k_4 \theta_n^8} \tag{5}

also starting from $\theta_0 = \theta_d$ . It needs no derivative, but it converges more slowly, as the charts in the figure show. Near the solution, each step multiplies the remaining error by the iteration’s slope there,

1 - \frac{g'(\theta)}{D(\theta)}

where $D(\theta)$ is the denominator in $(5)$ . So each step removes a fixed fraction of the error, where Newton-Raphson roughly doubles the number of correct digits. The iteration only converges where this slope is less than 1, and it nears 1 close to a fold, where $g'(\theta) = 0$ and a strongly distorting lens maps two angles to the same radius. There the fixed point crawls, while Newton-Raphson’s tangents only get flatter. In the figure, drag $k_1$ toward $-0.4$ to bring the fold close to the curb. Newton-Raphson then takes 7 steps and the fixed point 99.

So we use Newton-Raphson, but keep each step inside an interval $\theta_a < \theta < \theta_b$ known to hold the solution. It starts as $\theta_a = 0$ and $\theta_b = \theta_{\max}$ , the largest angle the lens covers, which is half its field of view from the calibration, or the fold if that comes first. The solution lies in between, since $g(0) = -\theta_d < 0$ , $g(\theta_{\max}) > 0$ for every pixel the lens can produce, and $g$ only grows in between. After each step, $\theta_n$ replaces $\theta_a$ if $g(\theta_n) < 0$ , and $\theta_b$ otherwise. When a Newton step would leave the interval, or $g'(\theta_n)$ isn’t positive, take the interval’s midpoint instead. It then always converges, and as fast as plain Newton-Raphson wherever that one does. For a lens that compresses, like the figure’s, the midpoint is rarely needed.

With $\theta$ and $\varphi$ , equation $(1)$ gives the direction of the ray. Neither method goes through $\tan(\theta)$ , so both work for every angle the lens sees, also past 90 degrees.

This is where OpenCV’s undistortPoints falls short. It solves for $\theta$ with the same Newton-Raphson, but then returns the point in the pinhole image, $\mathbf{u} = \tan(\theta) \, (\cos\varphi, \sin\varphi)$ . A ray at 90 degrees has no point there, and past it $\tan(\theta)$ changes sign. So OpenCV limits the distorted angle to 90 degrees, since the model is only valid up to a 180 degree field of view. Lenses that look around a vehicle often see more than that, and the direction $(1)$ has no such limit.

Down to the ground

For the view from above, the direction is turned into the car’s frame by the camera’s mounting, the rotation $R$ , and followed down to the ground. A camera at the position $\mathbf{t}$ , at the height $t_z$ , sees the ground at the point

\mathbf{X} = \mathbf{t} + \frac{t_z}{-(R\mathbf{d})_z} \ R\mathbf{d} \tag{6}

when the ray points down, $(R\mathbf{d})_z < 0$ . Otherwise it points above the horizon and never meets the ground. The figure gives the distance along the ground from the point right below the camera. The position and the rotation come from calibrating where the camera sits on the car, and the height matters, since for the same direction, every distance on the ground scales with it. Things further away look smaller in the image, but on the ground each point gets its true position, so the view from above shows the ground to scale. This assumes the point lies on flat ground. A point on something that stands up, like another car’s bumper, lands too far away, which is why a car’s view from above smears standing things outward.

Back to the curb

From the pixel that made the curb look 0.9 meters away, we now get its true distance, the 2 meters the scene was laid out with. Scale the pixel to the normalized image, read off $\theta_d$ and $\varphi$ , invert one polynomial with a guarded Newton-Raphson, and follow the direction to the ground. The answer is however only as good as the camera’s calibration, its lens coefficients, mounting and height. That calibration can come from the manufacturer, and finding it is a topic of its own.

The obvious way to undistort a fisheye is to flatten it into a straight lens’s image, but that throws away the very edges the fisheye was bought for. Stop at the direction instead. It has no 90 degree limit, and it is what the rest of the car needs anyway, whether the ray meets the ground, another camera’s ray or a lidar point.