OpenCV: Calculate Angle between camera and object

Question

how can i compute the angle of an object in front of my camera? The resolution of my camera is 1280x1024, the focal length of my lens is 8mm and the pixel size of each pixel on the CMOS is 4.8 micrometer. Surely it must be possible to compute the angle from that. Also i computed the distance of the object to the camera and everything is on one level. So only the X coordinate is interesting, right?

I am using OpenCV and Python for the processing.

My idea was to use the focal length of the lens in combination with the X-Offset of the detected object from the sensor middle, but i do get weird angles from that.

This is the code for the angle estimation:

first the point X coordinate, second the width of the whole sensor (1280 pixels * 4.8um) in mm, third the focal length in mm.

angle = (pointInterpolatedX*6.144)/8

Could anybody give me some help here? Thanks!

Also, i had a look at this topic here, but i can't quite understand it. I have a lot more informations about my camera and also my object can only move in 2 Dimensions and not three. So there might be a clever way of estimating its position on the ground in front of the camera. Does OpenCV have any function i could use for that?

Answer 1

To get any real accuracy you'll need to calibrate the camera. What follow is enough for just a first approximation.

The image below depicts the image (Xi, Yi) and camera (Xc, Yc, Zc) coordinate systems I'll use in this response - they are the ones used by OpenCV. It also shows two image points p1 and p2, which may be the boundaries of the image of your object of interest, and the corresponding rays r1 and r2 projecting them to the camera center.

First, let's convert your focal lens to pixels to simplify the calculations. At 4.8 um dot pitch, the width of your sensor is 4.8 * 1280 um = 6.14 mm. So, in proportion, f_pix : 8 mm = 1280 pix : 6.14 mm, hence f_pix = 1667 pixels. We can now write the simplest possible pinhole camera matrix, which assumes the camera's focal axis is orthogonal to the image, and intersects it at the image's center. In numpy's notation:

K = np.array([[1667, 0, 640], [0, 1667, 512], [0, 0, 1]])

Given this matrix, and any 3D point P = (X,Y,Z) in camera coordinates, the image coordinates (x, y) of its projection onto the image are computed as:

p = K.dot(P)
x, y = p[0]/p[2], p[1]/p[2]

Conversely, given a pair of pixel coordinates (x, y), the 3D ray r back-projecting that pixel into 3D space is given by:

Ki = np.linalg.inv(K)
r = Ki.dot([x, y, 1.0])

This is a "ray" in the sense that all the 3D points R = s * r, obtained by multiplying it for an arbitrary number s, will lie on the same line going through the camera center and pixel (x, y).

Therefore, given your boundary image points p1 = (x1, y1) and p2 = (x2, y2), you can compute as above the rays r1 and r2 back-projecting them into 3D space. The angle between them is easily computed from the dot product formula:

cos_angle = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2))
angle_radians = np.acos(cos_angle)

To reiterate, the above formulae are just a first approximation. A real camera will have some nonlinear lens distortion which you'll have to correct to get accurate results, and will have a focal axis slightly de-centered with respect to the image. All these issues are addressed by calibrating the camera.