Localization Using Tape Lines

Technical Programming
#1

I’m working on the localization problem using cameras. i.e., given an image and some knowledge of the 3-D world, find the position of the camera in that 3-D world.

Previously, we have used knowledge of a vision target to do this. Given that we know the 3D position of the corners of the vision target, and we have successfully found the rectangle of the vision target in the image, we can call the openCV function solvePNP, and get our position relative to the target.

So far so good, but what if you don’t have the exact points? Specifically, what if you have a line? Like a piece of gaffer’s tape that you happen to know is on the floor. If you can find the corresponding line in the image, how can you use this information to compute your distance from that line? (You can’t get a complete 3-D position without using some other information.)

If you happen to be at right angles to the line, I know how to solve the problem. If the camera axis is horizontal to the floor, and at height h, and the camera is facing perpendicular to the line, it’s fairly straightforward.

let h=height of camera
Y=point in image space, pixels (since you approaching it perpendicularly, the line witll be horizontal)
f=focal length of camera (in pixels)
d=distance to the line (the thing you’re trying to find)

From similar triangles, d=h*f/Y. (Be careful on signs and units, and in this case I’m assuming the center of the image is 0.) Now you know where you are, or at least how far you have to drive to get to the line. (You don’t know horizontal position)

However, suppose you are not approaching the line perpendicularly. In the image, there is now a non-horizontal line. And now my math skills fail me. I can’t work out how to translate the angle of the line in the image to the angle of the line in the 3-D world, with respect to my camera.

I’m sure this problem has been solved many times in the past, and it looks like fairly straightforward trigonometry, but I just haven’t figured it out, and my google skills have so far been inadequate to find a description of the problem on the web. Any help would be appreciated.

I’m sure there is, somewhere, a solution to the more general problem of, given a known iine in 3D space, and a projection of that line onto an image, determine the camera position and orientation with respect to the line. In other words, I’m sure there is a description of the problem where it is not assumed that the camera is horizontal or that the line is on the floor. However, I would be content just to have a solution for the simpler problem described above.

ETA: And for those interested, a thread from last summer discussed the simpler problem of using solvePNP to find your position if you have exact points. I would link to it, but I’m running late for work, so I must run.

#2

I can’t spend the time to help with the math right now (I might edit this post later tonight if I have time then), but I would caution against relying on gaffers tape lines for autonomous routines.

Often with many robots driving over them, the gaffers tape will tear. The field crew should repair the broken lines, but they may replace it in a slightly different line than previously. These discontinuities could cause problems for your algorithms. I know last year in Israel we ran out of white gaffers tape and had to start replacing the white lines with other colors. This may not be as big of a problem in the US where you can buy gaffers tape locally, but it is another thing you might want to consider when deciding if this will actually we worthwhile at competition.

Additionally, gaffers tape is also used to repair the carpet when it tears. Often, teams without the resources to make an accurate carpet-scraper who attempt it anyways will actually dig into the carpet and tear it up. These tears, as well as wear from pushing matches, are repaired by gaffers tape. In a perfect world, the tape they use to repair would be the same color as the carpet in that area, but in reality the color will always be a little bit off, and in the worst cases could be different enough to register with your algorithm (especially if they run out of the correct color).

#3

Hmmm…yes, that could be a problem. Stray bits of tape could send us into some odd places.

We’re planning on using dead reckoning with encoders/gyros, but recognizable landmarks could certainly help verify location. The tape lines seemed an obvious choice, especially to cross the field to the opposite side. I don’t think any algorithm that doesn’t include recognition of field landmarks will be able to score the scale on the opposite side of the field during autonomous.

#4

You could use the LEDs on the switch/scale.

They are in known locations, at a known range of heights, and are a known size. With this info plus an idea of your starting position, you should be able to discern where you are on the localized map.

#5

There might be some variability with this, as the lights may not be the same brightness at all events.

#6

Agreed. One event may be lit with an orange hue to everything while DCMP is a properly lit field, and in another you may have the sun whitewashing everything.

#7

Agreed.

You’d need to tweek the values for the field during practice rounds (if you get any).

#8

I’ve already written some code to find them. It needs tweaking, but we are only in week 4, so it’s ok.

The problem is that they have exactly the same problem, but worse. If approaching them from straight on while they are balanced, I know exactly how to determine the distance to them. If, however, I am approaching them from an angle, that horizontal line will project onto the camera plane as at some angle, just like the tape lines on the floor. And if the scale happens to be already pressed, then it’s worse, because it isn’t even a horizontal line in 3-space.

The frustrating thing is that the problem is just complicated enough that it isn’t found in any “introduction to computer vision” examples, but it’s simple enough that academic publications on the subject assume you know how to do the problem.

I really should be able to figure this out. It’s just trigonometry, but I was never very good at visualizing 3-D geometry.

#9

Project (at least) two points from your detected line onto the ground plane by finding the intersection of the ray from the focal point and the ground plane. There’s your line.

The scale is a more interesting problem. A fixed line in 3D space has 4 degrees of freedom. In the tape line case, your camera is sensing 2 of them, and your constraints on camera height and pitch are giving you the other 2. But a line on the scale has 5 degrees of freedom (due to rotation of the scale). So there is an ambiguity that needs to be resolved (using an additional constraint, ex. estimating distance from line length).

#10

Our team has considered using vision processing to line up with the cable carrier that runs from the platform to the switch to ensure we are perfectly centered as we drive up onto the platform for the deployment of our ramps. While I like this idea a lot, I doubt it will end up being implemented given that we don’t have a sample of the cable carrier or time, of course. More importantly though, there are probably much easier solutions at least for this application, and there are definitely more valuable places for our programmers’ efforts to be placed, even within the domain of vision processing.

I understand the feeling. Give what Jared has recommended a shot for the tape on the floor. Perhaps the disambiguation that Jared was referring to could be accomplished by detecting the width of the light strip on the scale. If you can only see part of the scale, the size of the light contours may be enough information. I don’t know exactly though. Good luck!

#11

Thanks. I’m working out the math now. I found some lecture notes from a professor at Louisville that got me over the conceptual leap. I wasn’t considering the focal point as a point in 3-D space, so I couldn’t do the projection correctly. I think I’m going to get it from here.

Of course, that still leaves the problem of the openCV code, and subsequent processing, to actually find the lines, whether of tape or lights, but one step at a time. Once I work it out I’ll post some code.

#12