Deteccion y navegación esquivando objetos

En artículos anteriores como el artículo sobre reconocimiento básico de objetos con OpenCV y el artículo sobre segmentación básica en OpenCV hemos visto cómo realizar una deteccion de objetos de un determinado color en una imagen, e incluso calcular sus puntos medios y distancias entre ellos. En el artículo de hoy vamos a aplicar todos estos conocimientos para realizar un ejemplo práctico en el que gracias a dos imágenes tomadas por la cámara sabremos dónde se encuentran dos bolas rojas respecto al Robot, y lograremos esquivarlas con el robot mientras se mueve en línea recta.

Un poco de trigonometría:

Puesto que vamos a tomar dos fotos, y sabemos:

1) a qué altura está la cámara respecto del suelo, y
2) la inclinación de la cámara en grados,

podemos saber muchos datos, como por ejemplo la distancia hasta la que es capaz de ver la cámara. Teniendo en cuenta la siguiente imagen:

Si la cámara está a una altura b del suelo, mirando hacia abajo con un ángulo α, sabemos que la hipotenusa es:

c = b/cos(α)

y que la distancia a abarcada por la cámara es igual a:

a = c*sen(α)

por tanto la relación entre b y a será:

a = b*sen(α)/cos(α)

Así que con tan solo esos dos datos ya podemos saber muchas cosas. Pero, ¿cómo traducir las distancias que hemos hallado en píxeles a centímetros? Es fácil, ya que si sabemos la altura de la imagen (100 píxeles por ejemplo) y la distancia nos ha dado 50 píxeles, solo tenemos que dividir 50 entre 100, que serán 0.5, y multiplicarlo por a, de forma que nos dará más o menos la distancia equivalente en el mundo real (estamos suponiendo que la imagen abarca todo el plano horizontal cuando no es cierto). En cualquier caso es un método bastante preciso para realizar la deteccion.

Programa de OpenCV:

Para procesar las imágenes usaremos OpenCV, y cuando terminemos guardaremos los datos necesarios en un archivo que moveremos al NXT, y que utilizará nuestro programa en RobotC para esquivar las pelotas. Hemos tomado mediante una cámara situada en el NXT las dos imágenes siguientes:

con la cámara situada a 27 cm del suelo y con una inclinación de 25º, y:

con la cámara situada a 30 cm del suelo (sube un poco al rotarla), y con una inclinación de 50º. De la primera imagen podemos sacar la distancia aproximada del robot a la primera pelota (supondremos que el robot está situado en el píxel central de la última linea de la imagen). De la segunda imagen podemos sacar la distancia entre las dos pelotas. Ya sabemos como calcular distancias y puntos medios de objetos de determinado color en OpenCV, así como distinguir dos objetos del mismo color. Incluso como escribir los datos en un fichero para que RobotC los entienda (revisad el artículo sobre manejo de ficheros en RobotC). El programa completo sería:

[codesyntax lang=”cpp”]
#include
using namespace std;

int main(int argc, char *argv[])
{
IplImage* img = 0;
int altura,anchura,anchura_fila,canales;
uchar *data;
int i,j;

if(argc<3){
printf(“Uso: main <nombre_imagen>n7”);
exit(0);
}

// cargamos la imagen
img=cvLoadImage(argv[1]);
if(!img){
printf(“No se ha podido cargar la imagen: %sn”,argv[1]);
exit(0);
}

// cogemos la información de la imagen
altura = img->height;
anchura = img->width;
anchura_fila = img->widthStep;
canales = img->nChannels;
data = (uchar *)img->imageData;
printf(“Procesando una imagen de %dx%d píxeles con %d canalesn”,
altura, anchura, canales);

// creamos una ventana
cvNamedWindow(“mainWin”, CV_WINDOW_AUTOSIZE);
cvMoveWindow(“mainWin”, 100, 100);

// maximos y minimos

int y_anterior = 0;

int x1_cont = 0;
int y1_cont = 0;

int x1_total = 0;
int y1_total = 0;

int x2_cont = 0;
int y2_cont = 0;

int x2_total = 0;
int y2_total = 0;

bool bola1 = false;
bool bola2 = false;

// recorremos la imagen

for(i=0;i<altura;i++) for(j=0;j<anchura;j++) {

if ((data[i*anchura_fila+j*canales + 2] > 80) &&
!((data[i*anchura_fila+j*canales + 0] > data[i*anchura_fila+j*canales + 2]/2) ||
(data[i*anchura_fila+j*canales + 1] > data[i*anchura_fila+j*canales + 2]/2))){

if(!bola1 && !bola2){
bola1 = true;
y_anterior = i;
}
printf(“Punto rojo en %d, %d, valor: %dn”,j,i,
data[i*anchura_fila+j*canales+2]);
if (y_anterior + 20 < i){
bola1 = false;
bola2 = true;

}
if (bola1){
printf(“Bola1n”);
y1_total = y1_total + i;
x1_total = x1_total + j;
y1_cont++;
x1_cont++;
data[i*anchura_fila+j*canales] = 255;
data[i*anchura_fila+j*canales + 1] = 0;
data[i*anchura_fila+j*canales + 2] = 0;
} else if(bola2){
printf(“Bola2n”);
y2_total = y1_total + i;
x2_total = x1_total + j;
y2_cont++;
x2_cont++;
data[i*anchura_fila+j*canales] = 0;
data[i*anchura_fila+j*canales + 1] = 255;
data[i*anchura_fila+j*canales + 2] = 0;
}
y_anterior = i;
}

}

int x1_medio = x1_total / x1_cont;
int y1_medio = y1_total / y1_cont;

printf(“Punto medio bola1 en %d, %d, valor: %dn”,x1_medio, y1_medio,
data[i*anchura_fila+j*canales+2]);

int x2_medio = x2_total / x2_cont;
int y2_medio = y2_total / y2_cont;

printf(“Punto medio bola2 en %d, %d, valor: %dn”,x2_medio, y2_medio,
data[i*anchura_fila+j*canales+1]);

float distancia = sqrt(pow(x1_medio – x2_medio,2) + pow(y1_medio – y2_medio,2));

// mostramos la imagen
cvShowImage(“mainWin”, img );
// esperamos apretar una tecla
cvWaitKey(0);

// liberar la imagen
cvReleaseImage(&img );

img=cvLoadImage(argv[2]);
if(!img){
printf(“No se ha podido cargar la imagen: %sn”,argv[2]);
exit(0);
}

// cogemos la información de la imagen
altura = img->height;
anchura = img->width;
anchura_fila = img->widthStep;
canales = img->nChannels;
data = (uchar *)img->imageData;
printf(“Procesando una imagen de %dx%d píxeles con %d canalesn”,
altura, anchura, canales);

// creamos una ventana
cvNamedWindow(“mainWin”, CV_WINDOW_AUTOSIZE);
cvMoveWindow(“mainWin”, 100, 100);

// maximos y minimos

int x_cont = 0;
int y_cont = 0;

int x_total = 0;
int y_total = 0;

//recorremos la imagen

for(i=0;i<altura;i++) for(j=0;j<anchura;j++) {

if ((data[i*anchura_fila+j*canales + 2] > 80) &&
!((data[i*anchura_fila+j*canales + 0] > data[i*anchura_fila+j*canales + 2]/2) ||
(data[i*anchura_fila+j*canales + 1] > data[i*anchura_fila+j*canales + 2]/2))){
printf(“Punto rojo en %d, %d, valor: %dn”,j,i,
data[i*anchura_fila+j*canales+2]);
y_total = y_total + i;
x_total = x_total + j;
y_cont++;
x_cont++;
data[i*anchura_fila+j*canales] = 255;
data[i*anchura_fila+j*canales + 1] = 0;
data[i*anchura_fila+j*canales + 2] = 0;
}
}

int x_medio = x_total / x_cont;
int y_medio = y_total / y_cont;

printf(“Punto medio rojo en %d, %d, valor: %dn”,x1_medio, y1_medio,
data[i*anchura_fila+j*canales+2]);

int xr_medio = anchura / 2;
int yr_medio = altura – 1;

printf(“Punto medio robot en %d, %d, valor: %dn”,xr_medio, yr_medio,
data[i*anchura_fila+j*canales+1]);

float distancia2 = sqrt(pow(x_medio – xr_medio,2) + pow(y_medio – yr_medio,2));

printf(“Distancia entre ambas pelotas: %6.2fn”, distancia);
printf(“Distancia del robot a la pelota: %6.2fn”, distancia2);

ofstream fsalida(“distancias.dat”, ofstream::out | ios::binary);
fsalida.write(reinterpret_cast(&distancia), sizeof(distancia));
fsalida.write(reinterpret_cast(&distancia2), sizeof(distancia2));
fsalida.close();

// mostramos la imagen
cvShowImage(“mainWin”, img );
// esperamos apretar una tecla
cvWaitKey(0);

// liberar la imagen
cvReleaseImage(&img );
return 0;
}

[/codesyntax]

Ya tenemos toda la información que necesitamos dentro de fichero.dat. Ahora necesitaremos mover ese fichero al NXT, para ello podemos utilizar el File Managment de RobotC.

Programa de RobotC:

El programa de RobotC se encargará de, con los datos recibidos en el fichero de OpenCV, hacer los cálculos necesarios y moverse a lo largo de la línea esquivando las bolas. Para eso necesitamos los siguientes datos: altura y ángulo de la cámara en la primera foto, altura y ángulo de la cámara en la segunda foto, diámetro de la rueda (lo usaremos para saber cuántos cm se mueve el robot), altura de la imagen en píxeles, tamaño del robot en cm., y distancia de seguridad con la bola que necesitamos para realizar el giro para esquivarla. El programa quedaría de la siguiente manera:

[codesyntax lang=”cpp”]
const string sFileName = “distancias.dat”;

TFileIOResult nIoResult;
TFileHandle hFileHandle;

int nFileSize;
float altura1 = 27;
float altura2 = 30;
float grados1 = 25;
float grados2 = 50;
float diametro_rueda = 17;
float altura_imagen = 280;
float tamano_robot = 18;
float distancia_seg = 10;
{
float FloatData;

OpenRead(hFileHandle, nIoResult, sFileName, nFileSize);

ReadFloat(hFileHandle, nIoResult, FloatData);
nxtDisplayCenteredTextLine(3, “Distancia:”);
nxtDisplayCenteredTextLine(5, “%6.2f Pixeles” , FloatData);

float distancia = (FloatData/altura_imagen) *
(altura2*sinDegrees(grados2)/cosDegrees(grados2));
float rotacion2 = (distancia – distancia_seg – tamano_robot) *
360/diametro_rueda;

ReadFloat(hFileHandle, nIoResult, FloatData);
nxtDisplayCenteredTextLine(3, “Distancia:”);
nxtDisplayCenteredTextLine(5, “%6.2f Pixeles” , FloatData);

distancia = (FloatData/altura_imagen) *
(altura1*sinDegrees(grados1)/cosDegrees(grados1));
float rotacion1 = (distancia – distancia_seg) *
360/diametro_rueda;

Close(hFileHandle, nIoResult);

while ((nMotorEncoder[motorC] < rotacion1) &&
(nMotorEncoder[motorA] < rotacion1)){
motor[motorC]= 30;
motor[motorA]= 30;
}

motor[motorA]=10;
motor[motorC]=40;
wait1Msec(1000);
motor[motorA]=40;
motor[motorC]=15;
wait1Msec(2500);
motor[motorA]=0;
motor[motorC]=40;
wait1Msec(800);

motor[motorA]=0;
motor[motorC]=0;

nMotorEncoder[motorA] = 0;
nMotorEncoder[motorC] = 0;

while ((nMotorEncoder[motorC] < rotacion2) &&
(nMotorEncoder[motorA] < rotacion2)){
motor[motorC]= 30;
motor[motorA]= 30;
}

motor[motorA]=0;
motor[motorC]=0;

motor[motorA]=10;
motor[motorC]=40;
wait1Msec(1000);
motor[motorA]=40;
motor[motorC]=15;
wait1Msec(2500);
motor[motorA]=0;
motor[motorC]=40;
wait1Msec(800);

motor[motorA]=0;
motor[motorC]=0;

return;
}
[/codesyntax]

Ahora una breve explicación del programa:

1 a 14 – Declaración e inicialización de variables.

20 a 22 – Leemos del fichero y mostramos su contenido.

24 – Hacemos el cálculo de la distancia en cm, tal y como os he puesto en el apartado de trigonometría de este artículo.

26 – Para saber cuantos grados necesitamos rotar los motores para que el robot recorra determinada distancia, se multiplica la distancia por 360, y se divide entre el diámetro de las ruedas. Como en este caso es la distancia entre las dos bolas, a la distancia calculada anteriormente necesitamos restarle tanto la longitud del robot como la distancia de seguridad con la bola para poder realizar el giro necesario para esquivarla.

35 – En este caso es la distancia a la primera bola, por lo que solo le restaremos la distancia de seguridad para esquivarla.

40 a 44 – Aquí nos movemos hasta la primera bola, rotando los motores el número de grados calculados en la línea 35 del programa.

46 a 60 – Esta es la maniobra evasiva. Esta programada a mano, sin ningún cálculo, mediante ensayo y error para que el robot quedara justo pegado a la primera bola, entre la primera y la segunda bola. Esta maniobra de evasión variará mucho en función del objeto a esquivar, así como la distancia de seguridad en cm para realizarla.

62 a 66 – Aquí nos movemos hasta la segunda bola, rotando los motores el número de grados calculados en la línea 26 del programa.

71 a 79 – Otra maniobra para esquivar la segunda bola.

81 a 82 – Paramos el robot, y fin del programa

Y ahora os dejo un vídeo demostrativo del programa:

Espero que os haya gustado. Se que todo esto es un poco complicado, pero es solo una muestra de las cosas que se pueden hacer con una cámara. En un futuro los LEGO Mindstorms NXT tendrán cámaras, y su manejo será más sencillo que todo esto. Ante cualquier pregunta no dudéis en postear en el foro.