DIY基于摄像头的激光测距仪

介绍一下工作原理。一束激光被投射到目标物上，并在摄像头上被显示。激光束被认为是理想的平行于摄像头的中心光轴。激光点由摄像头的其余部分所捕获。一个简单的计算就是寻找最亮点。如果设激光点就是这个场景的最亮点（似乎在室内我的激光发射器确实是最亮的），那么这个点的位置在图帧中的位置是确定的。然后我们只需要计算这个点在沿着y轴的距离，就能计算出目标物离摄像头的距离，激光点距离中心越近，离目标物越远。

如同公式所示，距离D是可以被计算出来的。

为了计算这个等式，我们需要知道激光器和摄像头之间的距离h，这是个常数，还有角度，角度可以计算。

其中：

pfc＝从焦平面到中心的像素数量
rpc＝单个像素的弧度
ro＝弧度补偿（弥补对齐错误）


代入上式，我们得到：

这样，从图像中就能将焦平面到激光点像素数计算出来。那其他的常数怎么办呢？我们需要执行一个校准来得到这些数据。

为了校准这个系统，我们需要收集一系列测量的数据，每次测得的目标物的距离和这个激光点离中心点的像素数。数据如下

使用下面的公式，我们能够利用激光器和摄像头之间的距离h和真实距离计算出真实的角度：

 
θactual＝真实角度
Dactual＝真实距离（测量得出）

现在我们有了公式中的每个数值，我们可以利用一个关系式来计算点离中点的像素数。我用了一个线性关系式。这个公式看起来很有用，……
从我的校正数据中，我计算出:
Offset (ro) = -0.056514344 radians
Gain (rpc) = 0.0024259348 radians/pixel
使用:

 
下表是列举了根据上面ro和rpc值计算出的距离值，实际距离值和误差： RoboticFan

所需零部件

我的测距仪没有多少部件。我使用一块硬纸板来固定激光发射器和摄像头。摄像头和激光发射器被平行的布置在一起。

 

我组装的测距仪是这样的

软件

我通过两个方式编写了这个软件，一个是vc++，一个是VB。你能发现VB版本的软件会比VC＋＋的软件更容易一些，但是各有取舍。VC++版本能够自由的加入其他软件中？VB版本需要第三方软件支持（在Visual Studio中）

Visual Basic

vb_laser_ranger.zip

这里可以下载到我的VB版本软件。
要使用上面的程序，你必须要安装VideoOCX ActiveX 控件

主程序如下：

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)RoboticFan
        
    End If
    
    VideoOCX.Show capture_image
End Sub

截屏如下:

 

Visual C++

我的代码是基于Paul Oh教授的教程。

你需要注意，当跟进这个教程的时候，一些必要的文件也许不再正常连接或丢失，他们可以在下面的位置下载。

qcsdk.exe

qc543enu.exe

根据 TRIPOD 的教程的说明，可以在其源程序中插入一段用户自己的图像处理代码，在这里，我插入了下面的代码:

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
RoboticFan
        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);
}

完整的代码可以在下面下载到：
LaserRange.zip

可执行文件可在下面下载到：
LaserRange.exe

注意，为了执行这个文件，你可能需要qcsdk和qc543这两个驱动文件。

下面是摄像头激光测距仪的工作截图，注意它是如何工作的。在第二个例子中，有两个激光点，其中的一个是激光点在摄像头里面的反射，这个反射点由于没有那么强烈的，所以不适用于运算法则。

将来的工作

一个重要的改进就是将点改为线，这样可以计算每个光柱的距离而不是单个的光柱。这样的设置可以使车辆能够探测最大的前进距离，同样的，障碍物的最小距离也可以被探测到。