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

Details of the Laser Range finder

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