Estimating distance from camera to ground plane point

Question

How can I calculate distance from camera to a point on a ground plane from an image?

I have the intrinsic parameters of the camera and the position (height, pitch). Is there any OpenCV function that can estimate that distance?

Answer 1

You can use undistortPoints to compute the rays backprojecting the pixels, but that API is rather hard to use for your purpose. It may be easier to do the calculation "by hand" in your code. Doing it at least once will also help you understand what exactly that API is doing.

Express your "position (height, pitch)" of the camera as a rotation matrix R and a translation vector t, representing the coordinate transform from the origin of the ground plane to the camera. That is, given a point in ground plane coordinates Pg = [Xg, Yg, Zg], its coordinates in camera frame are given by

Pc = R * Pg + t

The camera center is Cc = [0, 0, 0] in camera coordinates. In ground coordinates it is then:

Cg = inv(R) * (-t) = -R' * t

where inv(R) is the inverse of R, R' is its transpose, and the last equality is due to R being an orthogonal matrix.

Let's assume, for simplicity, that the the ground plane is Zg = 0.

Let K be the matrix of intrinsic parameters. Given a pixel q = [u, v], write it in homogeneous image coordinates Q = [u, v, 1]. Its location in camera coordinates is

Qc = Ki * Q

where Ki = inv(K) is the inverse of the intrinsic parameters matrix. The same point in world coordinates is then

Qg = R' * Qc + Cg

All the points Pg = [Xg, Yg, Zg] that belong to the ray from the camera center through that pixel, expressed in ground coordinates, are then on the line

Pg = Cg + lambda * (Qg - Cg)

for lambda going from 0 to positive infinity. This last formula represents three equations in ground XYZ coordinates, and you want to find the values of X, Y, Z and lambda where the ray intersects the ground plane. But that means Zg=0, so you have only 3 unknowns. Solve them (you recover lambda from the 3rd equation, then substitute in the first two), and you get Xg and Yg of the solution to your problem.