NXT Laser Range Finding Camera Theory

www.pdf24.org    Send article as PDF   
Below is Lego Mindstorms Compatible Laser Unit uses a 650nm 5mW (Red) laser module.  I wanted to create a small compact Laser Unit that fitted in as small a footprint as possible. The Laser Module and other electronics fits easily into a standard LEGO ‘2×4 Brick’, with a ‘2×3 plate’ & Technics Connector attached to the bottom. It would have been possible with some effort to fit all the parts within a ‘2×3 Brick’. I decided on a Fly-lead instead of a standard NXT style Socket to keep the unit compact. The Laser is very bright, despite it’s very small size, so as a range-finder the camera can pick it up from a far. Like a NXT Light Sensor’s Red LED, the Laser can be toggled on and of via Software.

NXT Laser Module
NXT Laser Module

I’m intending to use this Laser Unit in-conjunction with a wireless video camera to aid in distance measurements.  Basically an affordable “Laser Range Finder”. The basic principle of this ranger is to measure distance using triangulation. The laser is used to illuminate the object you would like to measure to and a camera image sensor, or other sensor is used to measure the angle to the spot. The laser and image sensor are mounted at a known distance apart, pointed in the same direction.

NXT Laser Module
NXT Laser Module

The greater the resolution your image sensor (camera) has the better the accuracy. The Camera I have is capable of 720 pixels which should provide decent resolution and accuracy. A HD Camera would be even better! A cheaper camera, like that of LEGO Camera has only 300 pixels in a line, and your resolution will be severely effected. More is better, but all dependant on price.

Camera Image with Laser Dot
Camera Image with Laser Dot

I brought this Hamy C-600, 2.4Ghz Wireless 1/3 Sony CCD Rechargeable Camera to use with my Robots.  She image above was taken with it. It has an amazing Sensitivity: of 0.5 Lux at F1.2, which makes it great for lowlight conditions. As can bee seen from the Video made with just the light coming through a doorway into a large room, its sensitivity is EXCELLENT! More detail in my article: Rechargeable 2.4Ghz Wireless Camera.

Hamy C-600 Wireless Camera
Hamy C-600, 2.4Ghz Wireless Camera with Lego Technics Mount

Theory of Operation

The diagram below shows projecting a laser dot onto a target that is in the field of view of a camera. The distance to that target can be calculated very simple maths. This technique works very well for machine vision applications that need to run quickly.


How it Works.

A laser-beam is projected onto an object in the field of view of a camera. Ideally the laser beam is parallel to the optical axis of the camera. The dot from the laser is captured along with the rest of the scene by the camera. A simple algorithm is then run over the image looking for the brightest pixels. Using the assumption that the laser is the brightest area of the scene (which is usually true for cheap laser pointers indoors). The dots position in the image frame can now be calculated. Next we need to calculate the range to the object based on where along the y axis of the image this laser dot falls. The closer to the centre of the image, the farther away the object is.

As we can see from the diagram above, distance, D may be calculated:

[latex size=4″]D = h/tanTheta[/latex]

To solve this equation, you need to know h as well, which is a constant ,fixed as the distance between your laser pointer and camera, and theta (Ø). Theta is calculated:

[latex size=4″] Theta = pfc times rpc + ro [/latex]


pfc = Number of Pixels from Centre of Focal Plane

rpc = Radians per Pixel Pitch

ro = Radian Offset (Compensate for Alignment Errors)

Combining the above two equations together, we get:

D =[latex size=4″] h over tan(pfc times rpc + ro)[/latex]

The number of pixels from the centre of the focal plane that the laser dot appears on can be counted from the acquired image. The other parameters in this equation are derived from calibration data.

To calibrate the system, we will collect a series of measurements where the range to the target, is known. The number of pixels the dot is from the centre of the image each time is also needed. Example data is set out below:

Calibration Data
Pixels from Centre Actual D (cm)
103 29
81 45
65 58
55 71
49 90
45 109
41 127
39 159
37 189
35 218

Table 1

Using the following equation, we can now calculate the actual angle based on the value of h, as well as actual distance for each data point.

Øactual = arctan ( h / D actual)


Øactual =Actual Angle

Dactual = Actual Distance to Measured Target

Once we have a Theta_actual for each value, we can discover the relationship that lets us calculate theta from the number of pixels from image centre. To use a linear relationship, the gain and offset are required. Though this works well, it does not  take into account that the focal plane is a ‘plane‘, rather than curved at a constant radius around the centre of the camera lens.

From the calibration data, we can calculate both Offset & Gain :

Offset (ro) = -0.056514344 radians

Gain (rpc) = 0.0024259348 radians/pixel


D =[latex size=4″] h over tan(pfc times rpc + ro)[/latex]

We can calculate distances, including a error value from the actual distance using the calibration data above:

Actual and Calculated Range Data
Pixels from centre Calc D (cm) Actual D (cm) % Error
103 29.84 29 2.88
81 41.46 45 -7.87
65 57.55 58 -0.78
55 75.81 71 6.77
49 93.57 90 3.96
45 110.85 109 1.70
41 135.94 127 7.04
39 153.27 159 -3.60
37 175.66 189 -7.06
35 205.70 218 -5.64

Table 2




One possible improvement that can be made to this camera based Laser range finder, is to use a laser that projects a horizontal line rather than a dot onto a target. With this type of Laser, we could calculate the range for each column of  pixels in the image, rather than just one column as done here. This type of set-up could be used to locate areas of maximum range as places that a vehicle could steer towards. Likewise, areas with readings of minimum range would be identified as obstacles to be avoided.

From my initial experiments, the Red Laser’s Dot can be hard to distinguish at distances greater than 2 metres (6′). Also the Dot tends to blend into light backgrounds in well lit situations. I feel that a Green Laser may go along way to solving these two issues.

I’ve purchased a Mindsensor NXTCam v3, which is currently in transits. I feel the NXTCam may be a better alternative to the Hamy C-600 Wireless Camera, with it’s built in blob filtering. The NXTCam‘s Vision Subsystem has real-time image processing capabilities to detect and track up to 8 coloured objects. Hopefully the Laser Dot will be prominent enough for the NXTCam to pick up.

Object Tracking using Camera and Laser

Shivam Kalra has some OpenSource Windows Base software and source code written in C# available, which puts the above theory in to practice. This project is greatly based on his tutorial found here: Computer Vision – Laser Range Finder. Below is a screen-grab of what the camera sees with his software.

Range Finder Software

This project is greatly based on a tutorial found here: Computer Vision – Laser Range Finder

Previous Post

Long Range Target Location Using a Laser Sensor

I began testing my Mindstorms NXT Laser Target Sensor with 40mm P.V.C. Pipe Tubes 50mm Long wrapped with 3M Scotchlite 8910 Retroreflective Silver Fabric. The distance between the Laser Sensor ... Read more

Next Post

Mazes for Lego MindStorms Robots

There has been local interest in the new experimental International RoboCup "Rescue B" challenge introduced by the committee headed by Dr. Damien Kee. This was used as a demonstration event ... Read more

Short URL: http://tinyurl.com/gr7638o

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Do NOT follow this link or you will be banned from the site!