Hough Example

Sistema de Guiado Visual para Robots Móviles de
Transporte (SGVRMT)
Mendoza Quintana, Yeray
1
Indice
Transformada de Hough e integración con Aria______________________________ 3

Transformada de Hough ______________________________________________ 4
Transformada de Hough probabilística_________________________________ 6
Definición de comportamientos en Aria __________________________________ 8
Implementación_____________________________________________________ 9
Cliente Aria______________________________________________________ 9
Comportamiento de Interfaz de Usuario________________________________ 9
Comportamiento de Guiado Visual __________________________________ 10
Anexos __________________________________________________________ 13
Anexo A _______________________________________________________ 13
Anexo B _______________________________________________________ 14
Anexo C _______________________________________________________ 17
2
Transformada de Hough e integración con Aria
En esta etapa del proyecto implementaré una conducta de guiado visual que permita
realimentar el sistema de control con la información obtenida desde la cámara situada
sobre la plataforma móvil. Para realizar dicha implementación he optado por la biblioteca
Aria desarrollada por ActivMedia Robotics y que sustituye a la biblioteca Saphira en las
tareas de más bajo nivel de control del robot, encargándose de los aspectos más básicos
como la comunicación con el mismo y la lectura de los diferentes sensores.
Aria es una potente interfaz para clientes de los servidores de los robots de ActivMedia
Robotics totalmente desarrollada bajo el paradigma de orientación a objetos. Las
aplicaciones cliente desarrolladas con Aria pueden controlar dinámicamente la velocidad,
la orientación, la orientación relativa y otros parámetros de la navegación del robot
servidor.
Figura 1. Resolver
Aria también permite el desarrollo de técnicas de control basada en comportamientos

combinados mediante técnicas de control borroso. De esta forma el movimiento del robot
es definido por un conjunto de acciones de comportamiento, que son esquemas para
determinar el movimiento basado en la información sensorial y el estado interno. Los
resultados de las acciones de comportamiento son combinados por un “resolver” para
producir el movimiento. Esta aproximación es un método indirecto o particionado de
controlar el movimiento del robot, ya que el resultado final se obtiene a partir de la
contribución de módulos más pequeños.
Se desarrollará un comportamiento en Aria que obtenga las imágenes de la cámara y las

analice para detectar la línea de guiado. Para detectar la línea se utilizará la transformada
3
de Hough. La transformada de Hough es una técnica que puede ser usada para detectar
características de una figura particular dentro de una imagen. Ya que necesita que las
características deseadas sean expresadas en forma paramétrica, la transformada de Hough
clásica es usada comúnmente para detectar curvas regulares como líneas, círculos,
elipses, etc. Una transformada de Hough generalizada se puede emplear en aplicaciones
donde no es posible encontrar una descripción simple de las características.
Transformada de Hough
La transformada de Hough es especialmente útil para calcular una descripción global de

las características (donde el número de clases de soluciones puede no ser conocido a
priori), a partir de unas medidas locales (posiblemente con ruido). La idea principal de la
técnica de Hough para la detección de líneas es que cada medida de entrada (por ejemplo
las coordenadas de un punto) proporciona su propia aportación a una solución global
consistente (por ejemplo la línea física que origino la aparición de ese punto).
Un ejemplo muy simple es el problema de ajustar un conjunto de segmentos de línea a un

conjunto de puntos en una imagen. La figura 2 muestra algunas soluciones a este
problema. La falta de un conocimiento a priori del número de segmentos de línea
deseados convierte a este problema en subrestringido.
Figura 2. Coordenadas de los puntos y posibles ajustes a líneas rectas
Podemos representar analíticamente un segmento de línea de múltiple formas diferentes,

sin embargo, una ecuación muy conveniente para describir un conjunto de líneas usa la
noción de parametrización:
x cos θ + y sin θ = r
4
donde r es la longitud de la normal de la recta desde el origen y θ es la orientación de r
respecto al eje X , como se muestra en la figura 3. Para cualquier punto (x,y) sobre esta
línea, los valores de r y θ son constantes.
Figura 3. Descripción paramétrica de una línea recta
En el contexto del análisis de imágenes, las coordenadas ( xi , yi ) de los puntos de los

segmentos de borde en una imagen son conocidos y, por lo tanto, sirven como constantes
en la ecuación paramétrica de la línea, mientras que r y θ son las variables desconocidas
que estamos buscando. Si representamos los posibles valores de (r ,θ ) definidos por cada
( xi , yi ) , los puntos en el espacio de imagen cartesiano se convierten en curvas en el
espacio polar de parámetros de Hough. Esta transformación de puntos a curvas es la

transformada de Hough para líneas rectas. Los puntos colineales en el espacio cartesiano
de la imagen, cuando son vistos en el espacio de parámetros de Hough, se pueden
apreciar fácilmente como curvas que se intersectan en un punto (r ,θ ) común.
La transformada se puede implementar discretizando el espacio de parámetros de Hough

en intervalos finitos o celdas de un acumulador. Cuando el algoritmo es ejecutado, cada
valor ( xi , yi ) es transformado en una curva discretizada (r ,θ ) y el valor de la celda del
acumulador que representa esa curva es incrementado. Los picos resultantes en

acumulador son una evidencia de que una línea recta correspondiente existe en la imagen.
5
Figura 4. Detección de líneas con la transformada de Hough
Podemos usar el mismo procedimiento para detectar otras características dada se

descripción analítica. Por ejemplo, para detectar círculos, la ecuación paramétrica sería
( x − a ) 2 + ( y − b) 2 = r 2
Donde a y b son las coordenadas del centro del círculo y r es el radio. En este caso, la
complejidad computacional comienza a incrementarse ya que ahora tenemos tres
coordenadas en el espacio de parámetros y un acumulador de tres dimensiones.
Transformada de Hough probabilística

El principal problema de la transformada de Hough es su elevado coste computacional,
sin embargo han aparecido multitud de variantes que pretenden solucionar esto. Una
alternativa es considerar sólo un subconjunto de los datos de entrada seleccionados de
forma aleatoria. Las variantes que adoptan esta solución son conocidas como
transformadas de Hough Probabilísticas (PHT) y ha sido demostrado que pueden reducir
considerablemente el coste temporal de ejecución obteniendo resultados muy próximos a
los de la transformada de Hough estándar.
El coste computacional es un aspecto crítico para elegir la implementación a utilizar ya

que en este proyecto se pretende aplicar la transformada de Hough para realizar un
control en tiempo real de una plataforma móvil. OpenCV implementa tres versiones
6
diferentes de la transformada Hough, y en concreto se utilizará la implementación de la
Transformada de Hough Progresiva Probabilística (PPHT) propuesta por Matas,
Galambos y Kittler.
La idea de este algoritmo es considerar píxeles aleatorios uno a uno. En cada ocasión el
acumulador es actualizado y se comprueba si el pico más alto sobrepasa un umbral. En
caso de que esto ocurra, los puntos que pertenecen al pasillo especificado por el pico son
eliminados. Si el número de puntos excede un valor predefinido de longitud mínimo de
línea, entonces es considerado una línea, en otro caso en considerado ruido. Luego se
repite el proceso desde el principio hasta que no queden más píxeles en la imagen. El
algoritmo mejora el resultado en cada paso, por lo que puede ser interrumpido en
cualquier momento.
Este método posee una desventaja ya que, al contrario que la transformada de Hough
estándar, no es capaz de procesar correctamente algunas características de la imagen,
como por ejemplo líneas cruzadas. Sin embargo esto no supone un problema para la
aplicación concreta en este caso ya las líneas a detectar en la imagen no tienen por qué
cruzarse.
La implementación del algoritmo PPHT de OpenCV devuelve como resultado una

secuencia de líneas determinadas por dos puntos (y no en forma paramétrica) y permite
especificar por parámetros la longitud mínima que debe tener una línea para ser
considerada como tal, y el número mínimo de píxeles que pude haber entre dos líneas
para unirlas y considerarlas como una sóla.
Figura 5. Detección de líneas sobre una imagen de umbral.
7
Definición de comportamientos en Aria
Aria implementa la definición de comportamientos. La unidad más pequeña de

comportamiento es llamada action (acción). Las acciones son implementadas como
objetos y los tipos de acciones son clases de objetos. Hay una clase base para las
acciones llamada ArAction, de la cual derivan todo los tipos particulares de acciones
como subclases.
Las acciones de comportamiento pueden controlar diferentes valores del robot, como la
velocidad de desplazamiento, la posición, la velocidad de giro y la orientación. Las
salidas de las acciones son combinadas por un resolver.
En cada ciclo de 100ms, cada objeto acción es evaluado y produce una salida para la
velocidad de translación y de rotación junto con la fuerza de cada uno. La fuerza, que
puede variar entre 0 y 1, indica como de fuerte deseada la acción que sea ejecutado este
movimiento. Una vez que las salidas de todas las acciones han sido calculadas, son
pasadas al resolver para determinar cuál será la salida final. El resolver estándar de Aria
utiliza una estrategia de resolución de dos partes. Para resolver la prioridad, se asocia una
valor de prioridad con cada acción, el cuál será un entero positivo. Cuanto mayor sea el
valor mayor será la prioridad de la acción. Dentro de cada clase de prioridad ( definida
como todas las acciones que tienen la misma prioridad), se calcula el promedio de las
salidas de acuerdo con su fuerza. Para determinar el movimiento final se busca en las
clases de prioridad desde mayor a menor prioridad. Una energía de activación es
inicializada a 1 para el nivel más alto, y el nivel “utiliza” la activación de acuerdo con la
fuerza de su acción más fuerte, por ejemplo, una fuerza de 1 usará toda la activación,
mientras que una fuerza de 0 no usará ninguna. En el siguiente nivel, el comando de
movimiento es promediado, pero usando la energía de activación restante. Nuevamente,
el nivel utiliza la activación basándose en la fuerza de su movimiento de salida.
La resolución de prioridad permite a los niveles de prioridad alta bloquear completamente

a los niveles bajos cuando son fuertemente activados. Pero también permite a las
acciones de baja prioridad influenciar en el movimiento cuando las actividades más altas
son activadas débilmente. Por ejemplo, una acción IrA de baja prioridad permanecerá
activa mientras una acción de alta prioridad Evitar no halla detectado un obstáculo y trate
de evitarlo.
8
Implementación
He desarrollado un cliente Aria (ver Anexo A) capaz de conectarse con una plataforma
robótica móvil de ActivMedia Robotics así como con el simulador de las mismas que
viene con la biblioteca Aria.
También he desarrollado dos acciones de Aria. Una de ellas implementa una interfaz de
usuario al estilo de Saphira pero más sencilla, que permite monitorizar de forma gráfica
el estado del robot y controlar su movimiento mediante teclado. Es una acción muy
sencilla pero que, al mismo tiempo, sirve de ejemplo para mostrar como se pueden
combinar las bibliotecas Aria y OpenCV. La segunda acción implementa un guiado
visual del robot utilizando la información obtenida de una cámara junto con sus
parámetros extrínsecos e intrínsecos.
La función más importante de una acción Aria es la función fire que implementa el
cuerpo principal de la acción y que devuelve un valor de tipo ArActionDesired que indica
el movimiento deseado por la acción.
Cliente Aria
El cliente inicializa la biblioteca Aria y habilita el anillo de sónars del robot.
Seguidamente trata de conectarse con el robot, finalizando la ejecución si no lo consigue.
Si la conexión tiene éxito añade dos acciones a la lista de acciones del robot y pone el
robot en marcha.
La acción de mayor prioridad es de la clase IuAction, que implementa una interfaz de

usuario. Le he dado mayor prioridad porque esta acción sólo influye en el movimiento si
recibe órdenes por teclado, lo cuál debe ser atendido antes que cualquier otro movimento.
Comportamiento de Interfaz de Usuario

Un objeto de la clase IuAction (ver Anexo B) mostrará una ventana de información con
dos partes principales, una muestra información gráfica y otra información textual.
En la parte superior (información gráfica) se muestra el robot representado por una

circunferencia de la cuál sale una línea recta que representa la velocidad de
desplazamiento y rotacional del robot. La velocidad de desplazamiento se muestra por la
longitud de la línea. El robot siempre está orientado hacia la derecha en esta ventana por
9
lo que la línea de velocidad crecerá en esa dirección a si la velocidad actual es positiva, y
en sentido contrario en caso de ser negativa. La misma línea sirve para indicar la
velocidad de rotación. Si el robot está girando a la izquierda la línea se moverá en esa
dirección en la ventana, es decir, hacia arriba. Hay que tener en cuenta que si el robot se
está moviendo hacia atrás, o sea, con velocidad de traslación negativa, la línea se moverá
hacia arriba cuando el robot esté girando hacia la derecha. En la misma ventana se
muestra la lectura de los sónars realizadas cada 10º. Se mostrará un cuadrado si se detecta
algo a una distancia inferior a 2m a una distancia proporcional del centro del robot y en el
ángulo adecuado.
Figura 6. Ventana de información de IuAction
En la parte inferior se muestra la velocidad actual de traslación, de rotación y la

orientación actual del robot.
Esta acción también permite el control del robot a través del teclado. Con la tecla ‘w’ se
puede incrementar la velocidad de traslación y con la ‘s’ se puede disminuir. Con la tecla
‘a’ se puede girar a la izquierda y con la tecla ‘d’ hacia la derecha.
Comportamiento de Guiado Visual

La clase GvAction (ver Anexo C) implementa una acción de guiado visual del robot. Esta
acción carga desde fichero los parámetros extrínsecos e intrínsecos de la cámara y calcula
10
la matriz de proyección. Una vez calculada rellena una matriz que sirve como mapa en
donde se almacenan las coordenadas en la imagen correspondientes a los puntos 3D
usados como referencia durante el proceso de calibración de los parámetros extrínsecos.
También calcula las coordenadas que corresponden en la imagen con el centro del robot.
Una vez iniciada la cámara, se captura una imagen en cada ciclo de evaluación de la
acción y se procesa. Primero se corrige la distorsión de la imagen mediante una función
OpenCV y seguidamente se marca la región de la imagen que corresponde con la zona
del suelo situado frente al robot como región de interés, por lo que los procesos siguientes
se realizarán sobre esta zona de la imagen, y no tendrán en cuenta las regiones negras
añadidas por el proceso de corrección de distorsión. El siguiente paso es la búsqueda de
la línea de guiado del robot en la imagen. Para conseguirlo ser realiza un umbralizado de
la imagen y luego se aplica el algoritmo de Canny para la detección de bordes. El
umbralizado previo a la detección de bordes puede ser deshabilitado desde la ventana de
la acción, y también puede especificarse el valor de umbral. Sobre la imagen de bordes se
aplica la transformada de Hough progresiva probabilística. El centre de cada línea
detectada se marca sobre la imagen con un circulo azul. Al final de este proceso se
obtienen las coordenadas del centro de la línea situada más a la derecha. Con esta
información se busca el punto más cercano sobre la imagen que se encuentre en el mapa
y serán las coordenadas de ese punto el espacio hacia las que se dirija el robot.
Figura 7. Ventana de información de GvAction

11
En la figura 7 puede observarse la ventana de información de GvAction. Cada circulo
verde indica un punto de referencia del mapa, los azules indican el centro de cada línea
detectada el circulo grande rojo muestra el centro de la línea hacia la que se dirige el
robot. En realidad el robot se moverá hacia el punto del mapa que se encuentre más cerca
en la imagen. Se ha elegido esta estrategia por que no existe forma de determinar con
certeza las coordenadas reales de la línea en el mundo, pero podemos usar las
coordenadas de puntos conocidos y ahorrarnos el cálculo necesario para hacer
aproximaciones más precisas. De esta forma también se consigue discretizar las
orientaciones posibles del robot, lo que deriva en un movimiento más suave.
La línea azul une el centro del robot con el punto de destino del movimiento.
Figura 8. Cálculo del movimiento del robot
Las distancias dx y dy hasta al centro del robot son usadas para calcular el ángulo alfa,
que determinará la nueva orientación del robot. La velocidad de traslación se ajustará a
10cm por segundo siempre que se detecten líneas en la imagen.
12
Anexos
Anexo A
Fichero guivi_act.cpp que implementa un cliente Aria.
#include "Aria.h"
#include "IuAct.h"
#include "GvAct.h"
int main(int argc, char **argv)

{
ArRobot robot;
ArSonarDevice sonar;
ArSimpleConnector conector( &argc, argv);
IuAction interfaz;
GvAction gui_vi(true, 200);
if( !conector.parseArgs() || argc > 1)

{
conector.logOptions();
exit(1);
}
Aria::init();
robot.addRangeDevice( &sonar);
if( !conector.connectRobot( &robot))

{
printf( "No se pudo conectar con el robot... saliendo\n");
Aria::shutdown();
return 1;
}
robot.setAbsoluteMaxTransVel( 400);
robot.comInt( ArCommands::ENABLE, 1);
robot.comInt( ArCommands::SOUNDTOG, 0);
//----------------------------------------
robot.addAction( &interfaz, 50);

robot.addAction( &gui_vi, 40);
//----------------------------------------
robot.run( true);
Aria::shutdown();
return 0;
13
Anexo B
Fichero IuAct.h con la definición e implementación de la acción IuAction.
#ifndef IUACTION_H
#define IUACTION_H
#include "Aria.h"
#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
class IuAction : public ArAction

{
public:
IuAction();
virtual ~IuAction() {};
virtual ArActionDesired *fire(ArActionDesired currentDesired);
virtual void setRobot( ArRobot *robot);
protected:
void pinta_iu(IplImage *img, ArRobot *robot);
ArRangeDevice *miSonar;
ArActionDesired myDesired;
IplImage *miImg;
bool iniciar;
double miIncV;
double miIncDH;
double miVelDeseada;
double miGiroDeseado;
};
IuAction::IuAction() :
ArAction("IU", "Muestra información del robot.")
{
miSonar = NULL;
miImg = cvCreateImage( cvSize( 400, 400), IPL_DEPTH_8U, 1);
cvSet( miImg, cvScalar(255,255,255), 0);
iniciar = true;
miIncV = 15;
miIncDH = 10;
}
void IuAction::setRobot( ArRobot *robot)

{
myRobot = robot;
miSonar = myRobot->findRangeDevice("sonar");
if( miSonar == NULL)
{
printf("no se encontró ningún sónar");
deactivate();
}
}
void IuAction::pinta_iu(IplImage *img, ArRobot *robot)

{
CvFont tipof;
char texto[100];
double dist, angle;
int ancho_vent = 400;
int alto_vent = 400;
14
int alto_vent_inf = 90;
int rob_x = 150;
int rob_y = 150;
cvSet( img, cvScalar(255,255,255), 0);

// robot
cvCircle( img, cvPoint( rob_x, rob_y), 15, cvScalar(0, 0, 0));
// ventana de informacion
cvRectangle( img, cvPoint( 0, alto_vent-90), cvPoint( ancho_vent,
alto_vent), cvScalar(150,150,150),CV_FILLED);
// indicador de velocidad y orientación
if( robot->getVel() >= 0)
cvLine( img, cvPoint( rob_x, rob_y), cvPoint(
rob_x+(int)(robot->getVel()/3), rob_y-(int)(robot->getRotVel())),
cvScalar(0,0,255));
else
cvLine( img, cvPoint( rob_x, rob_y), cvPoint(
rob_x+(int)(robot->getVel()/3), rob_y+(int)(robot->getRotVel())),
cvScalar(0,0,255));
cvInitFont( &tipof, CV_FONT_HERSHEY_PLAIN, 1.f, 1.f, 0.0f, 0.5f,

CV_AA);
// velocidad actual
sprintf( texto, "Vel: %d", (int)robot->getVel());
cvPutText( img, texto, cvPoint(20, alto_vent-alto_vent_inf+30),
&tipof, cvScalar(0,0,0));
// velocidad de rotación actual
sprintf( texto, "Rot Vel: %d", (int)robot->getRotVel());
// orientación actual
sprintf( texto, "Th: %d", (int)robot->getTh());
// lecturas del sónar

for( int iang=-90; iang<90; iang+=10)
{
dist = robot->checkRangeDevicesCurrentPolar( iang, iang+10,
&angle);
if( dist < 2000)
{
int x, y;
x = (int)(cos( angle*CV_PI/180.)*((dist*150)/2000));
y = (int)(sin( angle*CV_PI/180.)*((dist*150)/2000));
cvRectangle( img, cvPoint(rob_x+x,rob_y-y),
cvPoint(rob_x+5+x,rob_y+5-y), cvScalar(0,0,255));
}
}
}
ArActionDesired *IuAction::fire(ArActionDesired currentDesired)

{
double rango;
double radio;
char c = 0;
myDesired.reset();
if( miSonar == NULL)
{
15
deactivate();
return NULL;
}
if( iniciar)
{
cvNamedWindow("IU", CV_WINDOW_AUTOSIZE);
iniciar=false;
}
miVelDeseada = myRobot->getVel();
miGiroDeseado = 0;
if( miImg != 0)
{
pinta_iu( miImg, myRobot);
cvShowImage("IU", miImg);
if( ( c = cvWaitKey(1)) != -1)
{
switch( c)
{
case 'w': miVelDeseada += miIncV; break;
case 's': miVelDeseada -= miIncV; break;
case 'a': miGiroDeseado = miIncDH; break;
case 'd': miGiroDeseado = -miIncDH; break;
case 32: miVelDeseada = 0; break;
case '\x1b': myRobot->stopRunning(); break;
}
myDesired.setVel( miVelDeseada);
myDesired.setDeltaHeading( miGiroDeseado);
return &myDesired;
}
}
return NULL;
}
#endif // IUACTION_H
16
Anexo C
Fichero GvAct.h con la definición e implementación de la acción GvAction.
#ifndef GVACTION_H
#define GVACTION_H
#include "Aria.h"
#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
class GvAction : public ArAction

{
public:
GvAction(int umbralizar=true, int umbral=200);
virtual ~GvAction() {};
virtual ArActionDesired *fire( ArActionDesired currenDesired);
virtual void setRobot( ArRobot *robot);
protected:
int BuscaPtoDestino( IplImage *src, CvPoint *pto_d);
int BuscaEnMapa( CvPoint pto_d);
ArActionDesired myDesired;
CvSize dim_frame;
IplImage *frame;
IplImage *img;
bool camara_iniciada;
int DibujaMapa( IplImage *src);
int carga_params_int( char *nombre_fichero);

float mat_camara[3*3];
float vec_distorsion[4];
CvSize dim_img_calib;
int carga_params_ext( char *nombre_fichero);

float vec_traslacion[3];
float mat_rotacion[3*3];
float P[3*4];
CvMat mP;
CvPoint *mapa;
CvPoint3D32f *mapa3D;
CvPoint centro_robot;
CvPoint3D32f centro_robot3D;
CvCapture *camara;
int miUmbral;
int miUmbralizar;
};
GvAction::GvAction(int umbralizar, int umbral) :

ArAction( "GvAction", "Conducta de guiado visual.")
{
camara_iniciada = false;
camara = 0;
miUmbralizar = umbralizar;
miUmbral = umbral;
float A[3*4];
carga_params_int( "params_int.txt");
17
A[0] = mat_camara[0]; A[1] = mat_camara[1]; A[2] = mat_camara[2];
A[3] = 0;
A[7] = 0;
A[11] = 0;
float D[4*4];
carga_params_ext( "params_ext.txt");
D[0] = mat_rotacion[0]; D[1] = mat_rotacion[1]; D[2] =
mat_rotacion[2]; D[3] = vec_traslacion[0];
D[12] = 0; D[13] = 0; D[14] = 0; D[15] = 1;
CvMat mA, mD;

cvInitMatHeader( &mA, 3, 4, CV_32FC1, A);
cvInitMatHeader( &mD, 4, 4, CV_32FC1, D);
cvInitMatHeader( &mP, 3, 4, CV_32FC1, P);
cvMatMul( &mA, &mD, &mP);
float XYZ[4];
float uv[3];
CvMat mXYZ;
CvMat muv;
cvInitMatHeader( &mXYZ, 4, 1, CV_32FC1, XYZ);

cvInitMatHeader( &muv, 3, 1, CV_32FC1, uv);
mapa = (CvPoint*)malloc( 10 * 6 * sizeof(CvPoint));

mapa3D = (CvPoint3D32f*)malloc( 10 * 6 * sizeof(CvPoint3D32f));
int cont = 0;
for( int j=0; j<6; j++)

{
for( int i=-5; i<5; i++)
{
XYZ[0] = i*20; XYZ[1] = j*20; XYZ[2] = 0; XYZ[3] = 1;
mapa3D[cont] = cvPoint3D32f( i*20, j*20, 0);
cvMatMul( &mP, &mXYZ, &muv);
mapa[cont] = cvPoint( uv[0]/uv[2], uv[1]/uv[2]);
cont++;
}
}
XYZ[0] = 0; XYZ[1] = -210; XYZ[2] = 0; XYZ[3] = 1;
centro_robot3D = cvPoint3D32f( 0, -210, 0);
cvMatMul( &mP, &mXYZ, &muv);
centro_robot = cvPoint( uv[0]/uv[2], uv[1]/uv[2]);
}
void GvAction::setRobot( ArRobot *robot)

{
myRobot = robot;
}
int GvAction::BuscaPtoDestino( IplImage *src, CvPoint *pto_d)

{
IplImage *gris = cvCreateImage( cvGetSize( src), IPL_DEPTH_8U, 1);
18
IplImage *gris2 = cvCreateImage( cvGetSize( src), IPL_DEPTH_8U,
1);
CvMemStorage *storage = cvCreateMemStorage( 0);
CvSeq *lines = 0;
*pto_d = cvPoint(0,0);
cvCvtColor( src, gris, CV_BGR2GRAY);

if( miUmbralizar)
cvThreshold( gris, gris2, miUmbral, 255,
CV_THRESH_BINARY_INV);
else
cvCopy( gris, gris2, 0);
// detectamos los bordes en la imagen

cvCanny( gris2, gris, 50, 200, 3);
// aplicamos la transformada de Hough probabilística progresiva
lines = cvHoughLines2( gris, storage, CV_HOUGH_PROBABILISTIC, 1,
CV_PI/180, 20, 30, 5);
int n_lines = lines->total;

if( lines->total > 0)
{
int xmax, i_xmax;
CvPoint* line = (CvPoint*)cvGetSeqElem(lines,0);

i_xmax = 0;
xmax = (line[0].x + line[1].x)/2;
// buscamos la línea más a la derecha
for(int i = 0; i < lines->total; i++ )
{
// mostramos con un círculo el centro de cada línea
cvCircle( img, cvPoint( (line[0].x + line[1].x)/2,
(line[0].y + line[1].y)/2), 3, cvScalar(255,0,0));
line = (CvPoint*)cvGetSeqElem(lines,i);
if( (line[0].x + line[1].x)/2 > xmax)
{
xmax = (line[0].x + line[1].x)/2;
i_xmax = i;
}
}
line = (CvPoint*)cvGetSeqElem(lines,i_xmax);
// pintamos la línea que está más a la derecha
cvLine( src, line[0], line[1], CV_RGB(255,0,0), 2, 8 );
*pto_d = cvPoint( (line[0].x + line[1].x)/2, (line[0].y +
line[1].y)/2);
}
cvReleaseImage( &gris);
cvReleaseImage( &gris2);
cvReleaseMemStorage( &storage);
return n_lines;
}
int GvAction::DibujaMapa( IplImage *src)

{
for( int i=0; i<60; i++)
cvCircle( src, mapa[i], 3, cvScalar(0,255,0));
return 0;
}
19
int GvAction::carga_params_int( char *nombre_fichero)
{
FILE *f;
f = fopen( nombre_fichero, "r");

if( !f) return 1;
fscanf( f, "%d%d", &dim_img_calib.width, &dim_img_calib.height);
fscanf( f, "%f%f%f", &mat_camara[0], &mat_camara[1],

&mat_camara[2]);
&mat_camara[5]);
&mat_camara[8]);
fscanf( f, "%f%f%f%f", &vec_distorsion[0], &vec_distorsion[1],

&vec_distorsion[2], &vec_distorsion[3]);
fclose( f);
return 0;
}
int GvAction::carga_params_ext( char *nombre_fichero)

{
FILE *f;
f = fopen( nombre_fichero, "r");

if( !f) return 1;
fscanf( f, "%f%f%f", &vec_traslacion[0], &vec_traslacion[1],

&vec_traslacion[2]);
fscanf( f, "%f%f%f", &mat_rotacion[0], &mat_rotacion[1],

&mat_rotacion[2]);
&mat_rotacion[5]);
&mat_rotacion[8]);
fclose( f);
return 0;
}
int GvAction::BuscaEnMapa( CvPoint pto_d)

{
int i = 0;
while( (pto_d.x > mapa[i].x) || (pto_d.y < mapa[i].y)) i++;
i--;
int dist = sqrt( pow( abs( pto_d.x - mapa[i+1].x),2) + pow( abs(

pto_d.y - mapa[i+1].y),2));
int i_mapa = i+1;
if( sqrt( pow( abs( pto_d.x - mapa[i].x),2) + pow( abs( pto_d.y -

mapa[i].y),2)) < dist)
{
dist =sqrt( pow( abs( pto_d.x - mapa[i].x),2) + pow( abs(
pto_d.y - mapa[i].y),2));
i_mapa = i;
}
20
if( i>9)
{
if( sqrt( pow( abs( pto_d.x - mapa[i+1-10].x),2) + pow( abs(
pto_d.y - mapa[i+1-10].y),2)) < dist)
{
dist =sqrt( pow( abs( pto_d.x - mapa[i+1-10].x),2) +
pow( abs( pto_d.y - mapa[i+1-10].y),2));
i_mapa = i+1-10;
}
if( sqrt( pow( abs( pto_d.x - mapa[i-10].x),2) + pow( abs(
pto_d.y - mapa[i-10].y),2)) < dist)
{
dist =sqrt( pow( abs( pto_d.x - mapa[i-10].x),2) +
pow( abs( pto_d.y - mapa[i-10].y),2));
i_mapa = i-10;
}
}
return i_mapa;
}
ArActionDesired *GvAction::fire( ArActionDesired currentDesired)

{
myDesired.reset();
CvPoint pto_dest = cvPoint( 0, 0);
if( !camara_iniciada)
{
camara = cvCaptureFromCAM( 0);
if(( camara) && ( frame = cvQueryFrame( camara)))
{
img = cvCreateImage( cvGetSize( frame), IPL_DEPTH_8U,
3);
camara_iniciada = true;
dim_frame = cvGetSize( frame);
cvNamedWindow( "GvAction", CV_WINDOW_AUTOSIZE);

cvCreateTrackbar( "Umbral", "GvAction", &miUmbral,
255, 0);
cvCreateTrackbar( "Umbralizar", "GvAction",
&miUmbralizar, 1, 0);
}
else
{
printf( "No se puede iniciar la camara\nDesactivando
GvAction\n");
deactivate();
return NULL;
}
}
frame = cvQueryFrame( camara); cvFlip( frame, 0, 0);
cvCopy( frame, img, 0);

cvUnDistortOnce( frame, img, mat_camara, vec_distorsion);
cvSetImageROI( img, cvRect( 50, 50, img->width-25, img->height-

25));
if( BuscaPtoDestino( img, &pto_dest))
{
pto_dest.x += 50;
21
pto_dest.y += 50;
cvResetImageROI( img);
DibujaMapa( img);
cvCircle( img, pto_dest, 6, cvScalar(0,0,255));
int i_mapa = BuscaEnMapa( pto_dest);

cvCircle( img, mapa[i_mapa], 3, cvScalar(0,0,255));
cvLine( img, centro_robot, mapa[i_mapa], CV_RGB(0,0,255));
if( mapa3D[i_mapa].x == 0)
{
myDesired.setDeltaHeading(0);
}
else
{
double hip = sqrt( pow( mapa3D[i_mapa].x, 2) + pow(
abs( centro_robot3D.y) + mapa3D[i_mapa].y, 2));
double theta = (asin( fabs( mapa3D[i_mapa].x) / hip) *
360) / CV_PI;
if( mapa3D[i_mapa].x > 0) theta = theta * -1;
myDesired.setDeltaHeading( theta);
}
myDesired.setVel( 100);
}
else
{
myDesired.setDeltaHeading( 0);
myDesired.setVel( 0);
}
cvResetImageROI( img);
cvRectangle( img, cvPoint( 50, 50), cvPoint(img->width-25, img-

>height-25), cvScalar( 0, 255, 0));
cvShowImage( "GvAction", img);
return &myDesired;
}
#endif // GVACTION_H
22
Bibliografía
[Ark98] Arkin R., "Behavior-Based Robotics", MIT Press, 1.998.
[Kor98] Kortenkamp D., Bonaso P., Murphy R. (Edit.) "Artificial Inteligence and Mobile
Robots. Case Studies of Successful Robot Systems", MIT Press, 1.998.
[Ocv04] Intel OpenCV Computer Vision Library (C++),

http://www.intel.com/research/mrl/research/opencv/.
[Ari04] ActivMedia Robotics.
[Kalv95] H. Kälviäinen, P. Hirvonen, L. Xu y E. Oja. “Probabilistic and non-
probabilistic Hough transforms: averview and comparisons”, Image and Vision
Computing, 1995.
23
24

Hough Example

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Hough Example

Cargado por

Copyright:

Formatos disponibles

Sistema de Guiado Visual para Robots Móviles de

Mendoza Quintana, Yeray

Transformada de Hough e integración con Aria______________________________ 3

Aria también permite el desarrollo de técnicas de control basada en comportamientos

Se desarrollará un comportamiento en Aria que obtenga las imágenes de la cámara y las

La transformada de Hough es especialmente útil para calcular una descripción global de

Un ejemplo muy simple es el problema de ajustar un conjunto de segmentos de línea a un

Figura 2. Coordenadas de los puntos y posibles ajustes a líneas rectas

Podemos representar analíticamente un segmento de línea de múltiple formas diferentes,

Figura 3. Descripción paramétrica de una línea recta

En el contexto del análisis de imágenes, las coordenadas ( xi , yi ) de los puntos de los

espacio polar de parámetros de Hough. Esta transformación de puntos a curvas es la

La transformada se puede implementar discretizando el espacio de parámetros de Hough

acumulador que representa esa curva es incrementado. Los picos resultantes en

Podemos usar el mismo procedimiento para detectar otras características dada se

Transformada de Hough probabilística

El coste computacional es un aspecto crítico para elegir la implementación a utilizar ya

La implementación del algoritmo PPHT de OpenCV devuelve como resultado una

Figura 5. Detección de líneas sobre una imagen de umbral.

Aria implementa la definición de comportamientos. La unidad más pequeña de

La resolución de prioridad permite a los niveles de prioridad alta bloquear completamente

La acción de mayor prioridad es de la clase IuAction, que implementa una interfaz de

Comportamiento de Interfaz de Usuario

En la parte superior (información gráfica) se muestra el robot representado por una

Figura 6. Ventana de información de IuAction

En la parte inferior se muestra la velocidad actual de traslación, de rotación y la

Comportamiento de Guiado Visual

Figura 7. Ventana de información de GvAction

Figura 8. Cálculo del movimiento del robot

int main(int argc, char **argv)

if( !conector.parseArgs() || argc > 1)

if( !conector.connectRobot( &robot))

robot.addAction( &interfaz, 50);

class IuAction : public ArAction

void IuAction::setRobot( ArRobot *robot)

void IuAction::pinta_iu(IplImage *img, ArRobot *robot)

cvSet( img, cvScalar(255,255,255), 0);

cvInitFont( &tipof, CV_FONT_HERSHEY_PLAIN, 1.f, 1.f, 0.0f, 0.5f,

// lecturas del sónar

ArActionDesired *IuAction::fire(ArActionDesired currentDesired)

class GvAction : public ArAction

int carga_params_int( char *nombre_fichero);

int carga_params_ext( char *nombre_fichero);

GvAction::GvAction(int umbralizar, int umbral) :

CvMat mA, mD;

cvMatMul( &mA, &mD, &mP);

cvInitMatHeader( &mXYZ, 4, 1, CV_32FC1, XYZ);

mapa = (CvPoint*)malloc( 10 * 6 * sizeof(CvPoint));

for( int j=0; j<6; j++)

void GvAction::setRobot( ArRobot *robot)

int GvAction::BuscaPtoDestino( IplImage *src, CvPoint *pto_d)

cvCvtColor( src, gris, CV_BGR2GRAY);

// detectamos los bordes en la imagen

int n_lines = lines->total;

CvPoint* line = (CvPoint*)cvGetSeqElem(lines,0);

int GvAction::DibujaMapa( IplImage *src)

f = fopen( nombre_fichero, "r");

fscanf( f, "%d%d", &dim_img_calib.width, &dim_img_calib.height);

fscanf( f, "%f%f%f", &mat_camara[0], &mat_camara[1],

fscanf( f, "%f%f%f%f", &vec_distorsion[0], &vec_distorsion[1],

int GvAction::carga_params_ext( char *nombre_fichero)

f = fopen( nombre_fichero, "r");

fscanf( f, "%f%f%f", &vec_traslacion[0], &vec_traslacion[1],

void IuAction::pinta_iu(IplImage img, ArRobot robot)

mapa = (CvPoint)malloc( 10 6 * sizeof(CvPoint));

int GvAction::BuscaPtoDestino( IplImage src, CvPoint pto_d)