Medición de distancias con una cámara web y un puntero láser

Basado en el artículo original de Todd Danko

con título Webcam Based DIY Laser Rangefinder

que a su vez se basa en el tutorial

Traducido con autorización del autor.

Introducción

Existen en el mercado muchos medidores de distancia (o telémetros) en base a ultrasonido, infrarrojo y hasta con sistema láser. Todos estos dispositivos cumplen bien su función, pero en el campo de la robótica aérea, el peso del dispositivo es una cuestión primordial. Cada componente que se agrega a la estructura de un avión debe tener un máximo de utilidad y eficiencia. Una nave aérea robótica miniatura puede llevar apenas 100 gramos de carga útil.

Medidores varios

(Esto es válido, también, para cualquier robot móvil, aunque sea terrestre, porque en estos casos también es importante controlar el peso para evitar un gasto excesivo de energía y así darle autonomía al robot y un mayor tiempo de funcionamiento sin necesidad de una recarga de sus baterías.)

Se pueden realizar tareas de visión robótica, tales como la identificación y sorteo de obstáculos, utilizando una cámara web (webcam) o una mini cámara de video inalámbrica conectada a una computadora a través de USB. Mejor aún, dos webcams pueden proporcionar una visión estéreo que nos dé mejor capacidad de evitar obstáculos, debido a que se puede determinar la profundidad. Esto, por supuesto, tiene la desventaja de agregar el peso de una segunda cámara de imagen.

Este trabajo describe la manera de utilizar un mini puntero láser de bajo costo junto a una única cámara de imagen barata, del tipo webcam, para obtener información de distancia.

La teoría

El diagrama que sigue muestra cómo se puede calcular la distancia hasta un objeto ubicado en el campo visual de una cámara de imagen proyectando un punto láser sobre él. La matemática necesaria para el cálculo es simple, de modo que es posible utilizar esta técnica en aplicaciones de visión robótica que requieren velocidad.

Veamos entonces cómo debemos hacer.

Se proyecta el haz del puntero láser sobre un objeto en el campo visual de una cámara de imagen; lo ideal es que este haz sea paralelo al eje óptico de la cámara. Junto con el resto de la escena tomada por la cámara de imagen, capturamos el punto luminoso del láser. Se aplica un simple algoritmo sobre la imagen, a la búsqueda de los pixeles más brillantes.

Asumiendo que la luz del láser es el área más brillante de la escena (algo que se cumple en las fotografías de más abajo, realizadas en interiores con un puntero láser común y de bajo precio), se puede conocer la posición de este punto luminoso en el cuadro de imagen.

Ahora debemos calcular la distancia hasta el objeto, en base a la posición donde cae el punto brillante respecto al eje "y" de la imagen. Cuanto más cerca se encuentra el punto brillante del centro de la imagen, más lejos está el objeto. Tomando como base el diagrama anterior, se puede calcular la distancia (D):

Por supuesto que para resolver esta ecuación debemos conocer "h", que es una constante definida por la distancia entre el haz del láser y el centro visual de la cámara, y el ángulo theta. Theta se calcula:

Uniendo las ecuaciones, obtenemos:

Muy bien, la cantidad de pixeles desde el centro del plano focal hasta el lugar donde aparece el punto del láser se puede contar trabajando sobre la imagen (simplemente, es una cantidad de líneas de imagen). ¿Y qué pasa con los otros parámetros de la ecuación? Debemos hacer una calibración para obtenerlos.

Para calibrar el sistema, tomaremos una serie de mediciones en las que conozcamos la distancia hasta el blanco, y también tomamos como medición a qué cantidad de pixeles desde el centro de la imagen se encuentra el punto en cada caso. Veamos un ejemplo de recolección de datos:

**Datos de Calibración**
Pixeles desde el centro	D real (cm)
103	29
81	45
65	58
55	71
49	90
45	109
41	127
39	159
37	189
35	218

Usando la ecuación que sigue, podemos calcular el ángulo real en base al valor de h y también la distancia real para cada punto.

Ahora que tenemos un Theta_real para cada valor, podemos lograr una relación que nos permitirá calcular el ángulo theta a partir de la cantidad de pixeles desde el centro de la imagen. Usé una relación lineal, de modo que es necesario aplicar una ganancia y un ajuste de compensación. Esto parece funcionar bien aun cuando no se tiene en cuenta el hecho de que el plano focal es llano en lugar de una curva de radio constante alrededor del centro de la lente.

De mis datos de calibración, calculé:
Desplazamiento (ro) = -0,056514344 radianes

Ganancia (rpc) = 0,0024259348 radianes/pixel

Usando:

Obtuve las distancias calculadas, y además calculé el error contra la distancia real en los datos de calibración:

Datos de distancia calculados y reales
Pixeles desde el centro	D calc (cm)	D real (cm)	% de 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

El hardware

Este medidor de distancias se compone de pocos elementos. Utilicé un trozo de cartón para sostener el láser junto con la webcam, de manera que el puntero láser apunte en dirección paralela al eje de la cámara. Los elementos que se ven en la imagen están colocadas sobre una grilla de 1 pulgada de lado (25,4 mm) para dar una referencia de tamaño.

Este es el aspecto del medidor de distancias tal como quedó luego del montaje.

El software

Escribí el programa de dos maneras, en un caso utilizando Visual C++ y en el otro con Visual Basic. Es problable que la versión de Visual Basic sea mucho más fácil de seguir que el código en Visual C++, pero bueno, hay una compensación: el código en VC++ se puede compilar y utilizar así (asumiendo que usted tiene Visual studio), mientras que el código en VB requiere la compra de un paquete de software aparte (además del Visual Studio).

Visual Basic

El código en Visual Basic que escribí está disponible aquí: vb_laser_ranger.zip

Para que este código funcione se debe tener instalado en la computadora el componente ActiveX VideoOCX.

A continuación se pueden ver listadas las funciones que se encuentran en el formulario principal:

Private Sub exit_Click()
    ' sólo si está corriendo...
    If (Timer1.Enabled) Then
        
        Timer1.Enabled = False  ' Detener el Timer
        VideoOCX.Stop
        VideoOCX.Close
                
    End If
    
    End
End Sub

Private Sub Start_Click() 'Inicia el control VideoOCX, reserva memoria y comienza a tomar imagen
         
    If (Not Timer1.Enabled) Then
        Start.Caption = "Stop"
  
        ' Deshabilita los mensajes de error interno en VideoOCX
        VideoOCX.SetErrorMessages False
    
        ' Inicia el control
        If (Not VideoOCX.Init) Then
            ' Falló el inicio. Muestra mensaje de error y termina
            MsgBox VideoOCX.GetLastErrorString, vbOKOnly, "VideoOCX Error"
            End
        Else
            ' Reserva memoria para manejo global de imagen
            capture_image = VideoOCX.GetColorImageHandle
            ' Imagen resultante = VideoOCX_Processed.GetColorImageHandle
    
            Timer1.Enabled = True ' Inicia temporizador de captura
    
            ' Inicia modo de captura
            If (Not VideoOCX.Start) Then
                ' Falló el inicio. Muestra mensaje de error y termina
                MsgBox VideoOCX.GetLastErrorString, vbOKOnly, "VideoOCX Error"
                End
            End If
        End If
    Else
        Start.Caption = "Start"
        Timer1.Enabled = False  ' Detener el Timer
        VideoOCX.Stop
        VideoOCX.Close
    End If
    
End Sub

Private Sub Timer1_Timer()
    ' Temporizador para captura - maneja 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
    
    ' Parámetros calibrados de pixel para conversión de distancia
    gain = 0.0024259348
    offset = -0.056514344
    h_cm = 5.842
    
    max_red = 0
    
    ' Capture una imagen
    If (VideoOCX.Capture(capture_image)) Then
        
        ' VideoOCX.Show capture_image
        
        ' Inicialización de matriz de transformación
        matrix = VideoOCX.GetMatrix(capture_image)
        
        height = VideoOCX.GetHeight
        width = VideoOCX.GetWidth
        
        ' Código para el proceso de imagen
        
        ' El punto láser estará debajo de la mitad de la imagen
        For r = height / 2 - 20 To height - 1
            
            ' Nuestra configuración física está calibrada para que el punto
            ' del láser caiga, más o menos, en la zona central de la imagen. 
            ' No hay que preocuparse por mirar fuera de esa columna
            For c = width / 2 - 25 To width / 2 + 24
        
                ' Buscar el pixel rojo de valor más grande en la escena (láser rojo)
                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
        
        ' Calcular la distancia del punto láser desde la mitad del cuadro
        pixels_from_center = max_r - 120

' Calcular la distancia en cm en base a los parámetros de calibración
        range = h_cm / Tan(pixels_from_center * gain + offset)

' Mostrar la posición del punto láser y la columna en pantalla
        row_val.Caption = max_r
        col_val.Caption = max_c
        
        ' Mostrar la distancia desde el objeto iluminado
        range_val.Caption = range
        
        ' Dibujar una línea vertical que intersecte el blanco
        For r = 0 To height - 1
            matrix(max_c, r, 2) = 255
        Next r
        
        ' Dibujar una línea horizontal que intersecte el blanco
        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

A continuación se ven imágenes registradas con este programa:

Visual C++

Nota del autor original: para correr este ejecutable, son necesarios los drivers de la webcam, que en el caso del ejemplo (Logitech) son qcsdk y qc543. Lamentamos no disponer de ellos, pero se podrán hallar con una búsqueda en internet. Nosotros encontramos estos, pero no los hemos probado aún. Aquí hay muchos archivos fuente del programa.

void CTripodDlg::doMyImageProcessing(LPBITMAPINFOHEADER lpThisBitmapInfoHeader)
{
 // doMyImageProcessing:  Aquí es donde usted escribe su propio código de proceso de imagen
 // Tarea: Leer la escala de grises de un pixel y procesarla

unsigned int W, H;   // Ancho y alto del cuadro [pixeles]
 unsigned int    row, col;  // Posición de Hilera (row) y Columna (col) del Pixel
 unsigned long   i;   // Variable para el vector row-column
 unsigned int max_row;  // Hilera del pixel más brillante
 unsigned int max_col;  // Columna del pixel más brillante
        BYTE  max_val = 0;         // Valor del pixel más brillante

// Valores utilizados para calcular la distancia a partir de los datos de la imagen capturada
 // Estos valores sólo sirven para una cámara y un láser determinados
 const double gain = 0.0024259348; // Constante de ganacia utilizada para la conversión
      // desplazamiento del pixel al ángulo en radianes
 const double offset = -0.056514344; // Constante de desplazamiento
 const double h_cm = 5.842;  // Distancia entre el centro de la cámara y el láser
        double  range;          // Distancia calculada
 unsigned int pixels_from_center; // Ubicación del pixel más brillante desde el centro
      // no desde la parte inferior del cuadro
 
 char  str[80];         // para mostrar un mensaje
 CDC  *pDC;   // contexto necesario para mostrar un mensaje

W = lpThisBitmapInfoHeader->biWidth; // biWidth: cantidad de columnas
        H = lpThisBitmapInfoHeader->biHeight; // biHeight: catidad de hileras
 
 for (row = 0; row < H; row++) {
  for (col = 0; col < W; col++) {

// Recordar que cada pixel se compone de 3 bytes
   i = (unsigned long)(row*3*W + 3*col);
   
   // Si el valor del pixel actual es mayor que el de los otros, 
   // es el nuevo pixel máximo
   if (*(m_destinationBmp + i) >= max_val) 
   {
    max_val = *(m_destinationBmp + i);
    max_row = row;
    max_col = col;
   }

}
 }
 // Después de cada cuadro, poner valor del pixel máximo en cero
        max_val = 0;

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

i = (unsigned long)(row*3*W + 3*col);
   
   // Dibujar una cruz blanca sobre el pixel más brillante en la pantalla
   if ((row == max_row) || (col == max_col)) 
    *(m_destinationBmp + i) = 
    *(m_destinationBmp + i + 1) = 
    *(m_destinationBmp + i + 2) = 255;

}
 }

// Calcular la distancia del pixel más brillante desde el centro, no desde el pie del cuadro
        pixels_from_center = 120 - max_row;

// Calcular la distancia en cm en base a la ubicación del pixel más brillante, 
 // y definir constantes específicas
 range = h_cm / tan(pixels_from_center * gain + offset);

// para mostrar un mensaje en (row, column) = (75, 580)
 pDC = GetDC();

// Muestra las coordenadas del cuadro y la distancia calculada
 sprintf(str, "Max Value at x= %u, y= %u, range= %f cm    ",max_col, max_row, range);
 pDC->TextOut(75, 580, str);
 ReleaseDC(pDC);
}

Mi código completo para este proyecto se puede bajar de: LaserRange.zip

O si usted quiere probar directamente el ejecutable, bájelo de aquí: LaserRange.exe

Para correr el ejecutable, usted deberá tener ambos drivers, qcsdk y qc543, instalados en su computadora.

A continuación se ven dos imágenes de ejemplo del medidor de distancia funcionando con este programa. Observe que en el segundo ejemplo aparecen dos puntos láser. Esta luz errónea es causada por reflejos internos en la cámara. El punto reflejado pierde intensidad al rebotar dentro de la cámara, de modo que no interfiere con el algoritmo que detecta el pixel de mayor brillo de la imagen. (Nota del traductor: las imágenes han perdido colores en alguna parte del manipuleo, pero de todos modos sirven de ejemplo).

Mejoras posibles

Una mejora concreta que se puede hacer en este medidor de distancia es proyectar sobre el blanco una línea horizontal en lugar de un punto. De esta manera, podemos calcular la distancia en cada columna, en lugar de en una sola. Una disposición así se podría utilizar para localizar las áreas de distancia máxima, que serían los lugares hacia los que se debería dirigir un vehículo. De la misma manera, todas las áreas de distancia mínima serían identificadas como obstáculos que se deben evitar.

Usted puede ver el artículo original en -> Webcam Based DIY Laser Rangefinder