Maestría en ciencia de la ingeniería en

mecatrónica.

Ovalle Ozuna Edgar Omar

Navegación autónoma
usando una sola cámara.
Método doctor Abiel

21 de Noviembre del 2021

 RESUMEN

En el siguiente trabajo implementaremos el método para la obtención de

profundidad con una sola cámara. En este método se usan algoritmos de convolución,
reducción piramidal y flujo óptico para estimar la profundidad a partir de dos
fotogramas consecutivos. Una vez obtenida la imagen de profundidad se realiza un
algoritmo para la navegación autónoma.

INTRODUCCION

Los Robots Autónomos (RA) son sistemas completos que operan eficientemente en
entornos complejos sin necesidad de estar constantemente guiados y controlados por operadores
humanos. Una propiedad fundamental de los RA es la de poder reconfigurarse dinámicamente
para resolver distintas tareas según las características del entorno se lo imponga en un momento
dado. Hacemos énfasis en que son sistemas completos que perciben y actúan en entornos
dinámicos y parcialmente impredecibles, coordinando interoperaciones entre capacidades
complementarias de sus componentes. La funcionalidad de los RA es muy amplia y variada desde
algunos RA que trabajan en entornos inhabitables, a otros que asisten a gente discapacitada.

La autonomía de un robot móvil se basa principalmente en el sistema de navegación

automática. En estos sistemas se incluyen tareas de planificación, percepción y control. En los
robots móviles el problema de la planificación, en el caso más general, puede descomponerse en
planificación global de la misión, de la ruta, de la trayectoria y finalmente, evitar obstáculos no
esperados. Para facilitar la búsqueda existen técnicas de descomposición del espacio en celdas,
utilización de restricciones de varios niveles de resolución y búsqueda jerarquizada, que permiten
hacer más eficiente el proceso con vistas a su aplicación en tiempo real, minimizando el coste
computacional del mismo.

La planificación de la trayectoria puede realizarse también de forma dinámica,

considerando la posición actual del vehículo y los puntos intermedios de paso definidos en la
planificación de la ruta. La trayectoria se corrige debido a acontecimientos no considerados. La
definición de la trayectoria debe tener en cuenta las características cinemáticas del vehículo en
cuestión. Por ejemplo, en vehículos con ruedas y tracción convencional, interesa definir
trayectorias de curvatura continua que puedan ejecutarse con el menor error posible.

Una vez realizada la planificación de la trayectoria, es necesario planificar movimientos

concretos y controlar dichos movimientos para mantener al vehículo en la trayectoria planificada.

El uso de una cámara monocular en movimiento es capaz de estimar la profundidad de la

escena a partir de dos fotogramas consecutivos en los cuales se analiza el movimiento de los
pixeles (flujo óptico), esto permite saber cuánto se movió un píxel de una a otra imagen. Si un
píxel tiene un desplazamiento alto significa que el objeto se encuentra cerca y si el píxel tiene un
desplazamiento pequeño significa que el objeto se encuentra lejos. Esto permite estimar la
distancia de los objetos.
DESARROLLO
Comenzamos importando las librerías para el manejo de imágenes, matrices y una carpeta
donde se encuentra la secuencia de video.

Figura 1

Creamos un objeto llamado depth para llamar a las funciones dentro de la clase creada,
esta clase se nombra más adelante como Depth. Declaramos la carpeta de imágenes para el
análisis de dos imágenes consecutivas, estás imágenes las convertimos en matrices array y se las
enviamos a una función llamada fotogramas para manejar estás imágenes dentro de la clase.
También iniciamos la función optical_flow0, esta función tiene el flujo óptico con la resolución
de mayor tamaño, obteniendo de esta el resultado para la profundidad.

Figura 2
 En la figura 3 se muestra la clase creada Depth, donde comenzamos creando
una función de inicio y definimos una máscara de convolución self.G. Así también se
crea la función fotogramas, en está función recibimos las dos imágenes consecutivas
para manipularlas dentro de la clase en las funciones a continuación.

Figura 3

Continuamos creando una función para la convolución y reducción de las

imágenes originales y obtener dos imágenes con resolución de 310x94, siendo la de
menor resolución, para esto se toma un renglón y una columna cada cuatro espacios
de la imagen original. Teniendo la imagen de menor resolución pasamos a la
convolución de esta, donde seleccionamos una vecindad de pixeles de 3x3 que se
multiplicará con la máscara de convolución antes mencionada, retornando dos
imágenes filtradas.

Figura 4
 Aplicamos lo anterior en la figura 5 para la pirámide en la función pyr_conv1,
la diferencia es que tomamos cada dos espacios un renglón y una columna para
obtener la resolución de 620x188. Retornamos las imágenes filtradas.}
En la siguiente función pyr_conv0, tenemos las imágenes con mayor
resolución aplicando la convolución y retornando las imágenes filtradas.

Figura 5
 En la figura 6 se muestra una función para el flujo óptico en la pirámide de
menor resolución usando la convolución correspondiente creando una ventana de
búsqueda de 16X16 y una vecindad de pixeles de 3X3. Ya que la reducción es la
mitad de la siguiente, nuestros valores de disparidad deben guardarse en matrices del
mismo tamaño, por ello creamos dos matrices, dx y dy, multiplicadas por dos para
compensar la siguiente pirámide.
Para el flujo óptico comenzamos con el pixel central de acuerdo con las
ventanas utilizadas para calcular la sumatoria de diferencias absolutas de la imagen1
con la imagen 2. Se guarda como valor absoluto en suma_error. Guardamos los
valores de d y e cada que se cumpla la condición if. Estos valores corresponden a
cuanto se movió el píxel encontrado de una a otra imagen. Guardamos los valores en
dx2 y dy2 multiplicando estos para compensar el tamaño de resolución. Retornamos
dx2 y dy2.

Figura 6
 Para el flujo óptico en la pirámide siguiente, llamamos las imágenes de la convolución en
pyr_conv1 y el resultado del flujo óptico anterior. En esta estimación de flujo óptico la ventana de
búsqueda es de 2x2 para hacer mas precisa la estimación tomando en cuenta el flujo anterior. Al
valor de flujo óptico en x se guarda en la variable valorX_1 y los de y en valorY_1, estos se
toman en cuenta para la ubicación donde empezara la búsqueda en esta pirámide guardad en
suma_error. Nuevamente guardamos los valores de movimientos en d y e y se guardan en
matrices del tamaño a la siguiente pirámide.

Figura 7
 En la figura 8 se muestra la función para el flujo óptico de la pirámide con mayor
resolución, acá llamamos la convolución correspondiente y el flujo óptico anterior.
En la matriz dx0 y dy0 está el flujo óptico general de cada eje, para obtener la profundidad
se usa la matriz dy0 y se eleva al cuadrado.

Figura 8
 Para la navegación autónoma, tomamos la imagen de flujo óptico dy0.
Analizamos los valores de la imagen y definimos un rango para detectar a objetos
que estén cerca. A continuación, se muestra en la figura 9 un código para recorrer
de izquierda y derecha encontrando un valor mayor al rango definido. Y
dependiendo la cantidad de el recorrido en izquierda y en derecha el robot
decidirá hacia donde ir.

Figura 9
RESULTADOS.
En la figura 9 se muestra parte de las imágenes en el dataset Kitti.

Figura 10
Figura 11

Figura 12

Figura 13

Figura 14
 Figura 15

Figura 16

Figura 17

CONCLUSION
En el siguiente trabajo se seleccionó una zona específica de una imagen de profundidad
por la cual el robot navega, si dentro de esa zona el robot detecta un desplazamiento de los píxeles
alto, es decir, un objeto cercano, entonces toma una decisión, si ir a la derecha o a la izquierda.
 ANEXO A
from PIL import Image
import os
import numpy as np

class Depth:
def __init__(self):
self.G = np.array([[0.0751, 0.1238, 0.0751],###Kernel
[0.1238, 0.2042, 0.1238],
[0.0751, 0.1238, 0.0751]])

def fotogramas(self, img1, img2):

self.image1 = img1
self.image2 = img2

def pyr_conv2(self):
data_image1_2 = self.image1
data_image2_2 = self.image2
ren, col = data_image1_2.shape
print(ren,col)
reduccion1_2 = (data_image1_2[0:ren:4, 0:col:4])
reduccion2_2 = (data_image2_2[0:ren:4, 0:col:4])
r, c = reduccion1_2.shape
print(r,c)
im_new_reduce1_2 = np.zeros([r,c], dtype=np.uint8)
im_new_reduce2_2 = np.zeros([r,c], dtype=np.uint8)
for i in range(1,r-1):
for j in range(1,c-1):
suma_reduccion1_2 = 0
suma_reduccion2_2 = 0
for m in range(-1,1+1):
for n in range(-1,1+1):
suma_reduccion1_2 += reduccion1_2[i+m,j+n]*self.G[m+1,n+1]
suma_reduccion2_2 += reduccion2_2[i+m,j+n]*self.G[m+1,n+1]
im_new_reduce1_2[i,j] = suma_reduccion1_2
im_new_reduce2_2[i,j] = suma_reduccion2_2
#print(im_new_reduce1)

# PIL_self_mono1 = Image.fromarray(np.uint8(im_new_reduce1))
# PIL_self_mono2 = Image.fromarray(np.uint8(im_new_reduce2))
# PIL_self_mono1.show()
# PIL_self_mono2.show()
return im_new_reduce1_2, im_new_reduce2_2

def pyr_conv1(self):
data_image1_1 = self.image1
data_image2_1 = self.image2
#print(matriz1)
ren, col = data_image1_1.shape
print(ren,col)
reduccion1_1 = (data_image1_1[0:ren:2, 0:col:2])
reduccion2_1 = (data_image2_1[0:ren:2, 0:col:2])
r, c = reduccion1_1.shape
print(r,c)
im_new_reduce1_1 = np.zeros([r,c], dtype=np.uint8)
im_new_reduce2_1 = np.zeros([r,c], dtype=np.uint8)
for i in range(1,r-1):
for j in range(1,c-1):
suma_reduccion1_1 = 0
suma_reduccion2_1 = 0
for m in range(-1,1+1):
for n in range(-1,1+1):
suma_reduccion1_1 += reduccion1_1[i+m,j+n]*self.G[m+1,n+1]
suma_reduccion2_1 += reduccion2_1[i+m,j+n]*self.G[m+1,n+1]
im_new_reduce1_1[i,j] = suma_reduccion1_1
im_new_reduce2_1[i,j] = suma_reduccion2_1
return im_new_reduce1_1, im_new_reduce2_1

def pyr_conv0(self):
data_image1_0 = self.image1
data_image2_0 = self.image2
ren,col = data_image1_0.shape
print(ren,col)
im_new1 = np.zeros([ren, col], dtype=np.uint8)
im_new2 = np.zeros([ren,col], dtype=np.uint8)
for i in range(1,ren-1):
for j in range(1,col-1):
suma1 = 0
suma2 = 0
for m in range(-1,1+1):
for n in range(-1,1+1):
suma1 += data_image1_0[i+m,j+n]*self.G[m+1,n+1]
suma2 += data_image2_0[i+m,j+n]*self.G[m+1,n+1]
im_new1[i,j] = suma1
im_new2[i,j] = suma2
return im_new1, im_new2

def optical_flow2(self):
 pyr1_2, pyr2_2 = self.pyr_conv2()
data_op_1_2 = np.array((pyr1_2), dtype='int64')
data_op_2_2 = np.array((pyr2_2), dtype='int64')
window_size = 3
dmax = 16
ren, col = pyr1_2.shape
dx2 = np.zeros((ren*2,col*2))
dy2 = np.zeros((ren*2,col*2))

for r in range(window_size + dmax, ren - window_size - dmax):

for c in range(window_size + dmax, col - window_size - dmax):
dmin1 = 0
dmin2 = 0
emin = 255 * 2 * window_size
for d in range(-dmax,dmax+1):
for e in range(-dmax,dmax+1):
suma_error = 0
for j in range(-window_size, window_size + 1):
for k in range(-window_size, window_size + 1):
suma_error += abs(data_op_1_2[r+j,c+k] - data_op_2_2[r+j+d,c+k+e])
if suma_error < emin:
emin = suma_error
dmin1 = d
dmin2 = e
dx2[r,c] = dmin1*2
dy2[r,c] = dmin2*2

return dx2, dy2

def optical_flow1(self):
pyr1_1, pyr2_1 = self.pyr_conv1()
disparityX_2, disparityY_2 = self.optical_flow2()
data_op_1_1 = np.array((pyr1_1), dtype='int64')
data_op_2_1 = np.array((pyr2_1), dtype='int64')
window_size = 3
dmax = 2
ren, col = pyr1_1.shape
dx1 = np.zeros((ren*2,col*2))
dy1 = np.zeros((ren*2,col*2))

for r in range(window_size + dmax, ren - window_size - dmax):

for c in range(window_size + dmax, col - window_size - dmax):
dmin1 = 0
dmin2 = 0
emin = 255 * 2 * window_size
valorX_1 = np.int(disparityX_2[r,c])
valorY_1 = np.int(disparityY_2[r,c])
for d in range(-dmax,dmax+1):
for e in range(-dmax,dmax+1):
suma_error = 0
for j in range(-window_size, window_size + 1):
for k in range(-window_size, window_size + 1):
suma_error += abs(data_op_1_1[r+j,c+k] - data_op_2_1[r+j+d+valorX_1,c+k+e+valorY_1])
if suma_error < emin:
emin = suma_error
dmin1 = d
dmin2 = e
dx1[r,c] = dmin1*2
dy1[r,c] = dmin2*2

return dx1, dy1

def optical_flow0(self):
pyr1_0, pyr2_0 = self.pyr_conv0()
disparityX_1, disparityY_1 = self.optical_flow1()
data_op_1_0 = np.array((pyr1_0), dtype='int64')
data_op_2_0 = np.array((pyr2_0), dtype='int64')
window_size = 3
dmax = 2
ren, col = pyr1_0.shape
dx0 = np.zeros((ren,col))
dy0 = np.zeros((ren,col))

for r in range(window_size + dmax, ren - window_size - dmax):

for c in range(window_size + dmax, col - window_size - dmax):
dmin1 = 0
dmin2 = 0
emin = 255 * 2 * window_size
valorX_0 = np.int(disparityX_1[r,c])
valorY_0 = np.int(disparityY_1[r,c])
for d in range(-dmax,dmax+1):
for e in range(-dmax,dmax+1):
suma_error = 0
for j in range(-window_size, window_size + 1):
for k in range(-window_size, window_size + 1):
suma_error += abs(data_op_1_0[r+j,c+k] - data_op_2_0[r+j+d+valorX_0,c+k+e+valorY_0])
if suma_error < emin:
emin = suma_error
dmin1 = d*2
dmin2 = e*2
dx0[r,c] = dmin1
dy0[r,c] = dmin2
 #print(dx)
PIL_self_mono1 = Image.fromarray(np.uint8(dy0)**2)
PIL_self_mono1.show()
print(PIL_self_mono1.size)

def mostrar(self, img1, img2):

img1.show()
img2.show()

depth = Depth()

input_images = "C:/Users/ovall/Desktop/Cuda_From_Motion/kitti reduce/"

files_names = os.listdir(input_images)
for k in range(1,7):
filename1 = files_names[k-1]
filename2 = files_names[k]
#print(filename1,filename2)

image1 = Image.open(input_images+"/"+filename1)
image2 = Image.open(input_images+"/"+filename2)
# self.image1.show()
# self.image2.show()
mono1 = image1.convert('L')
mono2 = image2.convert('L')
data_image1 = np.array(mono1, dtype=np.uint16)
data_image2 = np.array(mono2, dtype=np.uint16)
#self.mono1.show()
#self.mono2.show()

#depth.mostrar(mono1, mono2)
depth.fotogramas(data_image1, data_image2)
#depth.pyr_conv0()
#depth.pyr_conv2()
depth.optical_flow0()