# Calculating distance to vision target

Hi there,

My team and I have tried to calculate the distance to vision target of this year (2020).
We are using Limelight 2 and in their documentation, we found this formula:

https://docs.limelightvision.io/en/latest/cs_estimating_distance.html#using-a-fixed-angle-camera

We found that this formula is pretty accurate when the camera is located in front of the target. However, when the camera positioned from the sides and there is an angle between the camera and the target - the result isn’t accurate.

We also tried to do a linear regression, but we can’t find any segment that remains the same when rotating the camera, its because of the mounting angle of the camera. In 2019 our camera was vertical to the floor and the height of the target remained the same in different angles with the same distance, and we did the regression on that.

We thought maybe the average of the sides’ length will remain the same because when you change the angle, one side increases and the other decreases.

Do you have any solutions? How did you do your vision?
Thank you.

1 Like

Use the Y center point reported from the limelight to do your distance calculations.

How accurate are you trying to be? We found a simple linear equation to be accurate to around +/- an inch most all of the time.

1 Like

Basic trigonometry (tangent to be specific) relying on the angle of your limelight with the tY variable and the height of the target compared to the height of your limelight can provide an accurate distance as long as your contour filtering is done well and your tY is somewhat accurate (as in very accurate lol).

frc-docs also has a great tutorial on vision processing

When I looked closely at computing distance based on height in the image, what I determined was that there is a missing factor related to the horizontal offset. What I found was:
tan( angle_y ) = delta_height / distance * cos( angle_x )
That is, as you go off center in x (left to right), the distance to the target is a little shorter.

Can anyone else confirm my calcs?

My calculations here will go off the post made by @Jared_Russell here:

From that post,

z = \frac{1}{\sqrt{1 + \tan^2{ty}+\tan^2{tx}}}
y = \frac{\tan{ty}}{\sqrt{1 + \tan^2{ty}+\tan^2{tx}}}
x = \frac{\tan{tx}}{\sqrt{1 + \tan^2{ty}+\tan^2{tx}}}

Because these coordinates are normalized, we need to “denormalize them” using a “known dimension”. Often times, this is the y-coordinate (the height of the target from the camera). We can calculate the scaling factor as follows:

\text{scale_factor} = \frac{\text{target_height} - \text{camera_height}}{y}

Now the actual distance to the target is defined as:

\text{Distance} = \sqrt{x^2 + z^2} \times \text{scale_factor}

For simplicity, I will define the following:

\text{dh} = \text{target_height} - \text{camera_height}
\text{Distance} = \sqrt{\frac{{1 + \tan^2{tx}}}{1 + \tan^2{ty}+\tan^2{tx}}} \times \frac{\text{dh}}{\tan{ty}} \times \sqrt{1 + \tan^2{ty}+\tan^2{tx}}

We will use the following identity:

\tan^2{x} + 1 = \sec^2{x}

Applying that identity,

\text{Distance} = \sec{tx} \times \frac{\text{dh}}{\tan{ty}}
\text{Distance} = \frac{\text{dh}}{\tan{ty} \cos{tx}}

Here, you can see that when tx = 0, the equation simplifies to

\text{Distance} = \frac{\text{dh}}{\tan{ty}}

I’m assuming you meant tan(angle_y) = delta_height / (distance * cos(angle_x)) based on my calculations above.

6 Likes

Wow that is an amazing solution!

I tried to create a linear equation according to the y center of the object. The results were pretty accurate like @Tom_Line said.

Your calculations are very convincing, do you think they are more reliable than regressions?

I am not sure really; I hadn’t discovered the idea of perspective transforms until Jared made that post just shortly before the cancellation of the season.

We had our camera on our turret and we would never shoot until tx = 0 anyway. Therefore, we just ended up using the formula on the Limelight website (which is the same formula, but cos tx evaluates to 1). This yielded distance values which were accurate to about an inch and we were pretty happy with our results then.

I will definitely recommend using the proper formula that I derived above to the students of my team once/if we go back to in-person meetings and ask them to observe the accuracy of the results when tx != 0.

The factors which are not taken into account are things like the accuracy of the alignment of the camera, etc. If you can really get a good set of data for the linear regression, then that may be somewhat better. OTOH, you will need to figure out the correct curve to fit with.

So, I had to look though my notes, and recreate some. I don’t question Prateek_M’s math, but I think it is really important to start with good definitions. In my thinking, the relevant quantities are the normalized coordinates of the measure point in the image. I have started from the 3d reconstruction discussion from OpenCV: OpenCV: Camera Calibration and 3D Reconstruction (about 1/2 way down on the page)

First, here is my coordinates:

So, we can parameterize the vector out to the target with l, which is the distance in the horizontal plane. Then any point on the vector would be:

( x, y, z ) = ( l \sin \phi , l \tan \theta, l \cos \phi )

We know h and we are trying to compute d. To do that, we need \theta. What we have for that are the coordinates measured in the image. We can convert the raw pixel coordinate into “normalized” coordinates (u, v); see the OpenCV discussion, and remember to compute u and v using the camera calibration.

(u, v) are the coordinates along the line when z=1. (Note that, as noted by others, the whole issue lies with the fact that the camera image is a planar projection, so there is some distortion as you move off center.) So, in the projection plane we have:

z = 1 = l \cos \phi
l = 1 / cos \phi

Then plugging that back in for the other coordinates:

( u, v ) = ( \sin \phi / \cos \phi, \tan \theta / \cos \phi ) = ( \tan \phi, \tan \theta / \cos \phi )

So, you can compute the angles:

\phi = \tan^{-1} u
\theta = \tan^{-1}( \cos \phi \times v )

With \theta, you directly get d = h / \tan \theta. (I see you can simplify, but IMO pushing further muddies the understanding.)

This definitely agrees with Prateek_M when \phi = 0. I would need to dig to figure out if we disagree off center, and as I said, it really depends on the definition of your variables.

1 Like

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.