# algorithm Simple Stereo Vision

I'm working in a project implementing a vision system. I'm a student and this is the first time I'm doing something like this, it has been a challenge.

I'm using a controller (Netduino+2, .Net MicroFramework) and a camera (CmuCam5 - Pixy) and for now it's working well. I'm communicating with the robot(Fanuc M430iA) using Modbus, and aquiring the data from the camera using I2C.

But, the next challenge is using 2 cameras to implement stereo vision and I'm not shure how to achieve that. I'm reading a lot about that and I understand the process and generally how it works, but I think my case is very specific.

My cameras detect the center of an object and give me the coordinates, so, I have that, and that's good.

What do you think it's the more reasonable approach?

(sorry for my english, let me know if I'm not being explicit, I'll edit the question if I see there's not enough information)

• I am not sure you can do stereo vision on a microcontroller like Arduino, it may have too small footprint in terms of both memory and computing power. I guess you need to step up to at least something like Raspberry Pi. Also, I am not sure OpenCV can run on Arduino. – Kozuch Apr 16 '15 at 13:01
• (it's a Netduino+2) Yes, you are correct. I was studying the OpenCV approach and I don't think I can adapt to .Net Microframework, so I'm thinkin on building myself the algorithm. Something simple. – Jónatas Brás Apr 16 '15 at 13:55
• another thing. My cameras don's give me entire image frames. they just give me points coordinates – Jónatas Brás Apr 16 '15 at 13:56
• @JónatasBrás what sort of points coordinates, what sort of data are you working with? – Scott Downey Apr 16 '15 at 14:36
• What are the max and min x and y co-ordinates? divide each by the 75*, this gives you degrees per 'pixel'. If needed, transform the co-ordinates it gives you so that the 'origin' is in the center of the plane. This gives you the x-y co-ordinates of the blob from the center. Multiply each by the degrees per pixel which gives you the angle, you now have two angles and a side (the mounting distance) use Law Of Sines - that will give you the distance from each camera to the object, average it to get the distance from the robot (actually the camera mounting mid-point) to the object. – Scott Downey Apr 16 '15 at 15:22

## 3 Answers

To further explain my comment, I've decided to turn it into an answer.

Using the law of sines to calculate distance.

First mount the cameras on the robot parallel like this ---¦--- ---¦---

¦ is the lens. Mount them 100mm apart - measuring from the lenses.

Second find the viewing angle of your camera, in this case 75*.

Third get data from the camera: Cam1 : (50, 50); Cam2 : (10, 50);

Fourth (optional depending on the co-ordinate system used) Normalize the data (transform the co-ordinate system) to center the origin and re-map your co-ordinates.

Fifth Calculate your degrees per division - take your viewing angle and the range of the supplied co-ordinate system and calculate how many degrees are represented per division.

Sixth multiply this Degrees per division by each of your co-ordinate values. To get the degrees from center line to the object for each camera.

Seventh using the Law of Sines you can calculate the distance from each camera to the object using both of the angles you calculated plus the distance between the camera lenses. Optionally average them to get the distance from the camera mounting mid-point to the object.

There are many optimizations that can be applied to this process, especially calibrating the camera lens but that's another question. I would say that errors do creep in and taking multiple readings and averaging the result can improve reliability.

The first thing is to make sure that the cameras will get the coordinates of the object at the same time (I don't know if Pixy has a FREX or STROBE signal for synchronization), or that the object is not moving.

Then, have a look at OpenCV, it has a section on 3D calibration and reconstruction (i.e. find the depth of an object based on the coordinates of two pictures or frames, taken at the same time, from two different (known) angles).

As you start with the 2D coordinates and not the whole image, I guess you will have to dig in and adapt the examples (especially for the calibration part), but it might be a good place to start with

Other links worth checking:

http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration

As @Kuzi said, look into OpenCV for the camera calibration and you'll need to either sync your cameras or make sure things aren't moving. You will probably want to do the camera calibration connecting your sensor to a PC. Once you get the calibration parameters, then you can use them on your microcontroller.

To actually calculate the 3D position of the object, the following steps need to happen:

• Get (x,y) position on real image for each camera
• Convert (x,y) position on real image to (x,y) position on ideal camera. This uses the intrinsics camera calibration to compensate for lens distortion. You will be running the distortion correction in reverse so you will probably need an iterative solution like Gauss-Newton
• Calculate ray through (x,y) position in ideal camera to get ray in camera reference frame. This is just the ray originating at (0,0,0) and going through (x,y,1).
• Transform the ray from camera B into the reference frame for camera A using the extrinsics calibration which is just the transform between the cameras
• Find the intersection of the 2 rays. Typically, the rays won't quite intersect because of noise. That means you will actually be finding the location on each ray where the rays get the closest and then probably averaging them.