Range finder with a Laser and a Webcam in Visual Basic 6.0

The author of this ingenious project is a scientist, namely Dr. Todd Danko. I found Dr. Danko to be connected with General Electric, Lockheed Martin and DARPA. Nice to have such people in the VB6 community. Like every VB6 programmer, it's fluent in C ++, Java and ASM. From here I will let his text explain the project. 

Introduction

There are many off the shelf range finding components available including ultrasonic, infrared, and even laser rangefinders. All of these devices work well, but in the field of aerial robotics, weight is a primary concern. It is desirable to get as much functionality out of each component that is added to an air-frame. Miniature robotic rotor craft for example can carry about 100g of payload. It is possible to perform machine vision tasks such as obstacle identification and avoidance though the use of a webcam (or mini wireless camera interfaced to a computer via USB adapter). Better yet, two webcams can provide stereo machine vision thus improving obstacle avoidance because depth can be determined. The drawback of this of course is the addition of the weight of a second camera. This page describes how a mini laser pointer can be configured along with a single camera to provide mono-machine vision with range information.

Download from ME

Download from Dr. Danko

Theory of Operation

The diagram below shows how projecting a laser dot onto a target that is in the field of view of a camera, the distance to that target may be calculated. The math is very simple, so this technique works very well for machine vision applications that need to run quickly.

So, here is how it works. A laser-beam is projected onto an object in the field of view of a camera. This laser beam is ideally 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 run over the image looking for the brightest pixels. Assuming that the laser is the brightest area of the scene (which seems to be true for my dollar store laser pointer indoors), the dots position in the image frame is known. Then 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 center of the image, the farther away the object is.

As we can see from the diagram earlier in this section, distance (D) may be calculated:

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

Put the two above equations together, we get:

OK, so the number of pixels from the center of the focal plane that the laser dot appears can just be counted from the image. What about the other parameters in this equation? We need to perform a calibration to derive these.

To calibrate the system, we will collect a series of measurements where I know the range to the target, as well as the number of pixels the dot is from the center of the image each time. This data is below:

Calibration Data

pixels from center	actual D (cm)
103	29
81	45
65	58
55	71
49	90
45	109
41	127
39	159
37	189
35	218

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

Now that we have a Theta_actual for each value, we can come up with a relationship that lets us calculate theta from the number of pixels from image center. I used a linear relationship (thus a gain and offset are needed). This seems to work well even though it does not account for the fact that the focal plane is a plane rather than curved at a constant radius around the center of the lens.

From my calibration data, I calculated:

Offset (ro) = -0.056514344 radians

Gain (rpc) = 0.0024259348 radians/pixel

Using:

I solved for calculated distances, as well as error from actual distance from the calibration data:

Actual and Calculated Range Data

pixels from center	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

Components

There are not a lot of parts in my sample range finder. I used a piece of cardboard to hold a laser pointer to a webcam so that the laser pointer points in a direction that is parallel to that of the camera. The parts seen below are laid out on a one inch grid for reference.

My assembled range finder looks like this:

Software

I have written software two ways, one using visual c++ and the other using visual basic. You will probably find that the visual basic version of the software is much easier to follow than the vc++ code, but with everything, there is a tradeoff. The vc++ code can be put together for free (assuming that you have visual studio), while the vb code requires the purchase of a third party software package (also in addition to visual studio).

Visual Basic

The visual basic code that I have written is available as a package named vb_laser_ranger.zip at the bottom of this page. For this code to work, you will need the VideoOCX ActiveX component installed on your computer. The code that describes the functions found in the main form is seen below:

Private Sub exit_Click()
    ' only if running...
    If (Timer1.Enabled) Then
        
        Timer1.Enabled = False  'Stop Timer
        VideoOCX.Stop
        VideoOCX.Close
                
    End If
    
    End
End Sub

Private Sub Start_Click() 'Init VideoOCX Control, allocate memory and start grabbing
         
    If (Not Timer1.Enabled) Then
        Start.Caption = "Stop"
  
        ' Disable internal error messages in VideoOCX
        VideoOCX.SetErrorMessages False
    
        ' Init control
        If (Not VideoOCX.Init) Then
            ' Init failed. Display error message and end sub
            MsgBox VideoOCX.GetLastErrorString, vbOKOnly, "VideoOCX Error"
            End
        Else
            ' Allocate memory for global image handle
            capture_image = VideoOCX.GetColorImageHandle
            ' result_image = VideoOCX_Processed.GetColorImageHandle
    
            Timer1.Enabled = True 'Start capture timer
    
            ' Start Capture mode
            If (Not VideoOCX.Start) Then
                ' Start failed. Display error message and end sub
                MsgBox VideoOCX.GetLastErrorString, vbOKOnly, "VideoOCX Error"
                End
            End If
        End If
    Else
        Start.Caption = "Start"
        Timer1.Enabled = False  'Stop Timer
        VideoOCX.Stop
        VideoOCX.Close
    End If
    
End Sub

Private Sub Timer1_Timer()
    ' Timer for capturing - handles videoOCXTools
    Dim matrix As Variant
    Dim height, width As Integer
    Dim r, c As Integer
    Dim max_r, max_c As Integer
    Dim max_red As Integer
    Dim gain, offset As Variant
    Dim h_cm As Variant
    Dim range As Integer
    Dim pixels_from_center As Integer
    
    ' Calibrated parameter for pixel to distance conversion
    gain = 0.0024259348
    offset = -0.056514344
    h_cm = 5.842
    
    max_red = 0
    
    ' Capture an image
    If (VideoOCX.Capture(capture_image)) Then
        
        ' VideoOCX.Show capture_image
        
        ' Matrix transformation initialization
        matrix = VideoOCX.GetMatrix(capture_image)
        
        height = VideoOCX.GetHeight
        width = VideoOCX.GetWidth
        
        ' Image processing code
        
        ' The laser dot should not be seen above the middle row (with a little pad)
        For r = height / 2 - 20 To height - 1
            
            ' Our physical setup is roughly calibrated to make the laser
            ' dot in the middle columns...dont bother lookng too far away
            For c = width / 2 - 25 To width / 2 + 24
        
                ' Look for the largest red pixel value in the scene (red laser)
                If (matrix(c, r, 2) > max_red) Then
                    max_red = matrix(c, r, 2)
                    max_r = r
                    max_c = c
                End If
            Next c
        Next r
        
        ' Calculate the distance for the laser dot from middle of frame
        pixels_from_center = max_r - 120

        ' Calculate range in cm based on calibrated parameters
        range = h_cm / Tan(pixels_from_center * gain + offset)

        ' Print laser dot position row and column to screen
        row_val.Caption = max_r
        col_val.Caption = max_c
        
        ' Print range to laser illuminated object to screen
        range_val.Caption = range
        
        ' Draw a red vertical line to intersect target
        For r = 0 To height - 1
            matrix(max_c, r, 2) = 255
        Next r
        
        ' Draw a red horizontal line to intersect target
        For c = 0 To width - 1
            matrix(c, max_r, 2) = 255
        Next c
        
        VideoOCX.ReleaseMatrixToImageHandle (capture_image)
        
    End If
    
    VideoOCX.Show capture_image

End Sub

Screen shots from this code can be seen below:

Visual C++

void CTripodDlg::doMyImageProcessing(LPBITMAPINFOHEADER lpThisBitmapInfoHeader)
{
 // doMyImageProcessing:  This is where you'd write your own image processing code
 // Task: Read a pixel's grayscale value and process accordingly

 unsigned int W, H;   // Width and Height of current frame [pixels]
 unsigned int    row, col;  // Pixel's row and col positions
 unsigned long   i;   // Dummy variable for row-column vector
 unsigned int max_row;  // Row of the brightest pixel
 unsigned int max_col;  // Column of the brightest pixel
        BYTE  max_val = 0;         // Value of the brightest pixel

 // Values used for calculating range from captured image data
 // these values are only for a specific camera and laser setup
 const double gain = 0.0024259348; // Gain Constant used for converting
      // pixel offset to angle in radians
 const double offset = -0.056514344; // Offset Constant
 const double h_cm = 5.842;  // Distance between center of camera and laser
        double  range;          // Calculated range 
 unsigned int pixels_from_center; // Brightest pixel location from center
      // not bottom of frame
 
 char  str[80];         // To print message
 CDC  *pDC;   // Device context need to print message

        W = lpThisBitmapInfoHeader->biWidth; // biWidth: number of columns
        H = lpThisBitmapInfoHeader->biHeight; // biHeight: number of rows
 
 for (row = 0; row < H; row++) {
  for (col = 0; col < W; col++) {

   // Recall each pixel is composed of 3 bytes
   i = (unsigned long)(row*3*W + 3*col);
   
   // If the current pixel value is greater than any other, 
                        // it is the new max pixel
   if (*(m_destinationBmp + i) >= max_val) 
   {
    max_val = *(m_destinationBmp + i);
    max_row = row;
    max_col = col;
   }
  }
 }
 // After each frame, reset max pixel value to zero
        max_val = 0;

 for (row = 0; row < H; row++) {
  for (col = 0; col < W; col++) {

   i = (unsigned long)(row*3*W + 3*col);
   
   // Draw a white cross-hair over brightest pixel in the output display
   if ((row == max_row) || (col == max_col)) 
    *(m_destinationBmp + i) = 
    *(m_destinationBmp + i + 1) = 
    *(m_destinationBmp + i + 2) = 255; 
  }
 }

 // Calculate distance of brightest pixel from center rather than bottom of frame
        pixels_from_center = 120 - max_row;

 // Calculate range in cm based on bright pixel location, and setup specific constants
 range = h_cm / tan(pixels_from_center * gain + offset);

 // To print message at (row, column) = (75, 580)
 pDC = GetDC(); 

 // Display frame coordinates as well as calculated range
 sprintf(str, "Max Value at x= %u, y= %u, range= %f cm    ",max_col, max_row, range);
 pDC->TextOut(75, 580, str);
 ReleaseDC(pDC);
}

My complete code for this project is available as a package named LaserRange.zip at the bottom of this page.  Note, to run this executable, you will need to have both qcsdk and the qc543 driver installed on your computer.  Sorry, but you are on your own to find both of these. Below are two examples of the webcam based laser range finder in action. Note how it looks like there are two laser dots in the second example. This "stray light" is caused by internal reflections in the camera. The reflected dot loses intensity as it bounces within the camera so it does not interfere with the algorithm that detects the brightest pixel in the image.

Future Work

One specific improvement that can be made to this webcam based laser range finder is to project a horizontal line rather than a dot onto a target. This way, we could calculate the range for each column of the image rather than just one column. Such a setup would be able to be used to locate areas of maximum range as places that a vehicle could steer towards. Likewise, areas of minimum range would be identified as obstacles to be avoided.

Sources:

https://sites.google.com/site/todddanko/home/webcam_laser_ranger

Quick Sort, Selection Sort and Bubble Sort algorithm in VB6

Here we have an application that measures execution times for the three sorting algorithms: Quick Sort, Selection Sort and Bubble Sort.  A visual interface shows what these algorithms do in real time. The implementation is made in algorithm in Visual Basic 6.0 and the source code is shown below:

Download from me

The quicksort algorithm was developed in 1959 by Tony Hoare while in the Soviet Union, as a visiting student at Moscow State University. At that time, Hoare worked in a project on machine translation for the National Physical Laboratory. As a part of the translation process, he needed to sort the words of Russian sentences prior to looking them up in a Russian-English dictionary that was already sorted in alphabetic order on magnetic tape. After recognizing that his first idea, insertion sort, would be slow, he quickly came up with a new idea that was Quicksort. He wrote a program in Mercury Autocode for the partition but couldn't write the program to account for the list of unsorted segments. On return to England, he was asked to write code for Shellsort as part of his new job. Hoare mentioned to his boss that he knew of a faster algorithm and his boss bet sixpence that he didn't. His boss ultimately accepted that he had lost the bet. Later, Hoare learned about ALGOL and its ability to do recursion that enabled him to publish the code in Communications of the Association for Computing Machinery, the premier computer science journal of the time.

Quicksort gained widespread adoption, appearing, for example, in Unix as the default library sort subroutine. Hence, it lent its name to the C standard library subroutine qsort and in the reference implementation of Java.

Robert Sedgewick's Ph.D. thesis in 1975 is considered a milestone in the study of Quicksort where he resolved many open problems related to the analysis of various pivot selection schemes including Samplesort, adaptive partitioning by Van Emden as well as derivation of expected number of comparisons and swaps. Bentley and McIlroy incorporated various improvements for use in programming libraries, including a technique to deal with equal elements and a pivot scheme known as pseudomedian of nine, where a sample of nine elements is divided into groups of three and then the median of the three medians from three groups is chosen. Jon Bentley described another simpler and compact partitioning scheme in his book Programming Pearls that he attributed to Nico Lomuto. Later Bentley wrote that he used Hoare's version for years but never really understood it but Lomuto's version was simple enough to prove correct. Bentley described Quicksort as the "most beautiful code I had ever written" in the same essay. Lomuto's partition scheme was also popularized by the textbook Introduction to Algorithms although it is inferior to Hoare's scheme because it does three times more swaps on average and degrades to O(n2) runtime when all elements are equal.

In 2009, Vladimir Yaroslavskiy proposed the new dual pivot Quicksort implementation. In the Java core library mailing lists, he initiated a discussion claiming his new algorithm to be superior to the runtime library’s sorting method, which was at that time based on the widely used and carefully tuned variant of classic Quicksort by Bentley and McIlroy. Yaroslavskiy’s Quicksort has been chosen as the new default sorting algorithm in Oracle’s Java 7 runtime library after extensive empirical performance tests.

Selection sort is a sorting algorithm, specifically an in-place comparison sort. It has O(n2) time complexity, making it inefficient on large lists, and generally performs worse than the similar insertion sort. Selection sort is noted for its simplicity, and it has performance advantages over more complicated algorithms in certain situations, particularly where auxiliary memory is limited.

The algorithm divides the input list into two parts: the sublist of items already sorted, which is built up from left to right at the front (left) of the list, and the sublist of items remaining to be sorted that occupy the rest of the list. Initially, the sorted sublist is empty and the unsorted sublist is the entire input list. The algorithm proceeds by finding the smallest (or largest, depending on sorting order) element in the unsorted sublist, exchanging (swapping) it with the leftmost unsorted element (putting it in sorted order), and moving the sublist boundaries one element to the right.

Bubble sort, sometimes referred to as sinking sort, is a simple sorting algorithm that repeatedly steps through the list to be sorted, compares each pair of adjacent items and swaps them if they are in the wrong order. The pass through the list is repeated until no swaps are needed, which indicates that the list is sorted. The algorithm, which is a comparison sort, is named for the way smaller or larger elements "bubble" to the top of the list. Although the algorithm is simple, it is too slow and impractical for most problems even when compared to insertion sort. It can be practical if the input is usually in sorted order but may occasionally have some out-of-order elements nearly in position.

Implementation:

Public Declare Sub Sleep Lib "kernel32.dll" (ByVal dwMilliseconds As Long)

Public Sub QuickSort(vArray As Variant, L As Long, R As Long)
  '             Array            , LBound()     , UBound()
  Dim i As Long
  Dim j As Long
  Dim X
  Dim Y

  i = L
  j = R
  X = vArray((L + R) / 2)

  Do While (i <= j)
    DoEvents
    Do While (vArray(i) < X And i < R)
      i = i + 1
    Loop

    Do While (X < vArray(j) And j > L)
      j = j - 1
    Loop

    If (i <= j) Then
      Y = vArray(i)
      vArray(i) = vArray(j)
      vArray(j) = Y
      i = i + 1
      j = j - 1
    End If
  Loop

  If (L < j) Then QuickSort vArray, L, j
  If (i < R) Then QuickSort vArray, i, R
End Sub

Public Sub QuickSort33(vArray As Variant, AccordingTo As Integer, Dimension2Size As Integer, L As Integer, R As Integer)
  '   name of array,   sorting according to which dimension?,   size of second dimension,   lbound(),   ubound()
  Dim a As Integer, i As Integer, j As Integer  ' counters
  Dim X, Y, z   ' temporary values

  i = L
  j = R
  X = vArray((L + R) / 2, AccordingTo)
  Do While (i <= j)
    DoEvents
    Do While (vArray(i, AccordingTo) < X And i < R)
      i = i + 1
    Loop
    Do While (X < vArray(j, AccordingTo) And j > L)
      j = j - 1
    Loop
    If (i <= j) Then
      Y = vArray(i, AccordingTo)
      vArray(i, AccordingTo) = vArray(j, AccordingTo)
      vArray(j, AccordingTo) = Y
      For a = 0 To AccordingTo - 1
        z = vArray(i, a)
        vArray(i, a) = vArray(j, a)
        vArray(j, a) = z
      Next a
      For a = AccordingTo + 1 To Dimension2Size
        z = vArray(i, a)
        vArray(i, a) = vArray(j, a)
        vArray(j, a) = z
      Next a
      i = i + 1
      j = j - 1
    End If
  Loop

  If (L < j) Then QuickSort33 vArray, AccordingTo, Dimension2Size, L, j
  If (i < R) Then QuickSort33 vArray, AccordingTo, Dimension2Size, i, R
End Sub

Public Sub SelectionSort(vArray, L As Integer, R As Integer)
'    name of array,    lbound(),    ubound()
Dim i As Integer
Dim j As Integer
Dim best_value  ' smallest value in array
Dim best_j As Integer
    ' loop from left to right
    For i = L To R - 1
        DoEvents
        ' initialize lowest value
        best_value = vArray(i)
        best_j = i  ' initialize lowest value array location
        For j = i + 1 To R
            ' find the lowest value in the array in this loop
            If vArray(j) < best_value Then
                best_value = vArray(j)
                best_j = j
            End If
        Next j
        ' put the smallest value at the from (left) of the array
        ' and put the value on the left of the array in the smallest
        ' value's previous position
        vArray(best_j) = vArray(i)
        vArray(i) = best_value
    Next i
    
End Sub

Public Sub QuickSortBars(vArray As Variant, L As Integer, R As Integer, Optional SleepTime As Long = 0)
  Dim i As Integer    ' counter
  Dim j As Integer    ' counter
  Dim BarVal1         ' temporary bar value
  Dim BarVal2         ' temporary bar value

  i = L
  j = R
  BarVal1 = vArray((L + R) / 2)

  Do While (i <= j)
    DoEvents
    If SleepTime > 0 Then
      Sleep SleepTime
    End If
    Do While (vArray(i) < BarVal1 And i < R)
      i = i + 1
    Loop

    Do While (BarVal1 < vArray(j) And j > L)
      j = j - 1
    Loop

    If (i <= j) Then
      BarVal2 = vArray(i)
      vArray(i) = vArray(j)
      vArray(j) = BarVal2
      frmMain.Bar(i).Value = vArray(i)
      frmMain.Bar(j).Value = vArray(j)
      i = i + 1
      j = j - 1
    End If
  Loop

  If (L < j) Then QuickSortBars vArray, L, j, SleepTime
  If (i < R) Then QuickSortBars vArray, i, R, SleepTime
End Sub

Public Sub SelectionSortBars(vArray, L As Integer, R As Integer, Optional SleepTime As Long = 0)
  '    name of array,    lbound(),    ubound()
  Dim i As Integer    ' counter
  Dim j As Integer    ' counter
  Dim best_value  ' smallest value in array
  Dim best_j As Integer

  ' loop from left to right
  For i = L To R - 1
    DoEvents
    If SleepTime > 0 Then
      Sleep SleepTime
    End If
    ' initialize lowest value
    best_value = vArray(i)
    best_j = i  ' initialize lowest value array location
    For j = i + 1 To R
      ' find the lowest value in the array in this loop
      If vArray(j) < best_value Then
        best_value = vArray(j)
        best_j = j
      End If
    Next j
    ' put the smallest value at the from (left) of the array
    ' and put the value on the left of the array in the smallest
    ' value's previous position
    vArray(best_j) = vArray(i)
    vArray(i) = best_value
    frmMain.Bar(best_j) = vArray(best_j)
    frmMain.Bar(i) = vArray(i)
  Next i
End Sub

Sources:

https://en.wikipedia.org/wiki/Bubble_sort

https://en.wikipedia.org/wiki/Selection_sort

https://en.wikipedia.org/wiki/Quicksort