Localização e Mapeamento em Simultâneo com Monocâmara:

Detecção de Zonas de Aterragem

Nuno Miguel Banheiro Moutinho

Dissertação para a obtenção do Grau de Mestre em

Engenharia Aeroespacial

Júri

Presidente: Prof. João Manuel Lage de Miranda Lemos

Orientador: Prof. Alexandre José Malheiro Bernardino

Vogal: Dr. Ruben Martinez-Cantin

Novembro 2009

Resumo

O desenvolvimento de sistemas autónomos tem vindo a crescer ao longo dos últimos anos. A ca-pacidade de sistemas se localizarem e simultaneamente mapearem ambientes desconhecidos, semqualquer tipo de intervenção humana, permitiu que a exploração do desconhecido evoluísse para pata-mares nunca antes considerados. É necessário que um sistema deste tipo tenha capacidade de decidire agir por ele próprio em todas as fases da missão para a qual foi proposto. O desenvolvimento deferramentas que ajudem o sistema a decidir melhor em certas situações aproxima-os cada vez mais donível de autonomia desejado. Porém, a completa autonomia está ainda longe de ter sido atingida.

O trabalho apresentado nesta tese consiste no desenvolvimento de um sistema de localização emapeamento em simultâneo que utiliza apenas uma câmara como sensor. Este sistema permitedeterminar o movimento tridimensional efectuado por essa câmara e estimar com alguma precisão alocalização dos pontos de referência que esta usa para se localizar. Foi ainda desenvolvido umalgoritmo que permite ao sistema detectar zonas planas no mundo, a partir dos pontos de referênciaobtidos, que possam ser consideradas possíveis zonas de aterragem para veículos aerospaciais.

Todos os resultados apresentados nesta tese são referentes a simulações obtidas em ambientescontrolados, sendo que o sistema em causa não foi implementado em tempo real.

Palavras-chave: Localização, Mapeamento, Câmara, Zonas de Aterragem

i

Abstract

The development of autonomous systems has been growing throughout the years. The capacity systemshave to, simultaneously, locate and map unknown environments, without any kind of human intervention,empowered the unknown’s exploration evolution to levels never considered before. It is therefore nec-essary for this kind of system to have capabilities of action and decision in the required mission phasesfor which it was designed. The development of tools that can interact in the decision process of certainsituations, constitutes an advance, bringing the systems closer to the desired autonomy level. However,complete autonomy is yet to be reached.

The work presented on this thesis focus on the development of a simultaneous tracking and mappingsystem, which employs only one camera as a sensor. This system allows to determine thetridimensional movement completed by the specific camera and, estimate with a relative precision, theposition of the reference points used in the process to locate itself. Furthermore, an algorithm wasdeveloped aiming for the detection of flat areas in the real world, using the previously obtainedreference points, which can be considered feasible landing areas for aerospace vehicles.

All the results showed in this thesis concern to simulations obtained in controlled environments, notingthat the system was not implemented in real time.

Keywords: Localization, Mapping, Camera, Landing Areas

ii

Conteúdo

1 Introdução 11.1 Trabalhos relacionados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.1.1 Mobile Robot Navigation Using Active Vision . . . . . . . . . . . . . . . . . . . . . 31.1.2 Real-Time Simultaneous Localization and Mapping with a Single Camera . . . . . 41.1.3 MonoSLAM: Real-Time Single Camera SLAM . . . . . . . . . . . . . . . . . . . . 41.1.4 Uma Metodologia de Odometria Visual/Inercial e SLAM 3D com um Vant . . . . . 41.1.5 Bearing-Only SLAM for an Airborne Vehicle . . . . . . . . . . . . . . . . . . . . . . 51.1.6 Avoiding Moving Outliers in Visual SLAM by Tracking Moving Objects . . . . . . . 5

1.2 Inovação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3 Estrutura da tese . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Notação e Modelo da Câmara 72.1 Notação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.1.1 Vectores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.1.2 Referenciais Ortogonais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.1.3 Rotação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2 Modelo da Câmara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2.1 Calibração . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2.2 Pontos no Referencial da Câmara e no Plano da Imagem . . . . . . . . . . . . . . 102.2.3 Eliminação da Distorção na Imagem . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 Processamento de Imagem 143.1 Pontos característicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.1.1 Detecção de pontos característicos . . . . . . . . . . . . . . . . . . . . . . . . . . 153.2 Matching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4 Localização da Câmara e Mapeamento em Simultâneo 204.1 Filtro de Kalman e Filtro de Kalman Alargado . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.1.1 Predição . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224.1.2 Actualização . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.2 Implementação do Filtro de Kalman Alargado - Algoritmo de Localização e Mapeamento 234.2.1 Referêncial Principal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244.2.2 Vector de Estado e Matriz de Covariância do Sistema . . . . . . . . . . . . . . . . 244.2.3 Inicialização do Sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 284.2.4 Predição . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.2.5 Predição das medições . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354.2.6 Matching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.2.7 Actualização do estado do sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

iii

4.3 Eliminação de um ponto do sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.4 Factor de Escala . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5 Algoritmo de Detecção de Zonas de Aterragem 405.1 Orientação da câmara no quadrirotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405.2 Detecção de zonas planas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

6 Resultados Experimentais 456.1 Cenários . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 456.2 Factor de Escala . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.3 Resultados experimentais I - Avaliação do método de localização da câmara e mapea-

mento em simultâneo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.3.1 Ensaio experimental I . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.3.2 Ensaio experimental II . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.4 Resultados Experimentais II - Validação e avaliação do algoritmo de detecção de zonasde aterragem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 626.4.1 Fase I: Teste do algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 626.4.2 Fase II: Determinação de zonas planas reais a partir dos ensaios realizados . . . 64

6.5 Discussão dos resultados obtidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

7 Conclusões 687.1 Trabalho futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

Bibliografia 80

iv

Lista de Figuras

2.1 Modelo da Câmara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2 Sequencia de imagens para calibração da câmara . . . . . . . . . . . . . . . . . . . . . . 92.3 Distorção radial e centro geométrico real da imagem . . . . . . . . . . . . . . . . . . . . . 102.4 Representação do ponto P no referencial F da câmara e no plano da imagem . . . . . . 11

3.1 Ponto característico detectado (a vermelho) e respectiva janela de 21x21 pixéis (a cinzento) 143.2 Relação entre os valores próprios da matriz M e a natureza do pixel em análise . . . . . 153.3 Pontos característicos detectados em diferentes cenários . . . . . . . . . . . . . . . . . . 163.4 Processo de matching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4.1 Referêncial principal centrado na posição inicial da câmara, no instante de tempo t0 . . . 244.2 Parametrização do Ponto característico . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.3 Diferentes angulos de paralaxe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274.4 Orientação do raio m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.5 Ambiguidade nas medições . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5.1 Disposição da câmara num quadrirotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415.2 Plano definido a partir de ρ, θ e ϕ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

6.1 Cenário 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 456.2 Cenário 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.3 Posição da câmara real nos instantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo,

t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . 476.4 Pontos obtidos pelo sistema no instante inicial . . . . . . . . . . . . . . . . . . . . . . . . 476.5 Função densidade de probabilidade para as estimações do parâmetro x da câmara, para

t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo et = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

6.6 Função densidade de probabilidade para as estimações dos parâmetros y e z da câmara,para t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundoe t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

6.7 Traço da matriz de covariância P correspondente ao estado do sistema . . . . . . . . . . 496.8 Função densidade de probabilidade para a estimação do inverso da profundidade dos

pontos 1 e 15 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 506.9 Evolução das elipses de covariância para os pontos 1, 3, 7, 12 e 15, nos instantes t = 0.5

segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

6.10 Evolução do movimento da câmara no plano XZ, nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . 52

v

6.11 Evolução do movimento da câmara no plano XY , nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . 53

6.12 Evolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ eguinada ψ, da câmara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6.13 Evolução do movimento da câmara e da posição estimada dos pontos no plano XY nosinstantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5segundo e t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.14 Posição da câmara real nos instantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo,t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . 55

6.15 Pontos obtidos pelo sistema no instante inicial . . . . . . . . . . . . . . . . . . . . . . . . 556.16 Função densidade de probabilidade para as estimações do parâmetro x da câmara, para

t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo et = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6.17 Função densidade de probabilidade para as estimações dos parâmetros y e z da câmara,para t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundoe t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

6.18 Traço da matriz de covariância P correspondente ao estado do sistema . . . . . . . . . . 576.19 Função densidade de probabilidade para a estimação do inverso da profundidade dos

pontos 11 e 16 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 586.20 Evolução das elipses de covariância para os pontos 2, 8, 14 e 20, nos instantes t = 0.5

segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

6.21 Evolução do movimento da câmara no plano XZ, nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . 60

6.22 Evolução do movimento da câmara no plano XY , nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . 60

6.23 Evolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ eguinada ψ, da câmara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

6.24 Evolução do movimento da câmara e da posição estimada dos pontos no plano XY nosinstantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5segundo e t = 3.0 segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

6.25 Nuvem de pontos concentrada em torno de x = 10, com vistas no plano XY , XZ e Y Z,respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

6.26 Nuvem de pontos concentrada em torno de y = 10, com vistas no plano XY , XZ e Y Z,respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

6.27 Nuvem de pontos concentrada em torno de z = 10, com vistas no plano XY , XZ e Y Z,respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

6.28 Nuvem de pontos concentrada em torno de x = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente . . . . . . . . . . . . . . . . . . . . . . . 63

6.29 Nuvem de pontos concentrada em torno de y = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente . . . . . . . . . . . . . . . . . . . . . . . 63

6.30 Nuvem de pontos concentrada em torno de z = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente . . . . . . . . . . . . . . . . . . . . . . . 64

6.31 Zona plana real, determinada para a primeira experiência e pontos estimados, com vistano plano XZ e XY , respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

6.32 Zona plana real, determinada para a segunda experiência e pontos estimados, com vistano plano XZ e XY , respectivamente . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

vi

7.1 Rotação de um corpo rigido (cubo a tracejado) de um angulo θ (a azul) em torno de umvector a, unitário (a amarelo). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

vii

Lista de Tabelas

2.1 Parâmetros Intrínsecos da Câmara . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2 Coeficientes de distorção radial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

6.1 Valores estimados para o parâmetro x da câmara, para t = 0.5 segundo, t = 1.0 segundo,t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . . . . . . 48

6.2 Valores estimados para os parâmetros y e z da câmara, para t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . 49

6.3 Valores estimados para o inverso da profundidade dos pontos 1 e 15, para t = 0.5 se-gundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6.4 Valores estimados para o parâmetro x da câmara, para t = 0.5 segundo, t = 1.0 segundo,t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . . . . . . . 56

6.5 Valores estimados para os parâmetros y e z da câmara, para t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo . . . . . 56

6.6 Valores estimados para o inverso da profundidade dos pontos 11 e 16, para t = 0.5segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

viii

Capítulo 1

Introdução

Os sistemas de localização têm vindo a ser desenvolvidos ao longo do tempo, por forma a facilitarem anavegação dos veículos em que estão inseridos, sejam eles terrestres, marítimos ou aéreos. O GPS,por exemplo, permite que a localização de um determinado veículo num referêncial global seja bemdeterminada, recorrendo para isso a satélites geoestacionários. Todavia este sistema implica oconhecimento prévio do ambiente em que o veículo se insere, caso contrário, as coordenadasgeográficas obtidas através do sistema de nada serviriam. É necessário que o ambiente envolvente seencontre mapeado, e existam pontos de referência bem conhecidos, pois de outra forma, a navegaçãonunca seria possível. Como exemplo tem-se os faróis, que funcionam como guias às embarcaçõesque se encontrem perto da costa, facilitando-lhes a assim a navegação. A premissa de que o mundonão é plano nem vazio, permite afirmar que o deslocamento de um veículo entre um ponto A e umponto B, muito provavelmente não será realizado em linha recta. Há restrições fisicas que sãoimpostas pelo ambiente em si, como a existência de edifícios, zonas montanhosas ou zonas florestaise o conhecimento dessas restrições permite que a navegação possa ser feita sem o mínimo deproblema. Contudo, nem sempre o ambiente onde se navega é conhecido.

A exploração do desconhecido sempre foi uma ambição do Homem. Na era dos descobrimentos,pouco se sabia do mundo que hoje tão bem conhecemos, e a coragem e determinação denavegadores permitiu que terras outrora desconhecidas pudessem finalmente ser descobertas,acrescentando informação a um mapa até então limitado. Contudo, este mapeamento nunca poderiaser realizado se não houvesse pontos de referência, como as estrelas ou o Sol, que permitissem aosnavegadores saberem exactamente a latitude e a longitude a que se encontravam a cada instante.Assim como os navegadores, também uma sonda em Marte desconhece totalmente o ambiente que arodeia. Porém, ao contrário dos navegadores, a sonda, quando no solo, não tem nenhum ponto dereferência que lhe permita saber onde se encontra em cada instante, nem tão pouco pode utilizar umsistema de GPS para se localizar. O principal problema num caso deste tipo reside no facto de alocalização e mapeamento necessitarem do conhecimento mútuo, conhecimento este que não existe àpriori. Todavia, é possível um veículo em ambiente desconhecido localizar-se e simultaneamentemapear a zona envolvente, navegando e explorando zonas inexploradas tal e qual os navegadores ofizeram 500 anos antes.

O SLAM (Simultaneous Localization and Mapping) corresponde a uma técnica, inicialmente propostapor Randall Smith e Peter Cheeseman em 1984. Usualmente aplicada a veículos autónomos, estatécnica permite estimar com alguma precisão a localização de um veículo em cada instante, emambientes desconhecidos, ao mesmo tempo que procede a um mapeamento desse mesmo ambiente.

1

A base desta técnica, probabilística e não exacta, é o filtro de Kalman, que se baseia em mediçõesefectuadas sobre o meio envolvente para estimar a localização do sistema onde se encontraimplementado. Só que neste caso, as próprias medições são elas próprias baseadas em valoresestimados, pois o meio envolvente é totalmente desconhecido. Os pontos de referência, utilizados pelosistema como forma de localização, são obtidos a partir de sensores e têm inicialmente uma grandeincerteza associada. O valor dessa incerteza só irá diminuir à medida que o sistema for evoluindo notempo, permitindo desta forma reduzir também a incerteza associada à localização do sistema. Assim,o meio envolvente deixa de ser totalmente desconhecido e passa a ser parcialmente conhecido.Consideremos o caso de um robot que se desloca dentro de uma habitação, dividida em várioscompartimentos, sem qualquer tipo de informação sobre esta. Através desta técnica, e recorrendo asensores, é possível ao robot mapear o primeiro compartimento e localizar-se dentro dele. Contudo,esse conhecimento não lhe permite saber como são os outros compartimentos nem localizar-sedentro deles tendo unicamente a informação sobre o primeiro. O sistema necessita em cada instantede obter novos pontos de referência, tendo o cuidado de manter sempre todos os outros, que forampreviamente determinados. Apesar de não darem informação sobre o desconhecido, permitem que oque é já conhecido permaneça nessa condição, daí que o conhecimento seja considerado apenasparcial. Da mesma forma, a localização de um sistema deste tipo é sempre uma localização relativaao meio onde se encontra. Se o espaço onde o sistema se encontra corresponder por exemplo a umahabitação, não faz sentido considerar como referêncial principal uma cidade. Geralmente, a origem doreferêncial corresponde à posição inicial do sistema, o ponto inicial de exploração do meio. À sondaem Marte não interessa por exemplo saber a sua posição em relação à Terra.

A detecção de pontos de referência, a partir do meio onde o sistema se encontra, é feita, como já foireferido, por meio de sensores. Desde a sua primeira implementação, diversos sensores têm vindo aser utilizados em sistemas que utilizam esta técnica. Os sensores laser, normalmente equipados emveículos de solo, permitem medir o alcance a que pontos se encontram do sistema, em cada instante,permitindo mapear zonas 2D com grande precisão, pois facilmente detectam paredes ou arestas. Ossensores sonar, usualmente utilizados em ambientes submarinos, permitem por sua vez que o fundodo mar seja mapeado de forma igualmente precisa. Porém um outro sensor tem vindo a ser utilizadomais recentemente permitindo obter uma imagem real do ambiente em que se encontra. A utilizaçãode câmaras como sensores tem vantagens e desvantagens, como qualquer outro sensor considerado.O seu baixo peso, aliado a um baixo preço, permite que sejam facilmente implementadas em qualquersistema. Contudo, apesar de fornecerem informação real em cada instante, a detecção de pontos dereferência nas imagens assim como a obtenção das medições necessárias, só é possível graças àaplicação de técnicas de processamento, efectuadas externamente ao sensor. Esta corresponde àprincipal desvantagem na utilização da câmara face aos outros sensores. Apesar disso, a utilização deuma câmara permite ter uma maior quantidade de informação sobre o meio envolvente do qualqueroutro sensor, pois fornece uma visão do mundo semelhante à que nós, humanos, temos.

O conhecimento visual do meio em que um sistema se insere é muito importante, pois permiteassociar as medições obtidas a cenários reais, com texturas bem definidas. O mapeamento de umazona obtido com um sistema que utiliza uma câmara permite ter mais informação sobre a zona emcausa do que o mapeamento obtido recorrendo exclusivamente a um sensor laser ou um sensorsonar. Desta forma, o reconhecimento do solo, procedimento usualmente executado no ramo daaviação, poderia deixar de ser realizado em duas fases, pois aplicando um sistema deste tipo a umaaeronave seria possível obter imagens e medições do solo, em simultâneo. A aplicação deste sistemaao ramo aerospacial permite solucionar variados problemas e ajudar na navegação, principalmente deveículos não tripulados que voem de forma completamente autónoma. Através de um sistema deste

2

tipo, é permitido a um veículo desta natureza ter acesso à informação sobre a sua localização, assimcomo à disposição espacial dos pontos de referência que utiliza para se localizar, em cada instante.Esta informação, quando precisa e segura, pode ser útil para resolver problemas associados a estetipo de voo, sendo que um dos principais problemas relacionados consiste na aterragem do veículo emambientes totalmente desconhecidos. A capacidade de detectar zonas de aterragem de formacompletamente autónoma permite ao veículo aterrar em condições, sem qualquer tipo de intervençãohumana, seja em que ambiente for.

A determinação de zonas de aterragem por pilotos a bordo de aeronaves corresponde a uma tarefarelativamente fácil, pois mesmo através da observação do solo, este consegue distinguir à primeiravista entre zonas aproximadamente planas e zonas que não apresentem boas características deaterragem. Com a ajuda de instrumentos de navegação a bordo da aeronave, o piloto conseguerealizar uma aterragem segura, sem qualquer tipo de problema. Contudo, quando se trata de umaaeronave não tripulada em voo autónomo, a detecção de possíveis zonas de aterragem apresenta-secomo um problema de maior. Se essa aeronave se encontrar a sobrevoar uma zona desconhecida,nunca antes mapeada, sem efectuar qualquer tipo de monitorização do solo, não tem forma de saberse existe alguma zona do solo que possa ser utilizada como zona de aterragem. Assim, a aplicação deum sistema deste tipo a uma aeronave com estas características, estando a câmara constantementeapontada na direcção do solo, permite que a distância a que os pontos de referência, no solo, seencontram da câmara seja bem determinada, possibilitando desta forma a detecção de zonas deaterragem possíveis de serem usadas como tal.

O trabalho realizado nesta tese consiste exactamente na implementação de um sistema que permitaestimar com precisão o movimento tridimensional efectuado por uma câmara em cada instante,mapear toda a zona envolvente e detectar zonas planas que possam ser consideradas possíveiszonas de aterragem, recorrendo simplesmente às imagens obtidas por esta. O desenvolvimento desteprojecto tem em vista a aplicação do sistema implementado num quadrirotor, permitindo que todo oprocesso de aterragem seja feito sem qualquer tipo de intervenção humana, quer na detecção da zonade aterragem, quer na manobra em si. Contudo, o sistema apresentado visa apenas a detectar aspossíveis zonas de aterragem, sendo que nenhuma lei de controlo foi proposta, que permitisse aoquadrirotor aterrar baseando-se apenas na informação fornecida pelo sistema implementado.

1.1 Trabalhos relacionados

Diversos investigadores têm vindo a desenvolver ao longo dos anos sistemas nesta área, principalmentepara serem usados como sistemas de navegação. Apresentam-se nesta secção algumas das mais bemsucedidas implementações deste tipo de sistemas.

1.1.1 Mobile Robot Navigation Using Active Vision

Este trabalho, desenvolvido por Andrew Davison na Universidade de Oxford, em 1998, para obtenção dograu de Doutor, afigura-se como a primeira implementação bem sucedida de um sistema de navegaçãoautónomo utilizando visão stereo activa, ou seja, duas câmaras que simulavam a visão humana. Estesistema, que funcionava em tempo real, foi implementado num robot que conseguia simultaneamentelocalizar-se num ambiente desconhecido e mapear toda a zona envolvente. Utilizando apenas a infor-mação fornecida pelas duas câmaras, assim como a que lhe era fornecida por odometria, este sistemaera capaz de navegar de forma completamente autónoma guiando-se a partir de pontos de referência

3

no mundo por ele determinados. Utilizando a visão stereo activa, tinha a capacidade de reconhecerzonas já mapeadas, mesmo que a navegação não se efectuasse pelo mesmo trajecto realizado an-teriormente, graças ao extenso mapa que guardava informação sobre todos os pontos de referênciadetectados pelo sistema.

1.1.2 Real-Time Simultaneous Localization and Mapping with a Single Camera

Novamente desenvolvido por Andrew Davison, em 2003, este trabalho consistia na implementação deum sistema em tempo real que permitia determinar com precisão o movimento tridimensional de umasimples câmara e mapear toda zona circundante em simultâneo. Ao contrário do trabalho apresentadona secção 1.1.1, este sistema utilizava apenas uma câmara, mostrando algumas limitações ao nívelda estimação da profundidade a que os pontos se encontravam. O facto de só conseguir obter boasestimações para profundidades próximas da câmara, impossibilitava este sistema de mapear zonasextensas, ficando confinado a espaços fechados e pequenos. Só mais tarde, em 2006, num trabalhoem parceria com José Montiel e Javier Civera, Andrew Davison apresentou pela primeira vez umaparametrização que permitia a este sistema estimar a profundidade de qualquer ponto no espaço, semqualquer limitação. A Parametrização de Profundidade Inversa possibilitava a representação de pontosa partir do inverso da sua profundidade, permitindo que pontos localizados a grandes distâncias dacâmara pudessem ser bem estimados.

1.1.3 MonoSLAM: Real-Time Single Camera SLAM

Este sistema desenvolvido por Andrew Davison, Ian Reid, Nicholas Molton e Olivier Stasse, em 2007,apresenta-se como o primeiro sistema bem sucedido a conseguir descrever o movimento rápido deuma câmara, não controlada, em tempo real e num ambiente desconhecido. O algoritmo apresentadomostra-se extremamente robusto, conseguindo operar a 30Hz com um PC e câmara normais. Paraalém da estimação precisa dos pontos, o sistema implementado permitia também recriar a orientaçãoque estes apresentavam no instante em que foram observados, dando uma outra perspectiva do mundomapeado.

Foi nítida a evolução sofrida por este sistema ao longo dos anos de investigação e desenvolvimento. Osistema presente nesta secção apresenta-se como a principal base do sistema que foi implementadonesta tese, permitindo reproduzir o movimento realizado pela câmara e mapear toda a zonaenvolvente em simultâneo.

1.1.4 Uma Metodologia de Odometria Visual/Inercial e SLAM 3D com um Vant

Neste trabalho, desenvolvido por Luiz Mirisola e Jorge Dias, é apresentada uma nova metodologia quepermite determinar o movimento translaccional de um sistema, por odometria, a partir da informaçãosobre o solo, fornecida por uma câmara acopolada. Este sistema necessita contudo que a câmarase apresente em todos os instantes, paralela ao solo, por forma a que este se apresente sempre nahorizontal, baseando-se no facto, errado, de que o solo é sempre plano. As limitações do sistemasão claramente visíveis quando este opera junto a um solo que apresenta irregularidades, onde asestimações da translacção horizontal e vertical não podem ser obtidas a partir das equações por elesapresentadas. Porém, em terrenos aproximadamente planos, a aproximação é boa o suficiente paraproduzir bons resultados, permitindo inclusivamente que navegação seja efectuada recorrendo a estesistema.

4

1.1.5 Bearing-Only SLAM for an Airborne Vehicle

Este projecto, desenvolvido por Mitch Bryson e Salah Sukkarieh, consiste na implementação de umsistema em tempo real de localização e mapeamento num UAV (Unmanned Aerial Vehicle), que utilizaapenas uma câmara e um sensor inercial a bordo. Contudo a inicialização de cada ponto do sistemaimplica que o ângulo de paralaxe obtido a partir do deslocamento do UAV seja de aproximadamente40o, o que não permite que estes sejam adicionados ao sistema no instante em que são observados.Este sistema não tem capacidade de estimar a profundidade a que pontos se encontram desde oinstante em que são detectados devido à representação que é utilizada para cada ponto, semelhanteà inicialmente usada por Andrew Davison no trabalho descrito na secção 1.1.2. Assim, os pontos dereferência utilizados correspondem apenas aqueles que se encontram bem determinados, com umaincerteza inicial bastante reduzida ou praticamente nula. O sistema necessita, desta forma, de utilizarinicialmente a informação fornecida pelo sensor inercial para estimar a sua localização enquanto ainformação dada pelos pontos não se encontra disponível, ao contrário do sistema implementado nestatese, que vai estimando a localização da câmara e dos pontos à medida que este vai evoluindo notempo.

1.1.6 Avoiding Moving Outliers in Visual SLAM by Tracking Moving Objects

Este trabalho, desenvolvido por Somkiat Wangsiripitak e David Murray, em 2009,é recente e apresentauma implementação bem sucedida de um sistema que é capaz de determinar o movimento efectuadopor uma câmara no espaço ao mesmo tempo que efectua seguimento de um objecto tridimensionalem movimento detectado pela câmara. Ao contrário de todos os outros sistemas até aqui abordados,neste caso não é assumido que o ambiente envolvente se encontra estático, mas que é dinâmico. Estesistema é capaz de detectar pontos de referência no espaço, mesmo estando ocultos por um objectoem movimento, mostrando a influência que a ocultação de pontos tem na estimação da posição dacâmara em cada instante.

1.2 Inovação

Os trabalhos referidos na secção 1.1 correspondem aos mais recentes desenvolvimentos de sistemasque utilizam a visão para navegar ou simplesmente mapear zonas desconhecidas. Contudo ficou visívelque em nenhum destes projectos, ainda que não correspondendo à totalidade dos projectos desenvolvi-dos nesta área, o sistema implementado é aplicado para determinar zonas planas no mundo. Esta teseapresenta-se assim inovadora na medida em que permite que um sistema deste tipo seja utilizado nãosó para determinar o movimento de uma câmara no espaço mas também para detectar zonas planas nomundo a partir da localização estimada dos pontos de referência utilizados. Este sistema tem ainda acapacidade de avaliar as zonas planas detectadas e determinar quais destas podem ser consideradaspossíveis zonas de aterragem para veículos aerospaciais, de uma forma bastante precisa.

A aplicação deste sistema a um veículo aerospacial aumenta de forma considerável a sua autonomiapois não seria necessária uma constante monitorização do solo por parte de um operador para queeste conseguisse aterrar em condições. Apesar de tudo, este sistema determina apenas zonas deaterragem, não tendo sido desenvolvido nenhum sistema de controlo que utilizasse apenas ainformação disponibilizada para proceder à aterragem do veículo.

5

1.3 Estrutura da tese

A tese apresenta-se repartida em 7 capítulos, sendo que a sua estrutura foi escolhida de forma a quea informação presente em cada capítulo seja aplicada posteriormente nos capítulos seguintes. A suaestrutura é então dada por:

• Capítulo 2: Introduz toda a notação utilizada na tese. Apresenta também o modelo da câmarautilizada, descrevendo ainda o processo de calibração da câmara, assim como o processo deremoção e adição de distorção a uma imagem.

• Capítulo 3: Descreve técnicas de processamento de imagem, tais como a detecção de pontoscaracterísticos, ou de referência, numa imagem e o processo de matching.

• Capítulo 4: Corresponde a uma descrição de toda a implementação do método que permite de-terminar, em cada instante, o movimento efectuado pela câmara e a localização dos pontos dereferência do sistema.

• Capítulo 5: Apresenta o algoritmo que permite detectar zonas planas, a partir dos pontos dereferência obtidos pelo sistema, que possam ser consideradas zonas de aterragem.

• Capítulo 6: Contém todos os resultados experimentais, realizados para testar e avaliar os algorit-mos implementados.

• Capítulo 7: Conclui a tese, apresentando todas as elações retiradas desta e apresenta propostasde trabalho futuro nesta área.

6

Capítulo 2

Notação e Modelo da Câmara

Neste capítulo é feita uma breve descrição da notação usada na tese. É também feita uma descrição domodelo que representa a câmara utilizada na simulação, assim como do processo de calibração desta,da representação de pontos no referencial da câmara e plano da imagem e eliminação de distorção daimagem.

2.1 Notação

A descrição da notação usada incide sobre vectores, referenciais ortogonais e rotação de vectores entrereferenciais.

2.1.1 Vectores

Quando nesta tese é feita referência a um vector sem qualquer tipo de índice, indica simplesmente umvector genérico no espaço tridimensional com uma certa orientação. Este vector será independente detodo e qualquer referencial que tenha sido referido até então. Assim é possível escrever equações dotipo:

a = b+ c (2.1)

sem com isso ter de se definir um sistema de coordenadas. Contudo, para uma melhor compreensão,podem ser usados índices inferiores em alguns casos, nomeadamente quando um vector é específicode uma certa entidade, como é o caso de vc que corresponde ao vector velocidade da câmara, porexemplo.

2.1.2 Referenciais Ortogonais

Tais como os vectores, são também referidos nesta tese vários referenciais, tais como o correspondenteà câmara ou à Terra. Por se tratarem de referenciais diferentes, é necessário distingui-los para umamelhor compreensão, pois poderá eventualmente aparecer o mesmo vector representado em dois oumais referenciais. O índice superior em cada vector representa assim o referencial no qual esse vectorse encontra definido. Por exemplo, a seguinte notação:

aF =

aFx

aFy

aFz

(2.2)

7

representa o vector a definido no referencial ortogonal F . É igualmente possível escrever equaçõescom este tipo de notação, indicando o referencial onde estes vectores se encontram definidos:

aF = bF + cF (2.3)

2.1.3 Rotação

Por vezes é necessário representar a orientação de um vector em diferentes referenciais e para issoaplica-se uma rotação ao vector original. A notação correspondente à matriz de rotação aplicada a umvector que o coloca noutro referencial é dada por RFG, onde os índices superiores correspondem aoreferencial de destino e ao referencial original, respectivamente. Assim, a transformação do vector aG

no vector aF é dada por:

aF = RFGaG (2.4)

Note-se que a transformação inversa também é possível, sendo que para isso basta aplicar a matriz derotação inversa, que neste caso, corresponde à inversa da matriz original:

aG = RGFaF (2.5)

onde

RGF = (RFG)−1 (2.6)

Estas transformações não representam qualquer tipo de movimento do vector. São essencialmenterepresentações do mesmo vector em diferentes referenciais.

2.2 Modelo da Câmara

Todas as câmaras são caracterizadas por um conjunto de parâmetros intrínsecos que descrevem osseus modelos. O centro óptico de uma câmara representa a origem do referencial F dessa câmara,cujos eixos X e Y são ortogonais e paralelos ao plano da imagem e o eixo Z se encontra alinhado como centro do plano da imagem, de coordenadas (u0, v0). Por sua vez, o plano da imagem encontra-seseparado do centro óptico por uma distância f , denominada distância focal. Todos estes parâmetros,comuns a todas as câmaras, encontram-se representados na figura 2.1:

Figura 2.1: Modelo da Câmara

8

Contudo, estes parâmetros variam de câmara para câmara. A distância focal não é constante e ocentro do plano da imagem nunca corresponde ao centro geométrico desta. Por estes motivos énecessário sempre uma calibração prévia da câmara para que todos estes parâmetros sejamdeterminados convenientemente.

2.2.1 Calibração

A calibração da câmara é essencial para se conseguir bons resultados na determinação das coorde-nadas de pontos, tanto no plano da imagem como no referencial da câmara. Para a determinação dosparâmetros tirou-se cerca de 20 imagens de um padrão de xadrez, com diferentes perspectivas, talcomo mostra a figura 2.2:

Figura 2.2: Sequencia de imagens para calibração da câmara

Aplicando estas imagens ao programa desenvolvido por Jean-Yves Bouguet para calibração dacâmara, apresentado em [1], foi possivel obter todos os parâmetros intrínsecos necessários, a partirdo conhecimento das dimensões de cada rectângulo presente no padrão, sendo que os seus valoresse encontram definidos na tabela 2.1:

Parâmetros Intrínsecos da Câmara Valor (em pixéis)Distância focal f1 358.60Distância focal f2 381.72

Coordenada u0 do centro da imagem 167.51Coordenada v0 do centro da imagem 112.01

Tabela 2.1: Parâmetros Intrínsecos da Câmara

A não idealidade dos parâmetros faz com que a distância focal tenha duas componentes em vez deuma só, devido à distorção tangencial ou descentramento do centro do plano da imagem, que nãocorresponde ao centro geométrico, cujas coordenadas ideais para uma imagem de 320× 240 pixéisseriam u0 = 160 e v0 = 120.

9

Para além de permitir determinar os parâmetros já referenciados, a calibração é também útil namedida em que minimiza os efeitos causados pelas lentes das câmaras, que provocam algumadistorção na imagem. Uma lente perfeita numa câmara representaria uma linha recta como ela o é narealidade, ou seja, uma linha recta sem qualquer tipo de distorção. Porém, por estas não seremperfeitas, uma linha recta poderia aparecer como uma linha curva caso não cruzasse o centrogeométrico da imagem, devido a distorção radial, ou deslocada da sua real posição devido a distorçãotangencial. A figura 2.3 representa de uma forma exagerada, apenas para uma melhor clarificação, aslinhas distorcidas da imagem devido à distorção radial, assim como o centro geométrico real daimagem (a vermelho) deslocado do centro geométrico ideal:

Figura 2.3: Distorção radial e centro geométrico real da imagem

Desta forma, a calibração permite obter também os coeficientes de distorção radial (k1, k2) que irãopermitir posteriormente representar cada um dos pontos na imagem sem qualquer tipo de distorção.Os coeficientes encontram-se definidos na tabela 2.2:

Coeficientes de distorção radial Valork1 −0.54197k2 0.66424

Tabela 2.2: Coeficientes de distorção radial

2.2.2 Pontos no Referencial da Câmara e no Plano da Imagem

Até ser mencionado em contrário, considera-se que a distancia focal f é constituída por uma só com-ponente, sendo que o seu valor se encontra definido em metros. A projecção de um ponto P no planoda imagem da câmara encontra-se ilustrada na figura 2.4:

10

Figura 2.4: Representação do ponto P no referencial F da câmara e no plano da imagem

Considerando o ponto P, este pode ser representado através de um vector h, definido a partir daorigem do referencial F . Assim, o vector h tem componentes dadas por:

hF =

hFx

hFy

hFz

(2.7)

Este vector intersecta o plano da imagem num ponto p, que irá assim corresponder à imagem do pontoconsiderado, dando origem a um novo vector p, paralelo a h mas mais curto por um factor f/hFz . Noreferencial F , as componentes do vector p são dadas por:

pF =

xc

yc

f

=f

hFzhF (2.8)

onde xc e yc são as coordenadas do ponto p, em metros, definidas no referencial F da câmara. Contudo,o plano de uma imagem real é definido por pixéis e portanto, é necessário converter as coordenadasdeste ponto no referencial F para as unidades correspondentes. Admitindo que os pixéis têm umespaçamento de ku pixéis/metro na direcção horizontal e de kv pixéis/metro na direcção vertical, entãoo ponto p pode ser representado no plano da imagem, dadas as coordenadas (u0, v0), a partir dasseguintes equações:

u = u0 − kuxc (2.9)

v = v0 − kvyc (2.10)

Estas equações podem ser reescritas de outra forma:

u

v

1

=

−ku 0 u0f

0 −kv v0f

0 0 1f

xc

yc

f

(2.11)

e se

xc

yc

f

= pF , então:

11

u

v

1

=

−ku 0 u0f

0 −kv v0f

0 0 1f

pF (2.12)

Como pF ∝ fhF , então: u

v

1

∝

−fku 0 u0

0 −fkv v0

0 0 1

hF (2.13)

Como a distância focal é constituída por duas componentes f1 e f2, definidas na tabela 2.1, então aequação 2.13 vem escrita da seguinte forma: u

v

1

∝

−f1ku 0 u0

0 −f2kv v0

0 0 1

hF (2.14)

Esta equação permite assim obter as coordenadas de um ponto no plano da imagem a partir da local-ização tridimensional de um ponto característico no referencial F da câmara. A matriz responsável poresta transformação designa-se Matriz de Calibração da Câmara, C. Assim a equação 2.14 pode serescrita da seguinte forma: u

v

1

∝ ChF (2.15)

com C definida por:

C =

−f1ku 0 u0

0 −f2kv v0

0 0 1

(2.16)

A transformação inversa é igualmente fácil de obter, ou seja, a obtenção das coordenadas do ponto Pno referencial F da câmara a partir das coordenadas do mesmo ponto no plano da imagem. A matrizque permite essa transformação corresponde à inversa da matriz C:

hF ∝ C−1

u

v

1

(2.17)

com C−1 definida por:

C−1 =

−1f1ku

0 u0f1ku

0 −1f2kv

v0f2kv

0 0 1

(2.18)

Contudo, para que estas relações estejam correctas é necessário que as coordenadas dos pontos naimagem não contenham nenhum tipo de distorção. A eliminação da distorção na imagem é descrita nasubsecção seguinte.

12

2.2.3 Eliminação da Distorção na Imagem

Tal como já foi referido na subsecção 2.2.1, a distorção pode influenciar bastante a representação depontos. A calibraçao feita permitiu assim obter os coeficientes de distorção radial, (k1, k2) que serãousados para a remover da imagem. Sejam (ud, vd) as coordenadas de um ponto no plano da imagem,com distorção. Então, as coordenadas do mesmo ponto, sem distorção são dadas por:

uud = u0 + (ud − u0)(1 + k1.r

2d + k2.r

4d

)(2.19)

vud = v0 + (vd − v0)(1 + k1.r

2d + k2.r

4d

)(2.20)

onde rd é dado por:

rd =

√(ud − u0

ku

)2

+(vd − v0kv

)2

(2.21)

Desta forma, é possível saber as coordenadas de qualquer ponto na imagem sem distorção.

Porém, por vezes é necessário representar pontos no plano da imagem a partir das suas coordenadasno referencial F da câmara. Como essas coordenadas são obtidas considerando que não hádistorção, para que a representação na imagem seja correcta, é necessário adicionar posteriormentedistorção a cada ponto. Assim, o processo inverso ao apresentado nas equações 2.19 e 2.20encontra-se descrito nas seguintes equações:

ud =(uud − u0)

(1 + k1.r2d + k2.r4d)+ u0 (2.22)

vd =(vud − v0)

(1 + k1.r2d + k2.r4d)+ v0 (2.23)

onde rd, neste caso, é dado por:

rd =ru

(1 + k1.r2u + k2.r4u)(2.24)

com ru dado por:

ru =

√(ud − u0

ku

)2

+(vd − v0kv

)2

(2.25)

Desta forma é possível representar qualquer ponto no plano da imagem tendo em conta a distorçãoinerente.

13

Capítulo 3

Processamento de Imagem

Neste capítulo é descrito todo o procedimento necessário para extrair pontos característicos das ima-gens, pontos fixos no espaço e invariantes no tempo, que servem de referência para determinar qualo movimento efectuado pela câmara em cada instante. É também feita uma introdução e descriçãodo processo de matching, a procura de pontos característicos já conhecidos na imagem, que seránecessário numa fase posterior do projecto.

3.1 Pontos característicos

Para que se possa localizar no Mundo que a envolve, a câmara precisa em cada instante de pontosde referência, pontos característicos, cujas coordenadas são conhecidas e que funcionam como guias,orientando-a. Mas mais importante, é necessário que esses pontos sejam fixos no espaço, que assuas coordenadas não se alterem ao longo do tempo e que tenham a capacidade de ser detectadosindependentemente do ângulo ou da distância a que estão a ser observados pela câmara. A detecçãode pontos característicos com estas particularidades é assim um dos pontos importantes desta tese.Apesar de os pontos característicos detectados surgirem como pontos isolados na imagem, estes sãosempre representados por janelas de 21×21 pixeis em torno do ponto característico detectado, tal comomostra a figura 3.1:

Figura 3.1: Ponto característico detectado (a vermelho) e respectiva janela de 21x21 pixéis (a cinzento)

Isto permite reduzir o erro na fase de matching, como se verá mais à frente, pois estas janelas sãomais facilmente detectáveis e contém uma maior quantidade de informação. Se numa imagem podem

14

surgir variados pontos com iguais características às do ponto detectado, o mesmo não acontece com ajanela considerada. Contudo, a dimensão da janela e o tempo de processamento variam em sentidosopostos pelo que se optou por estas dimensões e não outras.

3.1.1 Detecção de pontos característicos

A detecção de pontos característicos é feita usando o método descrito em [2], e que foi proposto porChris Harris e Mike Stephens em 1988. O método descrito faz uso dos valores de intensidade quecaracterizam cada pixel da imagem e para efeitos de processamento, todas as imagens adquiridas pelacâmara foram convertidas para escalas de cinzento, em vez do formato original RGB. Isto permite tra-balhar com uma só matriz em vez das três que caracterizam cada uma das cores da imagem (vermelho,verde e azul).

É necessário que os pontos característicos detectados sejam invariavelmente cantos na imagem e nãoarestas ou zonas lisas, e este método permite que os cantos fiquem bem caracterizados. A zona deprocura de pontos característicos corresponde não à imagem toda mas sim apenas a uma áreacentral, separada 25 pixéis da margem. Assim, em vez de se utilizar uma imagem de 320× 240 pixéis,usa-se uma mais reduzida de 270× 190 pixéis, pois interessa que a maior parte dos pontos detectadospertença à zona central da imagem, conseguindo desta forma uma redução na velocidade deprocessamento. O primeiro passo deste método consiste em determinar a resposta de cada pixel avariações de intensidade da imagem, gradientes, segundo as direcções horizontal, gx e vertical, gy. Adeterminação dos gradientes permite obter uma matriz M , de dimensão 2× 2, simétrica, para cadapixel i definida por:

Mi =

[g2xi

gyigxi

gxigyi

g2yi

](3.1)

Esta matriz é de extrema importância na medida em que os seus valores próprios, αi e βi permitemsaber qual a natureza do pixel i analisado. A figura 3.2 mostra a relação entre os valores próprios damatriz e a natureza do pixel em análise:

Figura 3.2: Relação entre os valores próprios da matriz M e a natureza do pixel em análise

15

Se αi e βi forem ambos próximos de zero, então trata-se de uma zona lisa, ao passo que se αi forgrande e βi for pequeno (ou vice-versa) se trata de uma aresta. Apenas quando ambos são grandesse considera que o ponto corresponde a um canto na imagem. Contudo, é necessário encontrar umafunção que analise a qualidade da resposta obtida, que não permita qualquer tipo de ambiguidades nadeterminação de cantos na imagem. Essa função, representada por R, e que depende unicamente deαi e βi, permite assim obter curvas de amplitude constante, que se encontram representadas na figura3.2 a cor amarela.

A função R, para cada pixel i, é definida por:

Ri = Det (Mi)− kTr (Mi) 2 (3.2)

onde:

Tr (Mi) = αi + βi = g2xi

+ g2yi

(3.3)

Det (Mi) = αi.βi = g2xi.g2yi− (gxigyi)

2 (3.4)

e onde k corresponde a uma constante de sensibilidade do detector cujo valor varia normalmente entre0.04 e 0.06, para imagens com níveis de brilho entre 0 e 255. Quanto menor for o valor desta constante,maior sensibilidade terá o detector para detectar pontos característicos. O valor da amplitude destascurvas permite assim saber se o pixel em análise se trata de um canto (se a curva tiver amplitudepositiva), de uma aresta (se a curva tiver amplitude negativa) ou de uma zona lisa (se a curva tiveramplitude muito pequena, próxima de zero). Quanto maior for o valor da amplitude de R (positivo),melhor será a qualidade do canto analisado e por conseguinte, do ponto característico obtido. Paraevitar entrar numa zona de ambiguidade, com valores baixos próximos de zero, define-se um limiteinferior para o valor da amplitude de R, (igualmente positivo), acima do qual, qualquer valor consideradoserá muito provavelmente um canto na imagem. Assim garante-se a qualidade de todos os pontoscaracterísticos obtidos.

Após ser detectado um ponto característico, define-se uma zona de vizinhança em torno deste,separada 20 pixéis em cada direcção, que irá evitar o aparecimento de dois pontos característicoscontíguos. A figura 3.3 mostra os pontos característicos detectados e correspondentes janelas, avermelho, em 4 situações diferentes. Definiu-se como 10 o número máximo de pontos característicos adetectar em cada caso.

Figura 3.3: Pontos característicos detectados em diferentes cenários

16

Por vezes o número de pontos característicos na imagem não corresponde ao inicialmente desejado.Isto ocorre quando o número de pontos cujo valor da amplitude de R ultrapassa o limite imposto éinferior ao número de pontos característicos que se deseja detectar na imagem. Caso não houvesseesta limitação, poderia haver pontos característicos que não tinham as características pretendidas eque acabavam por ser considerados, apenas para perfazer o número desejado inicialmente, nãoacrescentando nada de novo em termos de informação, pois rapidamente se perderiam, contribuindopara um maior tempo de processamento e para a degradação dos resultados dos processossubsequentes.

3.2 Matching

A detecção de pontos característicos na imagem utilizando o método já descrito é feita apenas em doiscasos. Numa fase inicial da simulação, onde nenhum ponto característico é ainda conhecido e numaoutra fase, onde é necessário obter novos pontos, de modo a que o número de pontos característicosvisíveis pela câmara se mantenha sempre acima do valor pré-determinado. Em todos os outros casosé possível prever qual a localização do ponto característico no instante seguinte e determinar a sualocalização exacta através do processo de matching.

Cada ponto característico i tem associado uma janela real, patchireal, como mostra a figura 3.4a), cuja

localização real no instante seguinte, ou na imagem seguinte, irá muito provavelmente incidir sobreuma zona em torno da predição da sua localização, definida a amarelo na figura 3.4b), considerandoque entre duas imagens contínuas o deslocamento efectuado pela câmara não é grande. A partirdesta predição, que será explicada no Capítulo 4, é possível gerar novas janelas, designadas depatchipred

, que consistem em sub-imagens de 21× 21 pixéis, centradas em torno de cada um dospontos pertencentes à zona de predição. Se a zona de predição abranger por exemplo uma área de10× 10 pixéis em torno do ponto característico, serão nesse caso gerados 100 janelas deste tipo.

(a) (b) (c)

Figura 3.4: Processo de matching

Estas novas janelas podem conter ou não o ponto característico detectado inicialmente, poiscorrespondem apenas a predições. Desta forma, é necessário proceder a uma correlação cruzadaentre cada um dos patchipred

e o patchireal, para que se possa encontrar o ponto característico na nova

17

imagem. O valor obtido através da correlação cruzada será tanto maior quanto maior for a semelhançaentre as janelas consideradas, sinal de que se trata muito provavelmente da mesma. Porém énecessário garantir que entre imagens não há factores externos que possam ter influência directa nacorrelação cruzada. Como o brilho entre imagens pode variar bastante devido à luminosidade ou àexposição a que a câmara é sujeita em cada instante, é necessário normalizar cada uma das janelas,sendo que a normalização é feita da seguinte forma:

patchnormireal(u, v) =

(patchireal

(u, v)− patchireal

)σireal

(u, v)(3.5)

patchnormipred(u, v) =

(patchipred

(u, v)− patchipred

)σipred

(u, v)(3.6)

onde patchireale patchipred

correspondem aos valores médios de intensidade de cada uma das janelas,determinados a partir das seguintes equações:

patchireal=

∑u,v patchireal

(u, v)Nt

(3.7)

patchipred=

∑u,v patchipred

(u, v)Nt

(3.8)

com Nt correspondente ao numero de pixéis totais de cada janela e onde σireal(u, v) e σipred

(u, v)correspondem aos desvios padrão dos valores de intensidade ao longo de cada linha, obtidos a partirdas equações seguintes:

σireal(u, v) =

√√√√∑Nl

i=1

(patchireal

(u, i)− patchireal(u))2

Nl(3.9)

σipred(u, v) =

√√√√∑Nl

i=1

(patchipred

(u, i)− patchipred(u))2

Nl(3.10)

onde Nl corresponde ao numero de pixéis por linha de cada janela e patchireal(u) e patchipred

(u)equivalem aos valores médios de intensidade por linha de cada uma das janelas. Desta forma, épossível definir a correlação cruzada, CRC, de uma forma menos sensível a factores externos, a partirda equação seguinte:

CRC =∑u,v

patchnormireal(u, v)× patchnormipred

(u, v) (3.11)

De acordo com esta equação, cada ponto pertencente à zona de predição terá um valor de CRC associ-ado. Se o valor máximo, entre todos os valores obtidos, for superior a um determinado limite estipulado,considera-se que para esse ponto houve correspondência entre janelas, sinal de que o ponto carac-terístico existe e foi encontrado na imagem seguinte, tal como mostra a figura 3.4c). Este processo, queé aplicado a todos os pontos característicos inicialmente determinados, permite determinar posterior-mente o deslocamento real efectuado pela câmara, representados na figura 3.4c) a vermelho, assimcomo a nova localização do ponto característico no referencial da câmara e no referencial Terra.

Se a normalização das janelas não fosse feita, poderia haver valores de CRC muito grandes parapontos cujas janelas seriam maioritariamente constituídas por valores de intensidade muito altos, istoé, onde haveria uma predominância da cor branca. Como o valor CRC seria muito alto, esse ponto

18

poderia ser considerado erradamente como o ponto característico em procura, o que se traduzirianuma má medição. Através da normalização, consegue-se que uma janela desta natureza fique comvalores muito baixos, permitindo que o maior valor de CRC obtido seja efectivamente ocorrespondente ao ponto característico certo.

19

Capítulo 4

Localização da Câmara e Mapeamentoem Simultâneo

Na presença de um ambiente estranho, e não conhecendo nada sobre o meio em que se encontra, umser humano tem a capacidade de se integrar nesse mesmo meio apenas por observação e exploraçãodeste. Assim, o que antes era considerado desconhecido, aos poucos torna-se familiar. O uso damemória como ferramenta é fulcral para que consiga distinguir entre zonas já exploradas e zonas porexplorar, permitindo que navegue sem nunca ter medo de se perder. Imaginemos o caso de um serhumano, abandonado numa floresta e nada sabendo sobre esta. Com o passar do tempo, e apósexplorar toda a zona envolvente, ele passa a conhecer o espaço onde se encontra, pois criará nasua mente pontos de referência que lhe permitem localizar-se em cada instante. Este processo delocalização e mapeamento da zona circundante, facilmente realizado por um ser humano, tem vindoa ser desenvolvido ao longo dos anos para aplicação a sistemas autónomos, que não necessitem denenhuma informação à priori para conseguirem navegar em ambientes desconhecidos.

Tal como o ser humano utiliza os sentidos, também estes sistemas necessitam de sensores quepermitam uma interacção com o meio em que se encontram. Através dessa interacção, é possível aossistemas obterem pontos de referência que lhes permitam realizar uma navegação segura, tendoconhecimento em cada instante da sua localização, assim como de tudo o que os rodeia. A base destemétodo é o filtro de Kalman, que permite obter estimações da localização do sistema em cada instante,assim como a localização espacial dos pontos de referência utilizados pelo sistema para se localizar.

O primeiro objectivo desta tese consistiu em implementar um sistema que permitisse, utilizando comosensor apenas uma câmara sem qualquer tipo de informação externa, estimar com precisão omovimento realizado por esta, assim como a localização dos pontos de referência obtidos através doprocessamento das imagens captadas ao longo do tempo. Neste capítulo é feita uma descrição doalgoritmo do Filtro de Kalman e do Filtro de Kalman Alargado. São ainda descritos todos os processosde implementação do sistema, que permitem obter a localização da câmara em cada instante esimultaneamente mapear toda a zona envolvente.

4.1 Filtro de Kalman e Filtro de Kalman Alargado

O Filtro de Kalman é um algoritmo que permite obter uma estimação estatisticamente óptima, mediantedeterminadas condições, da evolução de sistemas físicos variantes no tempo, na presença de ruído.

20

Considera-se que o sistema é modelado através de um vector de estado x, cujas entradascorrespondem às variáveis que se pretendem estimar em cada instante k e onde a evolução que seespera do sistema em cada instante de tempo se encontra representada na função de transição deestado f , que depende das entradas u do sistema e do estado do sistema x, e que tem um certo ruídow associado. Considera-se ainda que as medições z são obtidas em cada instante como função dovector de estado, a partir da função de medição h, que depende igualmente do estado x do sistema eque se encontra afectada de ruído, v. Desta forma, as equações que definem a dinâmica global dosistema vêm dadas por:

x (k + 1) = f (x (k) , u (k) , w (k)) (4.1)

z (k) = h (x (k) , v (k)) (4.2)

Incorporando a informação proveniente das medições assim como das entradas do sistema em cadainstante, permite ao filtro estimar de uma forma eficiente o seu estado, à medida que este evolui notempo. Obtem-se então uma estimação do estado x do sistema, sendo que a cada entrada estimadacorresponde um certo valor de incerteza. Os valores dessas incertezas encontram-se representadosnuma matriz quadrada, simétrica, com dimensão igual ao número de elementos constituintes de x,denominada matriz de covariância P . Estes valores de incerteza tendem a diminuir com o tempo e sea dependência de f e h com x for linear e o ruído em cada transição de estado e medição for bemmodelado por uma distribuição Gaussiana, então a estimativa fornecida pelo filtro é considerada umaestimativa óptima. O estado do sistema no instante seguinte pode ser assim aproximado por umadistribuição Gaussiana, centrada no valor da sua estimação x para esse instante, com uma variânciadada por P :

p (x (k) | z (k) , u (k − 1) , x (k − 1)) ∼ N (x (k | k) , P (k | k)) (4.3)

onde:

x (k | k) = E [x (k) | z (k) , u (k − 1) , x (k − 1)] (4.4)

P (k | k) = E[(x (k)− x (k | k)) (x (k)− x (k | k))T | z (k) , u (k − 1) , x (k − 1)

](4.5)

A equação 4.3 corresponde à função densidade probabilidade, que permite determinar o valoresperado e a incerteza de x num certo instante k, dado que se conhece o valor da medição z nesseinstante, as entradas u do sistema e o estado do sistema x no instante anterior.

Este filtro tende desta forma a minimizar o erro de estimação do estado do sistema x (k | k) em cadainstante, erro este que corresponde à diferença entre o estado real num determinado instante k e apredição obtida a partir do filtro para o estado do sistema nesse mesmo instante e que se encontrarepresentado na equação 4.5 e que vem dado por:

x (k | k) = x (k)− x (k | k) (4.6)

A linearidade do sistema e a possibilidade de modelar o ruído do sistema como uma distribuiçãonormal são condições necessárias para uma boa estimação por parte do Filtro de Kalman. Contudo, a

21

maior parte dos sistemas físicos existentes não podem ser descritos a partir de um modelo linear, nemo ruído inerente pode ser modelado através de uma distribuição Gaussiana. Desta forma, o Filtro deKalman Alargado surge como uma alternativa ao Filtro de Kalman na medida em que permite obtermelhores estimações para sistemas com estas características, apesar de essas estimações nãoserem óptimas. Este sistema assume que o ruído é Gaussiano, de média nula e tende a linearizar asfunções de transição de estado e medição do sistema em torno da estimação anterior e actual,respectivamente. Isto permite que as funções densidade probabilidade condicionada do sistema, paracada instante, possam ser aproximadas a funções Gaussianas, tal como acontece na equação 4.3.Contudo, este filtro baseia-se em aproximações, que tanto podem descrever a evolução do sistema deuma forma correcta, como também podem dar uma descrição errada desta. Por este motivo, o filtrotende a divergir se as constantes linearizações do sistema não corresponderem a boas aproximaçõesdeste. Para além deste aspecto, há um outro igualmente importante a ser considerado. A covariânciado estado do sistema não corresponde à verdadeira covariância deste, mas sim à covariância daaproximação feita. Todos estes aspectos necessitam ser tomados em conta quando se implementa umfiltro de Kalman alargado num sistema não linear. Apesar disso, devido à não linearidade do sistemaem causa, o Filtro de Kalman Alargado apresentou-se como a melhor escolha, permitindo assim amelhor estimação possível do estado do sistema.

O Filtro de Kalman Alargado, assim como o Filtro de Kalman, podem ser descritos em duas fases.Uma primeira fase de predição onde se estima o estado seguinte sabendo apenas a estimação doestado anterior, sem qualquer tipo de medição e uma segunda fase de actualização onde se usam asmedições já efectuadas nessa fase para reduzir a incerteza e actualizar o valor da estimação.

A descrição das duas fases que caracterizam o filtro de Kalman Alargado, apresentada nassubsecções 4.1.1 e 4.1.2, é feita assumindo que o sistema considerado não recebe qualquer tipo deentradas u, o que não só facilita na análise como serve de introdução ao tipo de implementaçãorealizada nesta tese, por aproximação dos modelos considerados, como se verá mais à frente. Tendoem conta esta consideração, as equações 4.1 e 4.2 podem ser reformuladas da seguinte forma:

x (k + 1) = f (x (k) , w (k)) (4.7)

z (k) = h (x (k) , v (k)) (4.8)

onde se assume que os ruídos inerentes ao processo de transição e medição, w (k) e v (k)respectivamente, são brancos Gaussianos de média nula e caracterizados pelas matrizes decovariância correspondentes, Q (k) e R (k), dadas por:

E[v (k) v (k)T

]= R (k) , E

[w (k)w (k)T

]= Q (k) (4.9)

4.1.1 Predição

Tendo em conta a equação 4.7, e considerando que nenhuma medição é efectuada neste passo, épossivel obter uma estimativa do estado do sistema no instante de tempo (k + 1) através da função detransição de estado f e sabendo apenas o estado do sistema no instante anterior. Da mesma forma, amatriz de covariância do estado do sistema P no instante seguinte irá sofrer alterações em relação aoinstante anterior, devido ao aumento das incertezas, por influência do ruído presente na transição de

22

estado. Assim, a estimativa do estado do sistema e da matriz de covariância no instante seguinte sãodadas genericamente pelas seguintes equações:

x (k + 1 | k) = f (x (k | k)) (4.10)

P (k + 1 | k) =(∂f

∂x

)|x(k|k)

P (k | k)(∂f

∂x

)T|x(k|k)

+Q (k) (4.11)

onde(∂f∂x

)|x(k|k)

corresponde à jacobiana da transição de estado, linearizada em torno da estimação

do estado do sistema x (k | k) no instante actual, e contendo a variação da função de transição f emrelação a cada um dos parâmetros que definem o vector de estado do sistema.

4.1.2 Actualização

Quando uma medição é feita, há nova informação que é incorporada por forma a melhorar a estimativado estado e a diminuir o valor da incerteza. Seja ν o processo de inovação, correspondente à diferençaentre as medições reais z realizadas no instante actual a partir da equação 4.8 e a predição dessasmesmas medições z obtidas a partir da predição do estado do sistema para o instante seguinte, repre-sentada na equação 4.10. A actualização do estado do sistema e da matriz de covariância no instanteconsiderado são dadas por:

x (k + 1 | k + 1) = x (k + 1 | k) +K (k + 1) ν (k + 1) (4.12)

P (k + 1 | k + 1) = P (k + 1 | k)−K (k + 1)S (k + 1)KT (k + 1) (4.13)

onde K corresponde ao ganho de Kalman, dado por:

K (k + 1) = P (k + 1 | k)(∂h

∂x

)|x(k|k)

S (k + 1)−1 (4.14)

e S corresponde à matriz de covariância associada ao processo de inovação, dada por:

S (k + 1) =(∂h

∂x

)|x(k|k)

P (k + 1 | k)(∂h

∂x

)T|x(k|k)

+R (k + 1) (4.15)

com(∂h∂x

)|x(k|k)

correspondente à jacobiana das medições, linearizada em torno da predição do estadox (k + 1 | k) para o instante seguinte, e contendo a variação da função de medição h em relação a cadaum dos parâmetros que definem o vector de estado do sistema.

A implementação de todos estes passos no desenvolvimento da tese serão detalhadamante descritosna secção seguinte.

4.2 Implementação do Filtro de Kalman Alargado - Algoritmo deLocalização e Mapeamento

O sistema implementado utiliza apenas uma câmara como sensor, não recebendo qualquer tipo deinformação externa por meio de outros sensores sobre o estado real do sistema em cada instante. Pornão receber qualquer tipo de entrada, considera-se que o modelo apresentado nas equações 4.7 e 4.8representa em boa aproximação o sistema implementado.

23

4.2.1 Referêncial Principal

Para facilitar a percepção, a cada instante, da localização da câmara relativamente a cada um dospontos característicos determinados é necessária a existência de um referêncial principal que incorporeambos os dados. Contudo, se se considerar que a câmara usa pontos de um determinado meio para seconseguir localizar nesse mesmo meio, torna-se desnecessário saber qual a posição da câmara e dospontos num referêncial global, como o da Terra. Apenas a localização relativa de cada um interessa paraesse fim e como tal, um referêncial mais restrito torna-se uma melhor escolha. Assim, define-se comoreferêncial principal, W , aquele cuja origem está centrada na posição da câmara no instante inicial,ou seja, corresponde ao referêncial da câmara no instante inicial. Qualquer movimento da câmara apartir desse instante será considerado como um deslocamento a partir da origem e a localização dospontos no espaço será sempre relativa a este referêncial. Contudo, é necessário saber distinguir entreo referêncial principal e o referêncial da câmara em cada instante. Assim designa-se o referêncial dacâmara como C . A figura 4.1 mostra a localização da câmara e pontos característicos (a vermelho) eminstantes diferentes, no referêncial principal considerado (a preto):

(a) Instante de tempo inicial t0 (b) Instante de tempo t0 + t

Figura 4.1: Referêncial principal centrado na posição inicial da câmara, no instante de tempo t0

4.2.2 Vector de Estado e Matriz de Covariância do Sistema

O vector de estado do sistema x contém as estimações do estado do sistema em cada instante,nomeadamente a estimação da localização e orientação da câmara e de cada um dos pontos car-acterísticos obtidos. Este vector é actualizado a cada instante e encontra-se organizado da seguinteforma:

x =

xv

y1

y2...yi

(4.16)

onde xv corresponde ao vector de estado da câmara e yi corresponde ao vector de estado do ponto car-acterístico i. A constituição e organização de cada um destes vectores será explicada nas subsecçõesseguintes.

Da mesma forma, a matriz de covariância P contém o valor das incertezas correspondentes àsestimações efectuadas, estando organizada da seguinte forma:

24

P =

Pxvxv

Pxvy1 . . . Pxvyi

Py1xvPy1y1 · · · Py1yi

......

. . ....

PyixvPyiy1 . . . Pyiyi

(4.17)

Tanto o vector de estado do sistema x como a matriz de covariância P variam as suas dimensões aolongo do tempo, à medida que novos pontos vão sendo adicionados ao sistema ou eliminados deste.Contudo, estes processos serão descritos nas secções seguintes.

Vector de Estado da Câmara

O vector de estado da câmara é composto por 4 vectores principais que contêm informação sobre aposição 3D da câmara no referêncial principal, rW , a orientação da câmara no espaço dada a partir deum quaternião, qWC , a velocidade linear da câmara no referêncial principal, vW e a velocidade angularda câmara no referêncial da câmara, wC . Ao todo, o vector de estado da câmara é composto por 13parâmetros e encontra-se organizado da seguinte forma:

xv =

rW

qWC

vW

wC

(4.18)

onde rW , qWC , vW e wC são constituídos por:

rW =

xWc

yWc

zWc

, qWC =

qWC0

qWCx

qWCy

qWCz

, vW =

vWx

vWy

vWz

, wC =

wCx

wCy

wCz

(4.19)

A obtenção dos parâmetros do quaternião é feita de acordo com o método descrito nos Anexos I. Con-siderando que o valor das velocidades angulares da câmara no seu referêncial se mantêm constantesdurante um intervalo de tempo ∆t, é possível determinar a rotação instantânea sofrida pela câmara,em torno de cada um dos seus eixos principais, da seguinte forma:

rotCx = wCx ∆trotCy = wCy ∆trotCz = wCz ∆t

(4.20)

Assim, de acordo com as equações presentes nos Anexos I, define-se o angulo θ e o vector a como:

θ =√

(rotCx )2 +(rotCy

)2 + (rotCz )2 (4.21)

a =

ax

ay

az

=

rotCx

rotCy

rotCz

(4.22)

E portanto as componentes do quaternião, de acordo com as equações de q0, qx, qy e qz, ficam definidaspor:

qWC = q0 + i.qx + j.qy + k.qz (4.23)

25

onde:

qWC0 = cos

(θ

2

)(4.24)

qWCx =

ax||a||

. sin(θ

2

)(4.25)

qWCy =

ay||a||

. sin(θ

2

)(4.26)

qWCz =

az||a||

. sin(θ

2

)(4.27)

Vector de Estado do Ponto Característico i

A representação de um ponto característico i no espaço tridimensional pode ser feita a partir de 6parâmetros, presentes no vector de estado desse ponto, dado por:

yi =

xWci

yWci

zWci

θi

φi

ρi

(4.28)

onde xWci, yWci

e zWcicorrespondem às coordenadas do centro óptico da câmara no referêncial principal

no instante em que o ponto característico foi observado, θi e φi correspondem aos angulos de azimutee elevação que descrevem a orientação espacial do raio m ao longo do qual o ponto se encontralocalizado e ρi corresponde ao inverso da profundidade a que o ponto se encontra, tal como mostra afigura 4.2:

Figura 4.2: Parametrização do Ponto característico

Assim, a localização 3D de um ponto característico i no referêncial principal é dada por:

26

xWiyWizWi

=

xWci

yWci

zWci

+1ρim (θi, φi) (4.29)

onde m (θi, φi) é dado por:

m (θi, φi) =

cosφi sin θi− sinφi

cosφi cos θi

(4.30)

Esta parametrização dos pontos, proposta por Andrew Davison, em [3], designa-se Parametrização deProfundidade Inversa, e a sua aplicação traz grandes vantagens na implementação do filtro de Kalmanalargado, na medida em que este filtro produz melhores estimações quando o modelo do sistema éconsiderado linear. Esta parametrização permite que todos os pontos observados pelo sistemapossam ser incorporados e utlizados para estimar o movimento da câmara no espaço, e não só os quese encontrem a distâncias mais reduzidas. Repare-se que o valor da incerteza da distância de umponto considerado no infinito não pode ser bem modelado através de uma gaussiana, ao passo que aincerteza correspondente ao inverso dessa mesma distância pode facilmente ser representado poruma distribuição desse tipo. Andrew Davison demonstrou também que com uma parametrização destetipo, a função de medição pode ser linearizada para baixos ângulos de paralaxe, ou seja, o ângulo quecorresponde a um aparente deslocamento de um ponto quando observado de diferentes localizaçõesda câmara. A figura 4.3 mostra um exemplo de ângulos de paralaxe obtidos a partir da observação dedois pontos diferentes, a diferentes profundidades, após um deslocamento dx da câmara:

Figura 4.3: Diferentes angulos de paralaxe

Um ponto localizado a uma pequena profundidade produz um grande ângulo de paralaxe apenas comum pequeno deslocamento da câmara, ao passo que um ponto localizado a uma grande profundidadepode produzir um angulo de paralaxe muito baixo ou praticamente nulo para o mesmo deslocamento.Isto significa que no plano da câmara, o ponto ficou praticamente estático com o decorrer do tempo.

Como o deslocamento entre imagens contíguas é geralmente muito baixo, o incremento no ângulo deparalaxe é igualmente pequeno. Isto significa que inicialmente, os vectores correspondentes àsdistâncias d1 e d2, identificados na figura 4.3, são praticamente paralelos, pois o ângulo de paralaxenesse instante é praticamente nulo. Esta particularidade permite que para baixos ângulos de paralaxe,tendo em conta a parametrização utilizada, a função de medição h possa ter característicasaproximadamente lineares, fornecendo unicamente informação sobre a orientação da câmara, comose verá na subsecção 4.2.5. A incerteza inicial correspondente à profundidade a que o ponto seencontra pode ser bem modelada por uma gaussiana, sendo que a região de aceitação, inicialmente,se extende desde um ponto próximo até infinito. A extensa região cobre uma zona de incerteza na

27

ordem dos 95%. À medida que o ângulo de paralaxe vai aumentando, por translacção da câmara, afunção de medição h vai perdendo as características aproximadamente lineares, fornecendo tambéminformação sobre a localização espacial do ponto no espaço, inclusivamente da profundidade a queeste se encontra. Desta forma, o valor da incerteza correspondente vai diminuindo, o que se reflectenuma diminuição da região de aceitação, permitindo determinar a profundidade a que o ponto seencontra. Considera-se que a incerteza na determinação da localização do ponto no espaçocorresponde maioritariamente à incerteza na determinação da sua profundidade, pois as duas outrascoordenadas podem facilmente ser determinadas com alguma exactidão a partir das coordenadas doponto no plano da imagem, através de algumas transformações, como se verá mais à frente.

4.2.3 Inicialização do Sistema

Inicialização do Vector de Estado e Matriz de Covariância da Câmara

O vector de estado da câmara é inicializado considerando que a posição da câmara no referêncialprincipal, definido na secção 4.2.1, corresponde à origem deste, ou seja:

rW =

000

(4.31)

Considera-se que o valor do ângulo θ, usado para determinar os parâmetros do quaternião é nulo, poiso referêncial da câmara encontra-se alinhado com os eixos do referêncial principal, não havendoqualquer rotação em torno destes, pelo que:

qWC =

1000

(4.32)

Considera-se ainda que tanto a velocidade linear como a velocidade angular da câmara são nulas:

vW =

000

, wC =

000

(4.33)

A matriz de covariância da câmara é inicializada como uma matriz diagonal, considerando que não háincerteza nos valores de rW e qWC e alguma incerteza nos valores de vW e wC :

Pxvxv= diag (0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0.5, 0.5, 0.5) (4.34)

Neste ponto, o vector de estado do sistema corresponde só ao vector de estado da câmara, contendoos seus 13 parâmetros e a matriz de covariância do sistema P corresponde apenas à matriz decovariância da câmara Pxvxv , de dimensão (13× 13), pois inicialmente considera-se que não hánenhum ponto característico observado pelo sistema.

28

Inicialização do Vector de Estado e Actualização da Matriz de Covariância para um Ponto Carac-terístico i

A observação de um ponto característico, através do método descrito no Capítulo 3, implica a actual-ização do vector de estado e da matriz de covariância do sistema, pois novos parâmetros necessitamser incorporados. Após um ponto característico ser observado e identificado pela câmara quando estase encontra na posição dada pelo vector rW ,as suas coordenadas no plano da imagem, sem distorção,hiud, são dadas por (uud, vud). A representação desse ponto no referêncial da câmara é conseguidaatravés da seguinte conversão:

hCud =

hCx

hCy

hCz

=

u0−uud

f1.ku

v0−vud

f2.kv

1

(4.35)

com u0, v0, f1, f2, ku e kv definidos no Capítulo 2.

Aplicando a matriz de rotação RWC , definida no Anexo I, que possibilita a passagem do referêncial dacâmara para o referêncial principal, permite determinar as coordenadas desse ponto no referêncialprincipal:

nWud =

nWx

nWy

nWz

= RWC × hCud (4.36)

onde:

RWC =

q20 + q2x − q2y − q2z −2q0qz + 2qxqy 2qxqz + 2q0qy2qxqy + 2q0qz q20 − q2x + q2y − q2z −2q0qx + 2qyqz−2q0qy + 2qxqz 2qyqz + 2q0qx q20 − q2x − q2y + q2z

(4.37)

Esta matriz de rotação incorpora, ainda que indirectamente, a informação correspondente ao ângulode paralaxe para cada ponto, permitindo posteriormente obter a determinação da profundidade a queeste se encontra.

A partir das coordenadas do ponto no referêncial principal determinam-se os angulos de azimute eelevação que caracterizam a orientação do raio m. A relação trigonométrica entre os valores dascoordenadas do ponto e os ângulos encontra-se presente na figura 4.4, e é dada por:

θi = arctan(nWxnWz

)(4.38)

φi = arctan

−nWy√(nWx )2 + (nWz )2

(4.39)

29

Figura 4.4: Orientação do raio m

O valor de ρi inicialmente considerado, de 0.1, foi obtido por forma a que a região de aceitação geradaenglobasse uma área correspondendo a 95% de incerteza, extendendo-se desde um ponto próximoaté infinito. Desta forma, têm-se definidos todos os parâmetros correspondentes ao vector de estadodo ponto característico i e o vector de estado do sistema passa a ter dimensão (13 + 6), considerandoos 13 parâmetros iniciais da câmara e os 6 novos parâmetros do ponto. A dimensão deste vector comum número n de pontos observados será (13 + (6× n)).

A matriz de covariância necessita igualmente de ser actualizada por forma a incorporar o valor dasincertezas correspondentes aos parâmetros do ponto.

A variância associada a cada um dos parâmetros de yi é obtida a partir da matriz de covariância dasmedições Ri, que contém não só o erro de medição do ponto no plano da imagem como também oerro de medição do inverso da profundidade a que o ponto se encontra, e através da matriz decovariância do sistema P .

Considerando que se trata da observação do primeiro ponto do sistema, apenas para termos desimplificação, a matriz de covariância do sistema antes da observação do ponto correspondeunicamente à matriz de covariância da câmara Pxvxv

. Define-se assim uma matriz de covariânciaauxiliar P ′, dada por:

P ′ =

[Pxvxv (13× 13) zeros (13× 3)zeros (3× 13) Ri (3× 3)

](4.40)

onde Ri corresponde à matriz associada ao erro na medição, diagonal e definida por:

Ri =

∆u2 0 00 ∆v2 00 0 ∆ρ2

(4.41)

com ∆u e ∆v correspondentes ao ruído na medição das coordenadas do ponto no plano da imagem,com desvio de 1 pixel cada e ∆ρ correspondente ao ruído na medição do inverso da profundidade, comdesvio de 0.5 /metro. Tanto ∆u e ∆v, como ∆ρ são considerados ruído branco gaussiano, indepen-dentes e de média nula.

A matriz de covariância final, contendo o valor das covariâncias de cada um dos parâmetros do pontoé assim obtida através da aplicação de uma matriz de jacobianas J a esta matriz P ′, tal que:

30

P = J × P ′ × JT (4.42)

A matriz de jacobianas J contém informação sobre a variação do vector de estado yi em relação acada um dos parâmetros do vector de estado da câmara xv, ∂yi

∂xv, assim como em relação às medições

efectuadas sobre o ponto no plano da imagem, com distorção, ∂yi

∂hid

, pois estas correspondem àsmedições reais. A obtenção destas jacobianas implica a determinação de várias derivadas, que seencontram detalhadas no Anexo II.

Assim, a matriz das jacobianas J , aplicada na equação 4.42, vem definida por:

J =

[I (13× 13) zeros (13× 3)∂yi

∂xv(6× 13) ∂yi

∂hid

(6× 3)

](4.43)

A aplicação desta matriz J à matriz de covariância P ′ permite assim obter a matriz de covariância P ,que toma a seguinte forma:

P =

Pxvxv Pxvxv

∂yi

∂xv

T

∂yi

∂xvPxvxv

∂yi

∂xvPxvxv

∂yi

∂xv

T+ ∂yi

∂hid

Ri∂yi

∂hid

T

(4.44)

Mais uma vez se realça que neste caso se considera que o ponto característico observado eadicionado ao sistema corresponde ao primeiro, por permitir uma melhor percepção de todo oprocesso de inicialização do sistema. O processo é análogo para outros pontos adicionados aosistema que não o primeiro, apenas as dimensões do vector de estado e da matriz de covariância dosistema são maiores.

4.2.4 Predição

Por este sistema não receber qualquer tipo de entrada u, como já foi referido, a predição dos parâmetrosdo vector de estado do sistema é determinada a partir de um vector de ruído n, contendo os valoresdas velocidades linear e angular deste, estimados para o instante anterior e presentes no vector deestado do sistema, assumindo que para um intervalo de tempo pequeno ∆t, igual a 1

30 segundos, essasvelocidades não se alteram significativamente. Este vector é considerado ruído pois não há certeza nosvalores assumidos, podendo ou não corresponder à real velocidade do sistema no instante seguinte. Ovector de ruído n é assim constituído por:

n =

(vW (k | k)wC (k | k)

)=

vWx (k | k)vWy (k | k)vWz (k | k)wCx (k | k)wCy (k | k)wCz (k | k)

(4.45)

Assim, a predição genérica do estado do sistema x para o instante seguinte vem dada a partir da funçãode transição de estado f e do vector de ruído n por:

x (k + 1 | k) = f (x (k | k) , n (k + 1)) (4.46)

31

Contudo, devido à não linearidade da função de transição de estado f , é necessário que esta sejalinearizada em torno do valor estimado do estado, para o instante considerado. A função de transiçãof pode ser separada em duas outras funções, considerando que a predição do estado da câmara e doestado de cada um dos pontos são dois processos independentes. A função que descreve a transiçãodo estado da câmara é dada por fv, ao passo que a função que descreve a transição do estado decada ponto do sistema é dada por fy.

Predição do vector de estado da câmara xv

A partir da função fv é possível obter as predições para os parâmetros que definem o vector de estadoda câmara xv. Tendo em conta o vector de estado, contendo os seus valores estimados num dadoinstante k, e o vector de ruído n, define-se a função fv, linearizada em torno do valor estimado xv,como:

fv =

rW (k + 1 | k)qWC (k + 1 | k)vW (k + 1 | k)wC (k + 1 | k)

=

rW (k | k) + vW (k | k) ∆tqWC (k | k)× q′WC (k | k)

vW (k | k)wC (k | k)

(4.47)

A predição dos parâmetros rW , vW e wC encontra-se bem definida na equação 4.47. Contudo, para apredição das componentes do quaternião no instante seguinte são necessárias não só as componentesde qWC (k | k) dadas no vector de estado da câmara como também as componentes de um quaterniãoauxiliar q

′WC (k | k), obtido a partir dos valores estimados de wC (k | k), presentes no vector de ruídon. O produto externo entre as componentes destes dois quaterniões dará a predição das componentesdo quaternião do vector de estado da câmara, no instante seguinte. Assumindo então que a velocidadeangular da câmara não varia significativamente de um instante para o outro, a predição das rotaçõesda câmara em torno dos seus eixos principais é dada por:

ˆrotx (k + 1 | k) = wCx (k | k) ∆t (4.48)

ˆroty (k + 1 | k) = wCy (k | k) ∆t (4.49)

ˆrotz (k + 1 | k) = wCz (k | k) ∆t (4.50)

A partir desta predição, é possível determinar o ângulo θ e o vector a, a partir das equações 4.21 e4.22, que permitem obter o quaternião q

′WC (k | k):

q′WC (k | k) = cos θ +

a

‖a‖sin θ (4.51)

Por fim, a predição das componentes do quaternião qWC no instante (k + 1) é dada por:

qWC0 (k + 1 | k) = qWC

0 q′WC0 −

[qWCx qWC

y qWCz

] q′WCx

q′WCy

q′WCz

(4.52)

qWCx (k + 1 | k) = −qWC

z q′WCy +

[qWC0 qWC

x qWCy

] q′WCx

q′WC0

q′WCz

(4.53)

32

qWCy (k + 1 | k) = −qWC

x q′WCz +

[qWC0 qWC

y qWCz

] q′WCy

q′WC0

q′WCx

(4.54)

qWCz (k + 1 | k) = −qWC

y q′WCx +

[qWC0 qWC

z qWCx

] q′WCz

q′WC0

q′WCy

(4.55)

onde se suprimiu a indicação (k | k) nos termos das equações para uma melhor percepção destas.

Assim, a predição do vector de estado da câmara xv encontra-se completa e é definida por:

xv (k + 1 | k) =

rW (k + 1 | k)qWC (k + 1 | k)vW (k + 1 | k)wC (k + 1 | k)

(4.56)

Predição do vector de estado de cada ponto característico yi

A predição do vector de estado para cada ponto característico yi, consiste em assumir que o ponto semantém na mesma posição no instante seguinte, pois na realidade, correspondem a pontos estáticosno mundo, de modo que:

yi (k + 1 | k) = yi (k | k) =

xWci(k | k)

yWci(k | k)

zWci(k | k)

θi (k | k)φi (k | k)ρi (k | k)

(4.57)

Desta forma, a função que representa a transição de estado dos i pontos do sistema vem dada por:

fy =

y1 (k + 1 | k)

...yi (k + 1 | k)

=

y1 (k | k)

...yi (k | k)

(4.58)

Predição do vector de estado do sistema x

Por fim, a predição do vector de estado do sistema x é obtida a partir das predições dos vectores deestado da câmara e de cada um dos pontos característicos considerados:

x (k + 1 | k) =

xv (k + 1 | k)y1 (k + 1 | k)

...yi (k + 1 | k)

(4.59)

onde a função de transição de estado do sistema global f , linearizada, vem finalmente definida por:

f =

(fv

fy

)(4.60)

33

com fv definida em 4.47 e fy definida em 4.58.

Predição da matriz de covariância P

Como a função de transição de estado f foi linearizada a partir de valores estimados, correspondendoa uma aproximação da função real, a matriz de covariância P não representará a verdadeira incertezana estimação do estado do sistema, dependendo assim do erro associado à linearização. Contudo,considera-se que neste caso, o erro de linearização é baixo o suficiente para que as aproximaçõesfeitas sejam consideradas adequadas.

A predição da matriz de covariância implica a determinação da jacobiana da transição de estado dosistema

(∂f∂x

)|x(k|k)

, que corresponde à variação da função, linearizada em torno da estimação do

estado no sistema do instante considerado, em relação a cada um dos parâmetros do vector de estadodo sistema, assim como a determinação da matriz de covariância associada ao ruído presente noprocesso de transição do sistema, Q. Por ser extensa, a determinação da jacobiana encontra-sedetalhada de forma pormenorizada no Anexo II. Por sua vez, a determinação da matriz de covariânciaQ implica a determinação de uma outra jacobiana ∂f

∂n , correspondente à variação da função detransição de estado do sistema, f com os parâmetros do vector de ruído n definido em 4.45. Tendo emconta que a função de transição do estado de cada ponto do sistema, fy, não depende do vector deruído n, a sua variação com este, dada por ∂fy

∂n , será nula. Por outro lado, a função que representa oprocesso de transição do estado da câmara, fv, varia com as componentes do vector de ruído, comose viu na equação 4.47. Por análise da equação 4.47 é possível obter a jacobiana ∂fv

∂n , dada por:

∂fv∂n

=

I (3× 3) ∆t zeros (3× 3)

zeros (4× 3) ∂qW C(k+1|k)∂qW C(k|k)

∂qW C(k|k)∂wC(k|k)

I (3× 3) zeros (3× 3)zeros (3× 3) I (3× 3)

(4.61)

onde ∂qW C(k+1|k)∂qW C(k|k) e ∂qW C(k|k)

∂wC(k|k) se encontram definidas no Anexo II. Para determinar as componentes damatriz de covariância Q relativas ao ruído no processo de transição da câmara, representadas por Qv,é necessário conhecer a matriz de covariância associada ao vector de ruído n, Pn, que se assume serdiagonal, representando os erros associados às componentes linear e angular da velocidade em cadainstante. Esta matriz vem assim definida por:

Pn =

(σax∆t)2 0 0 0 0 00

(σay ∆t

)2 0 0 0 00 0 (σaz ∆t)2 0 0 00 0 0 (σαx∆t)2 0 00 0 0 0

(σαy ∆t

)2 00 0 0 0 0 (σαz ∆t)2

(4.62)

com σax = σay = σaz = 6ms2 e σαx= σαy

= σαz= 6 rads2 , valores que permitem que o movimento da

câmara através da mão seja relativamente bem determinado, onde se assume um movimento lento,com baixas variações instantaneas de velocidade.

Desta forma, a matriz Qv vem dada a partir da combinação das matrizes 4.61 e 4.62:

Qv =∂fv∂n

Pn∂fv∂n

T

(4.63)

34

e por sua vez, a matriz de covariância Q, com dimensão igual à da matriz de covariância do sistema P ,vem definida por:

Q =

Qv 0 · · · 00 0 · · · 0...

.... . .

...0 0 · · · 0

(4.64)

Por fim, a predição para a matriz de covariância do sistema, P , definida em 4.44, vem dada através daequação 4.65:

P (k + 1 | k) =(∂f

∂x

)|x(k|k)

P (k)(∂f

∂x

)T|x(k|k)

+Q (k) (4.65)

4.2.5 Predição das medições

Assim como a função de transição de estado f , também a função de medição h não tem caracterís-ticas lineares. Por esta razão, é necessário linearizar esta função em torno do valor correspondenteà predição do estado no instante seguinte, definida em 4.59. Desta forma, é possível prever quais asmedições que serão efectuadas sobre cada um dos pontos, isto é, qual a predição para as coordenadasde cada um dos pontos no plano da imagem, com distorção, z, no instante seguinte, tendo em conta apredição do estado do sistema para esse instante, tal como mostra a seguinte equação:

z (k + 1) = h (x (k + 1 | k)) (4.66)

A predição das medições permite assim gerar zonas de maior probabilidade, onde é mais provável quecada ponto se encontre na realidade, restrigindo desta forma a zona de procura na fase de matching,cujo processo se encontra descrito no Capítulo 3.

Determinação da predição das medições z

Considere-se a predição do vector de estado da câmara e do ponto i, definidas nas equações 4.56 e4.57. É possível prever qual a medição que será efectuada sobre este ponto, em termos dascoordenadas deste no plano da imagem, com distorção, a partir da função de medição hi. O primeiropasso consiste na obtenção das suas coordenadas no referêncial da câmara:

hCi =

hCxi

hCyi

hCzi

=(RWC

)−1

×

hWxi

hWyi

hWzi

(4.67)

com:

hWi =

hWxi

hWyi

hWzi

=

xWci

yWci

zWci

− xWc

yWc

zWc

ρi + mi

(θi, φi

) (4.68)

e onde RWC corresponde à matriz de rotação definida em 4.37, obtida a partir das predições doscomponentes do quaternião e mi

(θi, φi

)corresponde ao raio que define a orientação do ponto no

referêncial principal obtido a partir das predições dos parâmetros θi e φi do ponto.

35

Repare-se que para pequeno ângulos de paralaxe, os vectores[xWci

yWcizWci

]Te[

xWc yWc zWc

]Tsão praticamente paralelos. Esta particularidade, já referida na secção 4.2.2,

permite que a equação 4.68 seja aproximada por:

hWi ≈ mi

(θi, φi

)(4.69)

e por conseguinte, a equação 4.67 venha dada por:

hCi ≈(RWC

)−1

× mi

(θi, φi

)(4.70)

Esta aproximação, só possível graças à parametrização descrita na secção 4.2.2, fornece informaçãoapenas sobre a orientação espacial da câmara, quer numa fase inicial, quando a translacção dacâmara é ainda pequena, quer numa fase mais avançada, onde o ângulo de paralaxe para algunspontos é muito reduzido devido à grande distância a que se encontram. Contudo, esta aproximaçãoaparece apenas em teoria na medida em que não é feita nenhuma alteração na implementação porforma a considerar estes dois casos.

É necessário então converter estas coordenadas para o plano da imagem. Assim, a predição dascoordenadas do ponto i no plano da imagem, sem distorção, são obtidas por:

hiudi=

(uudi

vudi

)=

u0 −(hC

xi

hCzi

)f1ku

v0 −(hC

yi

hCzi

)f2kv

(4.71)

onde u0, v0, f1, f2, ku e kv correspondem mais uma vez aos parâmetros de calibração da câmaradefinidos no Capítulo 2.

Contudo, a lente da câmara produz distorção e como tal é necessário adicionar essa mesma distorçãoàs coordenadas do ponto, para que a predição da medição seja o mais próxima possível da mediçãoreal. Desta forma, as mesmas coordenadas com distorção (udi

, vdi), são obtidas a partir da aplicação

das equações definidas igualmente no Capítulo 2. Por fim, a predição das medições para o pontocaracterístico i, zi é dada por:

zi = hi (xv, yi) =(

udivdi

)(4.72)

De forma análoga se define a predição das medições para todos os outros pontos do sistema, dadapor:

z (k + 1) = h (x (k + 1 | k)) =

z1...zi

(4.73)

Por se tratarem de predições, a cada predição de medição z encontra-se sempre associado um valorde incerteza, uma variância em relação ao valor obtido. Essas incertezas encontram-se representadasna matriz de covariância associada à predição das medições, Spred (k + 1). Esta matriz permiteposteriormente na fase de matching determinar as zonas na imagem onde é mais provável que cadaponto se encontre no instante seguinte. A obtenção desta matriz implica a determinação da jacobiana(∂h∂x

)|x(k|k)pred

, que corresponde à variação da função de medição h, linearizada em torno do valor da

36

predição do estado do sistema, para o instante seguinte. A determinação desta jacobiana, por serextensa, encontra-se detalhada no Anexo II. Através da aplicação desta jacobiana à predição damatriz de covariância do estado do sistema P (k + 1 | k) é possível obter então a matriz Spred (k + 1),dada assim por:

Spred (k + 1) =(∂h

∂x

)|x(k|k)pred

× P (k + 1 | k)×(∂h

∂x

)T|x(k|k)pred

+R (4.74)

onde R corresponde à matriz de covariância associada ao erro das medições, análoga à definida em4.41, mas sem o erro associado à medição da profundidade, ∆ρ2. Para cada ponto i considerado, amatriz 4.74 corresponde então a uma matriz quadrada, de dimensão 2×2, cuja diagonal contém o valordas variâncias associadas a udie vdi , obtidos na equação 4.72, dada de forma simplificada por:

Spredi=(∂hi∂x

)pred

Pxvxv

(∂h

∂x

)Tpred

+2

((∂hi∂x

)pred

Pxvyi

(∂h

∂x

)Tpred

)+(∂hi∂x

)pred

Pyiyi

(∂h

∂x

)Tpred

+Ri

(4.75)

4.2.6 Matching

A partir dos valores de variância dados pela diagonal da matriz Spredi, definida na equação 4.75, é

possível gerar uma zona de confiança em torno dos valores da predição onde há uma probabilidadede aproximadamente 95% do ponto i ser encontrado. Considere-se os desvios padrão σudi

e σvdi,

associados às predições de medição udie vdi

e obtidos aplicando a raíz quadrada aos valores devariância correspondentes. A zona de confiança vem então dada por:

zona a 95% =[udi± 2σudi

, vdi± 2σvdi

](4.76)

Utilizando o algoritmo descrito no Capítulo 3 a cada zona de confiança obtida desta forma, é possívelobter o valor real das medições, z, para cada um dos pontos, a partir do conhecimento do estado dosistema no instante seguinte, ou seja, da imagem seguinte, como mostra a equação 4.77:

z (k + 1) = h (x (k + 1)) (4.77)

Caso um ponto i não seja encontrado dentro desta zona, considera-se que não houvecorrespondência entre a predição da medição e a medição real para esse ponto, sendo que ascomponentes de Spredi

são eliminadas da matriz de covariância Spred (k + 1). Desta forma, a matrizSpred (k + 1) passa a conter apenas a informação correspondente aos pontos para os quais houvecorrespondência, representando desta forma a covariância associada à diferença entre as mediçõesreais e a predição dessas medições, ou seja, o erro associado à medição. Esta diferença entre asmedições reais e sua predição corresponde ao processo de inovação do sistema e a matriz decovariância do processo de inovação vem assim definida por S (k + 1). Da mesma forma, é necessárioadaptar a jacobiana

(∂h∂x

)|x(k|k)pred

, por forma a incorporar apenas as derivadas relativas às prediçõesbem sucedidas. A adaptação da jacobiana corresponde a eliminar as linhas relativas aos pontos paraos quais não houve correspondência, e vem definida por

(∂h∂x

)|x(k|k)match

.

37

4.2.7 Actualização do estado do sistema

Este passo corresponde ao último na implementação do filtro de Kalman Alargado, na medida em quetanto o vector de estado do sistema, como a sua matriz de covariância são finalmente actualizadas.

A actualização do sistema implica a determinação do ganho de Kalman óptimo, K, que garante aminimização do erro da estimação. O ganho de Kalman, obtido a partir da estimação da matriz decovariância P (k + 1 | k), definida na equação 4.65, da jacobiana

(∂h∂x

)|x(k|k)match

e da matriz decovariância do processo de inovação, S, é dado por:

K (k + 1) = P (k + 1 | k)×(∂h

∂x

)T|x(k|k)match

× S (k + 1)−1 (4.78)

A partir do ganho de Kalman, é possível finalmente obter a estimação final do vector de estado dosistema, x (k + 1 | k + 1), e da matriz de covariância P (k + 1 | k + 1), assim definidas por:

x (k + 1 | k + 1) = x (k + 1 | k) +K (k + 1)× ν (k + 1) (4.79)

P (k + 1 | k + 1) = P (k + 1 | k)−K (k + 1)S (k + 1)KT (k + 1) (4.80)

onde o processo de inovação ν (k + 1) vem definido por:

ν (k + 1) = z (k + 1)− z (k + 1) (4.81)

4.3 Eliminação de um ponto do sistema

Por vezes há pontos que se perdem do sistema, ou seja, na fase de matching, descrita na subsecção4.2.6, esses pontos simplesmente não são encontrados nas imagens seguintes, eventualmente pornão terem as características necessárias de um bom ponto, descritas no Capítulo 3. Como tal, asua incorporação no sistema deixa de ser necessária, contribuindo apenas para aumentar o tempode processamento deste e afectar, de forma negativa, a estimação da posição da câmara. Por estesmotivos, torna-se necessário eliminar estes pontos do sistema.

A eliminação de um ponto característico i do sistema corresponde unicamente a eliminar os 6parâmetros yi pertencentes ao vector de estado do sistema x assim como todas as componentes damatriz de covariância P correspondentes a esse ponto. Como exemplo, considere que se pretendeeliminar o primeiro ponto do sistema, num conjunto de 3 pontos. A sua eliminação do sistemaencontra-se assim descrita no exemplo abaixo:

xv

y1

y2

y3

→ xv

y2

y3

,

Pxvxv Pxvy1 Pxvy2 Pxvy3

Py1xv Py1y1 Py1y2 Py1y3

Py2xv Py2y1 Py2y2 Py2y3

Py3xv Py3y1 Py3y2 Py3y3

→ Pxvxv Pxvy2 Pxvy3

Py2xv Py2y2 Py2y3

Py3xv Py3y2 Py3y3

(4.82)

38

4.4 Factor de Escala

O método apresentado e descrito nas secções anteriores permite fornecer informação sobre o desloca-mento realizado pela câmara e sobre a localização espacial de todos os pontos utilizados por esta comoreferência. Contudo, este método não tem a capacidade de perceber se o deslocamento efectuado cor-responde ao deslocamento real, ou se a disposição dos pontos corresponde à sua real disposição.Apesar de a relação entre todas medições obtidas ser correcta, num sistema deste tipo é semprenecessário definir um factor de escala que permita escalar todas as medições obtidas pelo sistema.Considere-se o caso exemplificado na figura 4.5:

Figura 4.5: Ambiguidade nas medições

Um objecto A encontra-se a 1 metro de distância de um ponto B. O objecto A desloca-se 1m nadirecção perpendicular e volta a avistar o ponto B. O ângulo de paralaxe α para esse pontocorresponde neste caso a 45o. Considere-se então que o mesmo objecto A se encontra igualmentedistanciado 2m de um outro ponto C. Se o objecto A se deslocar 2m na mesma direcção, quandoavistar novamente o ponto C, o ângulo de paralaxe para esse caso, α′, será exactamente igual aoângulo anterior. Existe assim uma ambiguidade associada a cada medição obtida, apesar de essaambiguidade não corresponder na realidade a um erro de medição. Esta pode ser eliminadainicializando o sistema com pontos característicos específicos, cujas coordenadas são conhecidas,com um valor de incerteza nulo ou escalando todas as medições efectuadas a partir das dimensõesconhecidas de um objecto que apareça na imagem, após todas as estimações terem sido feitas. Destaforma, todas as outras medições e estimações obtidas pelo sistema virão escaladas e corresponderãoo seu valor real. Se o ponto B corresponder à localização real do ponto e o ponto C for a sua medição,o factor de escala a aplicar neste caso seria de 1

2 .

O factor de escala é muito importante quando num sistema em tempo real se pretende ter informaçãosobre a localização real da câmara e dos pontos em cada instante. Se a navegação do sistemadepender desses valores, é importante que estes se apresentem correctos, caso contrário anavegação não seria possível. Assim, é sempre necessário inicializar um sistema deste tipo compontos bem conhecidos, que permitam escalar logo à partida todas as estimações que venham a serobtidas posteriormente.

39

Capítulo 5

Algoritmo de Detecção de Zonas deAterragem

Uma aeronave pilotada não tem geralmente dificuldade em aterrar num aeroporto. As zonas de ater-ragem encontram-se bem definidas e a distância da aeronave ao solo é bem monitorizada e controladapor quem a pilota. Contudo, se essa aeronave necessitar de efectuar uma aterragem de emergên-cia, em zona desconhecida, apenas a destreza do piloto aliada à informação disponibilizada pelosinstrumentos de navegação permitem que esta seja feita em condições. A informação fornecida pelosintrumentos dá ao piloto segurança suficiente para procurar, utilizando a sua visão, uma zona que seaproxime o mais possível de uma zona plana, pois a partir do momento em que esta seja determinada,todo o processo de aterragem pode ser efectuado baseando-se unicamente nos intrumentos que tem abordo. Todavia, em aeronaves não pilotadas, são necessários outros meios que permitam, nas mesmascondições, determinar possíveis zonas de aterragem que possam ser consideradas seguras.

O sistema descrito no Capítulo 4 permite que o movimento de uma câmara seja bem estimado aolongo do tempo, independentemente do movimento que é efectuado. Assim, um sistema deste tipoimplementado numa aeronave dessa natureza permite obter em cada instante a localização,orientação e velocidades linear e angular desta. Para além disso, este sistema permite ainda fazer ummapeamento de toda a zona envolvente, com uma grande precisão e é justamente esta informaçãodisponibilizada que será utilizada para detectar zonas planas que possam ser consideradas possíveiszonas de aterragem, de forma completamente autónoma.

Neste capítulo é apresentado e descrito um algoritmo que permite determinar zonas de aterragempara um quadrirotor, a partir do mapeamento feito através do sistema. Contudo, apesar de seconsiderar a aplicação deste sistema a um quadrirotor, esta aplicação foi proposta apenas em teoria,na medida em que não foi definida nenhuma lei de controlo que permitisse ao quadrirotor aterrar,baseando-se unicamente neste sistema.

5.1 Orientação da câmara no quadrirotor

Considerando a implementação de um sistema deste tipo num quadrirotor, a localização da câmarateria que permitir determinar em cada instante a distância da câmara ao solo, ou de outro modo nãoseria possível detectar correctamente zonas planas que pudessem servir como zona de aterragemdeste. Desta forma, ela teria que ser colocada na parte inferior do quadrirotor, sendo que o eixo Z do

40

referêncial da câmara estaria orientado na direcção do solo e os eixos X e Y seriam paralelos a este,tal como indica a figura 5.1:

Figura 5.1: Disposição da câmara num quadrirotor

O referêncial principal estaria assim centrado no centro óptico da câmara e por conseguinte, noquadrirotor, sendo que todos os pontos estimados pelo sistema seriam representados nestereferêncial.

5.2 Detecção de zonas planas

Um sistema de aterragem exige uma determinação precisa da distância que separa o veículo do solo,caso contrário o veículo não consegue efectuar uma aterragem em condições. Desta forma, e con-siderando neste caso que o quadrirotor não utiliza nenhum outro sistema auxiliar para correcção emelhoramento de medições, é necessário que os pontos utilizados na determinação do plano tenhama menor incerteza possível associada à sua profundidade ou distância à origem do referêncial, corre-spondente à distância da câmara ao solo e que a ambiguidade associada ao factor de escala, referidono capítulo 4, seja atempadamente eliminada.

Há diversos algoritmos que permitem determinar planos através de um conjunto de pontos no espaço,ou conseguem de algum modo encontrar um plano que englobe o maior número de pontos possível. Oalgoritmo aqui implementado consiste numa variante do algoritmo RANSAC, descrito em [4], utilizadoigualmente para determinação de planos no espaço. Este algoritmo, assim como o RANSAC, não sópermitem ter um tempo de processamento rápido como também permitem obter resultados bastanteaceitáveis.

Considere-se então um conjunto de pontos característicos P , obtidos através do sistema e cujaincerteza corresponde ao valor minimo possível. O algoritmo baseia-se no facto de que cada 3 pontosno espaço, não colineares, definirem sempre um plano. A equação geral que define um plano noespaço, em coordenadas cartesianas, é dada por:

Z = aX + bY + c (5.1)

41

Contudo, a definição de um plano em coordenadas cartesianas não é a melhor definição quando sepretende representar um plano de aterragem, onde interessa acima de tudo saber a inclinação doplano em relação à aeronave, assim como a distância entre os dois. Desta forma, a representação deum plano em coordenadas esféricas apresenta-se como a melhor escolha, permitindo ter toda ainformação necessária. A definição de um plano neste sistema de coordenadas é dada a partir dovector normal à sua superfície, que define a distância desde a origem do referêncial até ao plano, ρ e apartir dos ângulos θ e ϕ, que definem a orientação do plano no espaço, onde θ corresponde ao ângulode azimute e ϕ corresponde ao ângulo de elevação do plano, tal como indicado na figura 5.2:

Figura 5.2: Plano definido a partir de ρ, θ e ϕ

Assim, a equação que define o mesmo plano no sistema de coordenadas descrito é dada por:

ρ = (cos θ cosϕ)X + (sin θ cosϕ)Y + (sinϕ)Z (5.2)

É possível verificar que há uma relação entre os parâmetros que definem o plano em ambos ossistemas de coordenadas. Comparando as equações 5.1 e 5.2, obtém-se a seguinte relação entre osparâmetros ρ, θ e ϕ e os parâmetros a, b e c:

θ = arctan(b

a

)(5.3)

ϕ = arctan(

sin θb

)(5.4)

ρ = c sin (ϕ) (5.5)

Assim, para cada 3 pontos pertencentes a P , caracterizados pelas coordenadas X, Y e Z, sãodeterminados os parâmetros a, b e c do plano por eles definido, através da equação 5.1.Posteriormente, através das equações 5.3, 5.4 e 5.5 determinam-se os parâmetros ρ, θ e ϕ do mesmoplano. Contudo, é necessário verificar se cada ponto pertencente ao conjunto de pontos P pertenceao plano determinado. Esta verificação é feita a partir dos parâmetros anteriormente determinados,obtendo-se para cada ponto i o valor de ρi, através da equação 5.2. Idealmente, se um ponto ipertencer ao plano considerado, então ρi será igual a ρ. Contudo isto não acontece na realidadedevido à incerteza associada à determinação de cada ponto do sistema. Assim, é determinada adistância a que cada ponto i se encontra do plano, através da seguinte equação:

distanciai =| ρi − ρ | (5.6)

42

Como há incerteza associada a cada ponto, é necessário considerar um valor mínimo, abaixo do qualse considera que um ponto pertence ao plano. Este valor depende da precisão com que se pretendeobter um bom plano. No caso de planos de aterragem para um sistema deste tipo, o valor mínimoconsiderado não deve ser superior a 0.01m, o que permite obter uma precisão muito boa.

Porém, para que seja considerado um bom plano, é necessário que o número de pontos pertencentesa esse plano seja superior a um valor limite, que varia com o número de pontos em análise. O númerode pontos pertencentes a um plano permite ter informação sobre a natureza desse mesmo plano.Considere-se o caso onde 4 pontos, obtidos pelo sistema, definem um plano no solo que na realidadecorresponde a buraco. A falta de informação, ou a não presença de pontos no interior desse plano dizao sistema que muito provavelmente não corresponde a uma boa zona de aterragem. Assim, apenasos planos contendo o maior número de pontos serão considerados pelo algoritmo como os que melhorse ajustam aos pontos em análise, sendo por isso considerados bons planos.

Este algoritmo é aplicado a todos os pontos do plano, ao passo que o algoritmo RANSAC escolhealeatoriamente 3 pontos de entre o total de pontos de P e aplica o algoritmo a esses 3 pontosescolhidos. A escolha aleatória de pontos é repetida um número N de vezes, que depende não só daprobabilidade de encontrar pelo menos um conjunto de pontos que defina um bom plano de entre ototal de pontos considerado, geralmente variando entre 0.9 e 0.99, mas também do número máximo depontos que é mais provável pertencerem ao mesmo plano, tendo em conta o número total.

Como o algoritmo de RANSAC depende de valores de probabilidade, e os pontos são escolhidosaleatoriamente, pode ocorrer o caso em que nenhum dos planos determinados contém um númeromínimo de pontos necessário para serem considerados bons planos. O algoritmo implementadocontém sempre o plano que seria obtido através do RANSAC. Porém, o contrário não é verdade,apesar na maior parte dos casos os resultados convergirem. Como neste caso o tempo deprocessamento não era uma questão importante, pois o processamento era feito após a obtenção dosvídeos, optou-se pelo algoritmo descrito. Contudo, o RANSAC permite uma maior rapidez, reduzindo onúmero de iterações determinadas e como tal seria a melhor escolha para implementar num sistemaem tempo real ou num sistema com um número elevado de pontos.

É necessário escolher de entre os bons planos obtidos o que melhor se apresenta como plano deaterragem. Um bom plano de aterragem corresponde a um plano definido por um ângulo ϕ igual ouaproximadamente igual a 90ž, considerando que o eixo Z se apresenta na direcção do solo. Assim,para cada instante é necessário analisar o valor de ϕ do plano obtido, e só se considera como bomplano de aterragem aquele cuja inclinação não varie mais de 5o para além do valor considerado ideal,assumindo que o referêncial da câmara na primeira imagem, onde são adquiridos os pontos, seencontra perfeitamente alinhado com o plano:

ϕland = 90o ± 5o (5.7)

É possivel constatar que, com um plano definido desta forma, para ϕ = 90o o ângulo θ torna-se umpouco irrelevante, não acrescentado nenhuma informação ao sistema, pois representa uma rotaçãoazimutal. Assim, é necessário encontrar uma outra forma de representação que permita definir umbom plano de aterragem através dos 3 parâmetros disponíveis, fazendo uso do máximo de informaçãopossível. Trocando o eixo X do plano com o eixo Z, um bom plano de aterragem passa a ser definidopor θ = ϕ = 0o, como é possível observar na figura 5.2. Desta forma é possível saber a orientaçãocompleta do plano, não desperdiçando qualquer tipo de informação. Apenas um plano cujos ângulos θe ϕ não variem mais do que 5o do valor ideal é considerado um bom plano de aterragem:

43

θland = 0± 5o (5.8)

ϕland = 0± 5o (5.9)

Este algoritmo baseia-se nos pontos obtidos pelo sistema para determinar o melhor plano que seadeque a estes. Desta forma, o erro associado à determinação do plano depende do erro associado àestimação da posição de cada ponto. É necessário para cada caso que os pontos utilizados sejampontos de confiança, com pouca incerteza associada, pois a determinação de zonas de aterragemexige a máxima precisão possível.

44

Capítulo 6

Resultados Experimentais

Foram realizados uma série de ensaios experimentais com vista a avaliar o método de localização emapeamento descrito no Capítulo 4, assim como o algoritmo de detecção de zonas de aterragem, apre-sentado no Capítulo 5. Estes ensaios foram realizados em ambientes controlados, e de forma sequen-cial, permitindo a completa e correcta implementação dos algoritmos apresentados. Ambos os algo-ritmos foram implementados em Matlab, que apesar de permitir obter resultados bastante conclusivos,apresenta algumas limitações em termos do tempo de processamento e da memória disponível paraarmazenar dados. Desta forma, não foi possível realizar os ensaios em tempo real, sendo necessária aobtenção prévia de videos de curta duração (aproximadamente 3 segundos), descrevendo movimentosrealizados pela câmara no espaço, para depois poderem ser processados.

Neste capítulo é feita uma descrição dos diferentes cenários utilizados nos ensaios experimentais esão apresentados os resultados referentes à avaliação de cada um dos algoritmos implementados.

6.1 Cenários

Os ensaios experimentais foram realizados em cenários previamente montados para esse efeito. Oprimeiro cenário consistia em dois painéis iguais, com comprimento C = 1m e largura L = 0.7m,colocados por forma a perfazer um ângulo de 90o entre eles. Cada painel continha um padrão em tonsde preto e branco, com rectângulos de dimensões 0.110m×0.105m, com vista a facilitar a determinaçãode pontos através do método descrito no Capítulo 3. Este cenário encontra-se representado na figura6.1:

Figura 6.1: Cenário 1

45

O segundo cenário consistia apenas num único painel, com as mesmas dimensões dos primeiros,colocado na vertical, tal como mostra a figura 6.2:

Figura 6.2: Cenário 2

Para se conseguir avaliar de forma correcta as medições obtidas, foram ainda colocadas marcaçõesjunto a cada um dos cenários, distanciadas 10cm umas das outras, na direcção paralela ao movimentoefectuado pela câmara em cada ensaio. O conhecimento das dimensões e medições correspondentesa cada um dos cenários e ensaios, ainda que aproximadas, permite posteriormente determinar o errodas estimações obtidas pelo sistema face às medições reais, como se verá mais à frente.

6.2 Factor de Escala

Na implementação do sistema em questão, todas as medições foram escaladas após terem sidoobtidas, por conhecimento das dimensões dos rectângulos pretos que aparecem nos padrões doscenários, apresentados na secção anterior. Isto pôde ser feito pois o sistema não se encontrava aoperar em tempo real, onde é necessário saber as medições reais em cada instante. Contudo, numsistema implementado em tempo real, a inicialização do sistema com pontos conhecidos ou autilização de outros sensores que permitissem eliminar a ambiguidade existente, tais como altímetrosou acelerómetros, corresponderiam às soluções a ser consideradas.

6.3 Resultados experimentais I - Avaliação do método de localiza-ção da câmara e mapeamento em simultâneo

Nesta secção são apresentados os resultados experimentais que permitem avaliar o método propostono Capítulo 4. Foram realizados dois ensaios, um em cada cenário proposto.

6.3.1 Ensaio experimental I

No primeiro ensaio experimental, realizado no cenário 1, a câmara encontrava-se inicialmente a cercade 0.25m do chão e a 1m da aresta de intersecção dos dois painéis. O movimento realizado pelacâmara consistiu em deslocá-la segundo o sentido positivo do seu eixo X, aproximadamente 0.50m,retornar à posição inicial e continuar o deslocamento segundo o mesmo eixo, mas no sentido negativo,aproximadamente 0.10m, após passagem pelo ponto inicial. Os valores do deslocamento da câmaracorrespondem a valores aproximados, obtidos a partir das marcações colocadas paralelamente ao seu

46

movimento, pois o movimento foi efectuado com a mão e por isso assume-se um erro real de mediçãona ordem dos 0.05m. O deslocamento real da câmara pode então ser descrito como:

• Instante inicial t0 = 0s: assume-se este ponto como a origem do referêncial principal

• Instante t = 1.50s: ponto de inversão de sentido, com x = 0.50± 0.05m

• Instante t = 2.57s: ponto onde a câmara passa novamente pela origem do referêncial

• Instante t = 3.0s: ponto correspondente ao final do movimento, com x = −0.10± 0.05m

Tendo os dados reais do movimento da câmara, é possível analisar os resultados obtidos através dosistema implementado.

A figura seguinte representa a posição da câmara em diferentes instantes de tempo considerados.

Figura 6.3: Posição da câmara real nos instantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo,t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Por sua vez, os pontos obtidos pelo sistema no instante inicial encontram-se representados na figura6.4:

Figura 6.4: Pontos obtidos pelo sistema no instante inicial

Todos estes pontos são mantidos no sistema até ao final, não sendo necessário apagar ou readquirirnovos pontos ao longo da simulação.

47

Consideremos a função densidade probabilidade para cada um dos parâmetros do sistema. Como jáfoi referido no Capítulo 4, a estimação de cada um destes parâmetros pode ser modelada através deuma função gaussiana, centrada no seu valor estimado e com variância dada através da matriz decovariância do sistema P . Desta forma é possível perceber e analisar a evolução das estimações dosparâmetros e do erro inerente ao longo do tempo. Inicialmente assume-se que a posição da câmara éconhecida, centrada na origem do referêncial principal, com uma variância nula. Os pontos por suavez são inicializados a uma profundidade de 10m, ou considerando o seu inverso, com um valor deρ = 0.1, correspondendo a este um desvio padrão relativamente elevado. As figuras 6.5 e 6.6 mostrama evolução da função densidade de probabilidade correspondente ao valor estimado da posição dacâmara no referêncial principal, ou seja, a estimação de x, y e z, em vários instantes da simulação.Estes valores foram escalados por forma a corresponderem às coordenadas reais da câmara em cadainstante e não aos valores estimados pelo sistema:

Figura 6.5: Função densidade de probabilidade para as estimações do parâmetro x da câmara, parat = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio x (m) 0.0031 0.1647 0.5465 0.3232 0.0431 −0.1288

Desvio padrão σx (m) 0.0048 0.0297 0.0439 0.0216 0.0121 0.0203

Tabela 6.1: Valores estimados para o parâmetro x da câmara, para t = 0.5 segundo, t = 1.0 segundo,t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

48

Figura 6.6: Função densidade de probabilidade para as estimações dos parâmetros y e z da câmara,para t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio y (m) 0.00063 −0.0352 −0.0304 0.0286 0.0598 0.0478

Desvio padrão σy (m) 0.0039 0.0137 0.0158 0.0115 0.0119 0.0160

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio z (m) −0.0145 −0.1068 −0.0833 −0.0540 −0.0167 −0.0479

Desvio padrão σz (m) 0.0177 0.0254 0.0224 0.0088 0.0050 0.0070

Tabela 6.2: Valores estimados para os parâmetros y e z da câmara, para t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Apesar de inicialmente a posição da câmara ser bem conhecida, a verdade é que com a introdução denovos pontos no sistema, a incerteza associada aos parâmetros da posição da câmara, assim comoaos outros parâmetros, aumenta substancialmente. Isto ocorre pois a câmara estima a sua posiçãoem cada instante baseando-se na informação dada através da medição dos pontos. Enquanto aincerteza associada a cada ponto não diminuir, a estimação dos parâmetros da câmara será sempreafectada pelo valor dessa incerteza. A figura 6.7 mostra o traço da matriz de covariância Pcorrespondente ao estado do sistema, para cada instante de tempo considerado:

Figura 6.7: Traço da matriz de covariância P correspondente ao estado do sistema

49

A observação da figura 6.7 permite ver que inicialmente a incerteza do sistema é muito grande, masque vai dimuindo à medida que o sistema vai avançando. Comparando a figura 6.5 com a figura 6.7,pois a componente x da posição da câmara corresponde ao parâmetro mais importante, repara-se queo valor do desvio padrão até ao instante t = 1.50s vai sendo cada vez maior, devendo-se em grandeparte ao processo de estabilização do sistema. Para a estimação de x no instante correspondente àinversão de sentido, é possível gerar um intervalo de confiança a 65%,[0.5026m; 0.5904m], que englobao valor real da estimação, tendo em conta o erro inerente à medição real. Contudo o intervalo idealseria aquele permitisse ter uma certeza de aproximadamente 95% com um desvio padrão de poucoscentímetros. Correpondente a este instante, o intervalo de confiança a 95% é de [0.4587m; 0.6343m],um intervalo um pouco grande tendo em conta o intervalo desejado para esse instante, obido a partirda medição real influenciada pelo erro real de medição, [0.45m; 0.55m]. Da mesma forma, para o valorestimado de x no instante final, onde o sistema já se encontra estabilizado, o intervalo de confiança a65%,[−0.1491m;−0.1085m], é grande suficiente para englobar o valor real mas não oferece totalsegurança no valor da estimação obtida. O mesmo intervalo a 95% de confiança é de[−0.1694m;−0.0882m], maior do que o intervalo desejado para esse instante, [−0.15m;−0.05m], eportanto, não corresponde a um intervalo óptimo, mas ainda assim aproximado.

A figura 6.8 mostra o valor de probabilidade associada à estimação do inverso da profundidade dospontos 1 e 15, escolhidos por se encontrarem a profundidades diferentes no cenário, tal como mostra afigura 6.4.

Figura 6.8: Função densidade de probabilidade para a estimação do inverso da profundidade dos pon-tos 1 e 15

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio ρ1 (m) 0.0491 0.2676 0.3433 0.3623 0.3505 0.3853

Desvio padrão σρ1 (m) 0.1472 0.0489 0.0274 0.0239 0.0225 0.0211

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio ρ15 (m) 0.0573 0.2298 0.3102 0.3294 0.3180 0.3634

Desvio padrão σρ15 (m) 0.1750 0.0479 0.0266 0.0233 0.0218 0.0201

Tabela 6.3: Valores estimados para o inverso da profundidade dos pontos 1 e 15, para t = 0.5 segundo,t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

50

Comparando os valores correspondentes ao desvio padrão destes dois pontos, é possível constatarque o desvio padrão para o ponto 1 é maior que o do ponto 15 em todos os instantes de tempo, aindaque esta diferença seja pequena. Isto ocorre pois a estimação da profundidade a que um ponto seencontra depende do ângulo de paralaxe, como foi referido no Capítulo 4. Contudo, apesar de seencontrar a uma maior profundidade, o ângulo de paralaxe para o ponto 15 é maior devido aodeslocamento efectuado pela câmara até ao instante t = 1.50s. Como a diferença de profundidadeentre o ponto 15 e o ponto 1 é pequena, na ordem dos centímetros, não se considera o ponto 15 comoum ponto longínquo. Neste caso, por este ponto se encontrar no centro da imagem no momento emque foi observado, o deslocamento efectuado pela câmara foi suficiente para produzir um melhorângulo de paralaxe. O ponto 1, apesar de se encontrar mais perto, encontrava-se mais deslocado docentro da imagem e portanto não sentiu tanto o deslocamento da câmara. Aos valores estimadosdestes parâmetros correspondem profundidades dos pontos que variam entre os 1.044m e os 1.107maproximadamente, sendo que o valor máximo corresponde à profundidade associada ao ponto 15.Tendo em conta que o ponto 15 se encontra junto à aresta de intersecção dos dois painéis, significaque a profundidade real a que se encontra é aproximadamente 1m, o que permite determinar o erroassociado a esta profundidade, de aproximadamente 0.102m. Analisando o valor do desvio padrãoassociado à estimação do inverso da profundidade do ponto 15, é possível gerar um intervalo deconfiança a 65% para a profundidade a que o ponto se encontra, [1.049m; 1.172m] intervalo este queengloba o valor real da profundidade, mas com um desvio ainda assim considerável. A estimação daposição x da câmara, assim como da profundidade dos pontos poderia ter sido melhorada se odeslocamento efectuado tivesse sido maior, reduzindo a incerteza do sistema a valores mais baixos.Apesar de ao ponto 15 corresponder o maior ângulo de paralaxe, a verdade é que o deslocamentosegundo o eixo x deveria ter sido aproximadamente de 1m, o que permitiria obter um ângulo deparalaxe de 45o, suficiente para reduzir substancialmente a incerteza no valor da estimação. A figura6.9 mostra a evolução das elipses de covariância para a profundidade dos pontos 1, 3, 7, 12 e 15, semescalamento, em vários instantes de tempo, representando no plano XZ a incerteza associada à suaestimação.

Figura 6.9: Evolução das elipses de covariância para os pontos 1, 3, 7, 12 e 15, nos instantes t = 0.5segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

É possível observar que a partir do instante t = 1.50s, correspondendo ao máximo deslocamento

51

efectuado pela câmara segundo o eixo x, ou ao máximo ângulo de paralaxe para cada ponto, osvalores estimados da profundidade praticamente estabilizaram, assim como os valores associados àssuas incertezas, que permaneceram aproximadamente inalteradas enquanto o deslocamento dacâmara não era suficiente para que estas voltassem a decrescer. É ainda possível observar a variaçãonos valores das incertezas assicadas a cada ponto, sendo maior para os pontos que menos sentiram odeslocamento da câmara, nomeadamente os que se encontram mais à esquerda.

Tentou-se que durante o ensaio, a câmara não sofresse qualquer deslocamento segundo o eixo Y e oeixo Z, para que o deslocamento global da câmara fosse o mais linear possível segundo o eixo X.Contudo, por ter sido efectuado com a mão, não foi possível evitar deslocar a câmara nos outros eixos,ainda que por valores considerados baixos. Analisando a figura 6.6, é possível observar que odeslocamento segundo o eixo Y varia aproximadamente entre os −0.035m e os 0.06m em torno daposição óptima, 0m. Segundo o eixo Z o deslocamento é um pouco maior, variando aproximadamenteentre os −0.11m e os 0.02m em torno do valor desejado, correspondendo novamente a 0m, erros quese assumem ser aceitáveis num movimento deste tipo. As figuras 6.10 e 6.11 mostram exactamente aevolução do movimento da câmara no plano horizontal XZ e no plano vertical XY , em váriosinstantes de tempo, onde todos os valores apresentados foram escalados.

Figura 6.10: Evolução do movimento da câmara no plano XZ, nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

52

Figura 6.11: Evolução do movimento da câmara no plano XY , nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Como é possível observar, o movimento da câmara ao longo do tempo corresponde em grandeaproximação ao movimento real, apesar da variância associada às estimações não corresponder aovalor desejado em cada instante. Tentou-se igualmente manter uma orientação fixa da câmara aolongo do tempo, evitando ao máximo provocar rotações em torno dos seus eixos principais. A figura6.12 mostra a evolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ eguinada ψ, da câmara ao longo do tempo:

Figura 6.12: Evolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ e guinadaψ, da câmara

É possível verificar que os valores se aproximam bastante dos valores considerados ideais, variandopouco em torno de 0o. Todas as variações sentidas são normais num movimento deste tipo, pelo quese considera aceitávels os valores obtidos.

Também as posições dos pontos no plano vertical XY correspondem a estimações aceitáveis, como épossível observar na figura 6.13, onde se mostra a evolução do movimento da câmara em simultâneocom a posição estimada de cada ponto, no plano XY , para vários instantes de tempo:

53

Figura 6.13: Evolução do movimento da câmara e da posição estimada dos pontos no plano XY nosinstantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo

É possível verificar que as relações entre as estimações e as medições reais são aproximadas, aindaque não óptimas, pois utilizou-se a dimensão real de um rectângulo pertencente ao padrão paraefectuar o escalamento. Comparando com a figura 6.4, confirma-se a proximidade dos pontos 4, 8, 13e 16 ao chão, pois inicialmente a câmara encontrava-se a 0.25m deste. Por a incerteza naprofundidade dos pontos, tal como a figura 6.9 indica, induzirem algum erro na estimação da posição xdos pontos no referêncial principal, o comprimento correspondente à zona do painel detectada tem umerro associado de aproximadamente 0.125m, sendo que o comprimento real é de aproximadamente0.44m e o comprimento estimado de 0.3147cm, tendo-se utilizado os pontos 1 e 15 para estimar essecomprimento. Tal como já foi referido, um maior deslocamento da câmara produziria melhoresresultados, reduzindo bastante o erro associado a cada estimação, pelo que se assume que o erroobtido é aceitável e compreensível. É preciso ter ainda em conta que o sistema em causa é não linear,como foi descrito no Capítulo 4 e por conseguinte, todas as estimações obtidas correspondem aestimações dos valores aproximados, obtidos através da linearização do sistema. Desta forma, o erroassociado à linearização, ainda que pequeno, pode ter alguma influência nos resultados aquiapresentados.

6.3.2 Ensaio experimental II

No segundo ensaio experimental, realizado no cenário 2, a câmara encontrava-se inicialmente a cercade 0.30m do chão e a 1.15m do painél colocado de forma a ficar aproximadamente paralelo ao planoXY do referêncial principal. O movimento realizado pela câmara consistiu novamente em deslocá-lasegundo o sentido positivo do seu eixoX, aproximadamente 0.90m, retornar à posição inicial e continuaro deslocamento segundo o mesmo eixo, no sentido negativo, aproximadamente 0.40m. Assim como noprimeiro ensaio, os valores do deslocamento da câmara correspondem a valores aproximados pelo quese assume novamente um erro real de medição na ordem dos 0.05m. Assim, o deslocamento real dacâmara pode ser descrito como:

• Instante inicial t0 = 0s: assume-se este ponto como a origem do referêncial principal

• Instante t = 2.0s: ponto de inversão de sentido, com x = 0.90± 0.05m

54

• Instante t = 2.67s : ponto onde a câmara passa novamente pela origem do referêncial

• Instante t = 3.0s: ponto correspondente ao final do movimento, com x = −0.40± 0.05m

A principal diferença entre este ensaio e o anterior corresponde ao facto de todos os pontos obtidospelos sistema se encontrarem aproximadamente à mesma profundidade no cenário. Também o deslo-camento efectuado pela câmara foi maior, comparado com o anterior, permitindo desta forma obter ummelhor ângulo de paralaxe para cada ponto, aproximadamente 38o no melhor caso.

A figura seguinte representa a posição da câmara em diferentes instantes de tempo considerados paraeste ensaio.

Figura 6.14: Posição da câmara real nos instantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo,t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Os pontos obtidos pelo sistema no instante inicial encontram-se representados na seguinte figura:

Figura 6.15: Pontos obtidos pelo sistema no instante inicial

Assim como no ensaio anterior, também estes pontos são mantidos no sistema, até ao final dasimulação. Como a inicialização do sistema é igual à do primeiro ensaio, não será novamente descrita.

As funções densidade probabilidade para cada um dos parâmetros estimados, escalados, que definema posição da câmara, encontram-se representadas nas figuras 6.16 e 6.17:

55

Figura 6.16: Função densidade de probabilidade para as estimações do parâmetro x da câmara, parat = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio x (m) 0.0008 0.0185 0.1994 0.9031 0.2601 −0.4064

Desvio padrão σx (m) 0.0060 0.0072 0.0219 0.0656 0.0167 0.0208

Tabela 6.4: Valores estimados para o parâmetro x da câmara, para t = 0.5 segundo, t = 1.0 segundo,t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Figura 6.17: Função densidade de probabilidade para as estimações dos parâmetros y e z da câmara,para t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio y (m) −0.00088 0.0101 0.0422 0.0924 0.1396 −0.0013

Desvio padrão σy (m) 0.0073 0.0056 0.0123 0.0287 0.0145 0.0157

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio z (m) −0.0022 0.0424 −0.0768 0.0398 0.0383 0.0040

Desvio padrão σz (m) 0.0283 0.0141 0.0149 0.0244 0.0069 0.0098

Tabela 6.5: Valores estimados para os parâmetros y e z da câmara, para t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

56

O sistema, para este ensaio, estabiliza a partir do instante de tempo t = 2.0s, correspondente ao pontode inversão de sentido da câmara. O ponto de estabilização relativa do sistema corresponde sempreao ponto de maior deslocamento da câmara, quer no primeiro como no segundo ensaio, pois é paraeste ponto que o ângulo de paralaxe é maximizado. A estabilização é relativa pois se o movimentocontinuasse segundo o mesmo sentido do eixo, haveria de chegar a um instante onde a incertezaassociada a cada ponto seria aproximadamente nula, devido ao ângulo de paralaxe ser óptimo paratodos os pontos do sistema. Esse instante corresponderia assim ao ponto de estabilização absolutado sistema. Contudo para este caso, o instante de estabilização relativa correspondeaproximadamente ao instante de estabilização absoluta do sistema. A figura 6.18 mostra a evoluçãodo traço da matriz de covariância do sistema ao longo do tempo:

Figura 6.18: Traço da matriz de covariância P correspondente ao estado do sistema

Analisando o valor estimado da posição x da câmara no instante t = 2.0s, imediatamente antes daestabilização do sistema, verifica-se que este varia aproximadamente 0.0031m do valor real. Contudo,o desvio padrão associado a esta estimação é de aproximadamente 0.0656m, um valor ainda assimelevado se considerarmos um intervalo de confiança a 95%, [0.7719; 1.0343], que não permite ter ograu de certeza desejado, quando comparado ao intervalo de medição real [0.85; 0.95], obtido a partirdo erro de medição real de 0.05m. Por sua vez, no instante final, após o sistema estar estabilizado, ointervalo de confiança a 95% corresponde a [−0.448;−0.3648] para o valor estimado de x nesseinstante. Tendo em conta o erro de medição real, o intervalo correspondente à medição real é[−0.45;−0.35], bastante aproximado do intervalo de confiança obtido. Por estes motivos, considera-seque a partir do instante de estabilização, a estimação da posição x da câmara é óptima.

As funções densidade de probabilidade relativas à profundidade inversa dos pontos 11 e 16encontram-se representadas na figura 6.19:

57

Figura 6.19: Função densidade de probabilidade para a estimação do inverso da profundidade dospontos 11 e 16

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio ρ11 (m) 0.0973 0.0768 0.2832 0.3646 0.4165 0.4193

Desvio padrão σρ11 (m) 0.2086 0.0488 0.0273 0.0202 0.0141 0.0134

Instante de tempo t (s) 0.5 1.0 1.5 2.0 2.5 3.0Valor médio ρ16 (m) 0.0402 0.2174 0.2301 0.3432 0.3882 0.3951

Desvio padrão σρ16 (m) 0.1586 0.0434 0.0332 0.0194 0.0136 0.0128

Tabela 6.6: Valores estimados para o inverso da profundidade dos pontos 11 e 16, para t = 0.5 segundo,t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

A escolha destes pontos deveu-se ao facto do painel utilizado não se encontrar completamente navertical, ainda que muito próximo dessa posição. Considerando a estimação do inverso daprofundidade a partir do instante t = 2.0s, é possível observar um decréscimo no valor do desviopadrão associado a cada estimação. A estimação final da profundidade para cada ponto é umaestimação muito boa, sendo de 1.167m para o ponto 11, com intervalo de confiança a 95% de[1.0827m; 1.2305m] e de 1.223m para o ponto 16, com intervalo de confiança de [1.148m; 1.307m].Considerando assim um possível erro de inclinação do painel, considera-se que os valores estimadosobtidos correspondem a valores muito próximo dos reais para a profundidade destes pontos.

A figura seguinte mostra a evolução das elipses de covariância, no plano XZ, representando aincerteza na estimação da profundidade dos pontos 2, 8, 14 e 20, sem escalamento:

58

Figura 6.20: Evolução das elipses de covariância para os pontos 2, 8, 14 e 20, nos instantes t = 0.5segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Como o ponto 20 corresponde a um dos pontos com maior ângulo de paralaxe, a incerteza associadaà estimação da sua profundidade é mais baixa que a do ponto 2, para o qual o deslocamento dacâmara produziu um menor ângulo. O valor estimado da sua profundidade é de aproximadamente1.1695m, com um intervalo de confiança a 95% de [1.1333m; 1.2081m], ao passo que para o ponto 2 ovalor estimado é de 1.199m, com um intervalo no mesmo nível de confiança de [1.12823m; 1.2815m].Apesar de ambas as estimações darem alguma margem de confiança, é claramente visível ainfluência do deslocamento da câmara nas estimações obtidas pelos sistema.

Tal como no primeiro ensaio, tentou-se deslocar o mínimo possível a câmara segundo os outros doiseixos, para que o movimento fosse o mais paralelo possível ao painel. Contudo esta sofreu semprealgum deslocamento indesejado, como é possível verificar na figura 6.17. A câmara sofreu umdeslocamento máximo de 0.1396m segundo o eixo Y , para o instante t = 2.50s, valor que se assumeser bem estimado devido ao valor correspondente do desvio padrão. Segundo o eixo Z, odeslocamento máximo realizado pela câmara correspondeu a 0.0768m no sentido negativo do eixo,também este com um desvio padrão bastante baixo. As figuras seguintes mostram o deslocamento dacâmara nos planos XZ e XY , para vários instantes de tempo, tendo em conta que todos os valoresrepresentados foram escalados:

59

Figura 6.21: Evolução do movimento da câmara no plano XZ, nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

Figura 6.22: Evolução do movimento da câmara no plano XY , nos instantes t = 0.5 segundo, t = 1.0segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0 segundo

É possível observar que o movimento estimado da câmara se aproxima muito do movimento real,sendo considerado óptimo a partir do instante t = 2.50s, com valores de variância muito próximos dezero. Também neste ensaio se tentou que a orientação da câmara no espaço se mantive-se sempreconstante e não variasse muito em torno dos valores desejados. A figura 6.23 mostra mais uma vez aevolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ e guinada ψ, dacâmara ao longo do tempo:

60

Figura 6.23: Evolução dos valores estimados para os ângulos de Euler, rolamento φ, picada θ e guinadaψ, da câmara

Tal como no primeiro ensaio, considera-se que estas variações são perfeitamente aceitáveis para ummovimento deste tipo. Os valores oscilam em torno de 0o, não havendo picos nos gráficos, apesar daoscilação ser um pouco maior para o ângulo de picada, ou rotação em torno do eixo Y , o que secompreende pelo natural movimento do pulso durante o movimento da câmara. Desta formaconsidera-se que estes valores são bastante razoáveis.

A estimação da posição dos pontos no plano XY é também considerada óptima, e encontra-serepresentada na seguinte figura, em simultâneo com o movimento da câmara no mesmo plano:

Figura 6.24: Evolução do movimento da câmara e da posição estimada dos pontos no plano XY nosinstantes t = 0.5 segundo, t = 1.0 segundo, t = 1.5 segundo, t = 2.0 segundo, t = 2.5 segundo e t = 3.0segundo

Comparando esta figura com a figura 6.15 é possível observar que as relações das distâncias entre ospontos correspondem muito aproximadamente às relações reais. Mais uma vez se confirma aproximidade dos pontos 4, 10 e 16 ao chão, tendo em conta a altura inicial a que a câmara seencontrava. As dimensões reais da zona determinada correspondem a uma largura de 0.33m, medidaentre os pontos 2 e 20, e uma altura de 0.515m, medida entre os pontos 7 e 10, ao passo que os valoresestimados dessas dimensões são de 0.3319m para a largura e 0.468m para a altura, correspondendo aerros relativamente baixos. Assim como no primeiro ensaio, também neste os erros de linearização dosistema podem ter afectado as estimações obtidas, que ainda assim se consideram bastante boas.

61

6.4 Resultados Experimentais II - Validação e avaliação do algo-ritmo de detecção de zonas de aterragem

Antes de poder ser aplicado aos resultados obtidos na secção 6.3, foi necessário validar o algoritmodescrito no Capítulo 5. Numa primeira fase o algoritmo foi aplicado a conjuntos de pontos aleatórios,dispersos no espaço de forma a que a zona correspondente à máxima concentração de pontos, emcada caso, permitisse a obtenção de planos. Numa segunda fase, e após o algoritmo ser validado, estefoi aplicado aos dois ensaios realizados, com vista à determinação do melhor plano para os pontosestimados, cujas características fossem as desejadas para uma boa zona de aterragem.

6.4.1 Fase I: Teste do algoritmo

Para o teste do algoritmo foram geradas três nuvens de pontos, com 100 pontos cada, contendo emcada caso uma grande concentração de pontos em torno de x = 10, y = 10 e z = 10, correspondendoem cada caso a 75% do total de pontos. As figuras seguintes mostram a distribuição de pontos noespaço para estes três casos:

Figura 6.25: Nuvem de pontos concentrada em torno de x = 10, com vistas no plano XY , XZ e Y Z,respectivamente

Figura 6.26: Nuvem de pontos concentrada em torno de y = 10, com vistas no plano XY , XZ e Y Z,respectivamente

62

Figura 6.27: Nuvem de pontos concentrada em torno de z = 10, com vistas no plano XY , XZ e Y Z,respectivamente

Idealmente, o plano definido para o caso em que x é constante teria como parâmetros, emcoordenadas esféricas, ρ = 10 e θ = ϕ = 0o. Da mesma forma, os planos para os outros casos seriamdefinidos por ρ = 10, θ = 90o e ϕ = 0o, no caso de y constante e ρ = 10 e ϕ = 90o no caso de zconstante, sendo que para este último o valor de θ não acrescenta informação nova, pois representaapenas a rotação em torno do eixo z, como foi descrito no Capítulo 5.

Aplicando o algoritmo a estes casos, obtiveram-se três planos bem definidos, englobando mais de 50%dos pontos considerados. As figuras seguintes mostram os planos obtidos através do algoritmoproposto:

Figura 6.28: Nuvem de pontos concentrada em torno de x = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente

Figura 6.29: Nuvem de pontos concentrada em torno de y = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente

63

Figura 6.30: Nuvem de pontos concentrada em torno de z = 10 e plano determinado, a verde, comvistas no plano XY , XZ e Y Z, respectivamente

No caso em que x é constante, o plano obtido é caracterizado por um valor de ρ = 10.1366, deθ = −2.7456o e de ϕ = −11.9924o. Os valores dos ângulos obtidos, ainda que não sendo próximos dezero, são baixos e por isso considerados aceitáveis, assim como o valor de ρ, cujo erro é deaproximadamente 0.1366, em comparação com o valor ideal. Para y constante, tem-se ρ = 10.1634,θ = 89.5863o e ϕ = 9.9608o. Idealmente, o valor de θ para um plano paralelo a XZ é de 90o, o queperfaz um erro de aproximadamente 0.4137o na determinação deste parâmetro. No caso de ϕ, o seuvalor é baixo, ainda que não corresponda ao valor ideal. No caso do valor de ρ, o erro associado é deaproximadamente 0.1634, um valor baixo e bastante razoável. No último caso, para z constante,tem-se como parâmetros do plano obtido ρ = 10.6147, θ = 53.8522o e ϕ = 84.6221o. Como foi referidoanteriormente, o valor de θ não é importante na determinação de planos para z constante, quando seutiliza esta representação. Assim, considerando apenas os outros parâmetros, há um erro associadode aproximadamente 0.6147, no caso de ρ e de 5.3779o no caso de ϕ. Todos os erros associados àdeterminação dos planos são considerados aceitáveis, na medida em que é definida uma distânciamínima ao plano, para cada ponto do plano, que depende da precisão com que se pretendedeterminar o plano. Neste caso, a distancia mínima considerada era de 0.75, porém no caso dadeterminação de zonas de aterragem, pretende-se que a precisão seja a maior possivel. Assim,considera-se que o algoritmo permite obter bons resultados, quando os pontos são favoráveis àobtenção de zonas planas, podendo a partir deste momento ser aplicado a casos específicos.

6.4.2 Fase II: Determinação de zonas planas reais a partir dos ensaios realiza-dos

Tendo testado com sucesso o algoritmo, foi possível passar à fase seguinte, de determinação de zonasplanas nos casos apresentados na secção anterior. Como o objectivo é a determinação de zonas deaterragem, a precisão na sua determinação é crucial para que o veículo aerospacial possa aterrar emcondições. Assim, para os dois casos, considerou-se um erro máximo de distância dos pontos ao planode 0.01m, um erro considerado aceitável numa estimação deste tipo. O primeiro ensaio não permitiu tercerteza absoluta da profundidade a que os pontos se encontravam, por o deslocamento efectuado pelacâmara não ter sido suficiente. A zona plana obtida para esse caso, através deste algoritmo, encontra-se representada na seguinte figura, em comparação com a localização estimada dos pontos que deramorigem ao plano gerado. É também representado o movimento global da câmara, para facilitar a análise:

64

Figura 6.31: Zona plana real, determinada para a primeira experiência e pontos estimados, com vistano plano XZ e XY , respectivamente

Tendo em conta a parametrização alternativa apresentada no Capítulo 5, com inversão dos eixos X eZ, este plano apresentava na realidade um valor de θ de aproximadamente 45± 5o e de ϕ deaproximadamente 0o ± 5o, em relação à câmara no instante inicial, considerando um erro de mediçãoreal de 5o. Contudo o deslocamento da câmara não foi suficiente para produzir o ângulo de paralaxeóptimo, necessário para reduzir ao máximo a incerteza na estimação dos pontos, acabando porinfluenciar assim os resultados obtidos. Desta forma, o erro na estimação dos pontos acabou por terinfluência directa na determinação do plano. Os pontos 1, 3, 5, 6, 7, 8, 11, 12, 13, 16 e 18, a verde naimagem, correspondem aos pontos pertencentes ao plano, ou seja, que se encontram a menos de0.01m deste, o que por análise e conhecimento do caso real, corresponde inteiramente à verdade. Damesma forma se verifica que os pontos 19 e 20 não pertencem ao plano determinado, pois narealidade estes fazem parte do segundo painel, que faz um ângulo de 90ocom o primeiro. Contudo, osvalores para os ângulos θ e ϕ do plano não correspondem aos valores reais, apresentando neste casovalores de aproximadamente 11.9775o e 21.2374o, respectivamente. Como é possível observar, háalguma inclinação no plano mas o erro associado é ainda grande em comparação com os valoresreais, sendo de aproximadamente 33.0225o no caso de θ e 21.2374o no caso de ϕ. É necessário ter emconta que no instante inicial, a orientação da câmara no espaço poderia ter algum erro associado, istoé, inicialmente a câmara poderia não estar a observar o plano a partir de um ângulo θ de 45o mas simcom uma outra inclinação desconhecida, que poderia ser maior ou menor do que a idealmenteconsiderada. Por sua vez, a distância correspondente à normal ao plano é de 0.9804m, permitindouma variação na profundidade dos pontos entre 0.94m e 1.12m, valores que se aproximam bastantedos valores estimados para os pontos 1 e 18, de 1.044m e 1.084m, correspondendo ao ponto maispróximo e mais distante, respectivamente. Desta forma, e tendo por base a estimação dos pontos comas incertezas associadas, considera-se que o algoritmo produziu o melhor plano para este caso,apesar de as suas características se afastarem um pouco das do plano real.

O segundo ensaio experimental, por sua vez, permitiu ter uma grande certeza na estimação daprofundidade a que os pontos se encontravam, pois o deslocamento da câmara foi suficiente paraproduzir grandes ângulos de paralaxe. Este caso corresponde ao caso ideal, onde todos os pontos se

65

encontram à mesma profundidade, pertencendo ao mesmo plano, que se apresenta praticamente navertical. O facto de o eixo Z da câmara se apresentar perpendicular ao plano significa que, numaimplementação real, este plano corresponderia a uma boa zona de aterragem, tendo em conta aorientação da câmara no quadrirotor.

A seguinte figura mostra a zona plana obtida para este caso, em comparação com a localizaçãoestimada dos pontos que permitiram obter essa zona:

Figura 6.32: Zona plana real, determinada para a segunda experiência e pontos estimados, com vistano plano XZ e XY , respectivamente

Neste caso, a inclinação real do plano correspondia a um ângulo ϕ e θ de aproximadamente 0± 5o,considerando novamente o erro na medição real e a parametrização alternativa do plano. Esteencontrava-se ainda a uma distância real de 1.15m da câmara no instante inicial. Observando o planoobtido, percebe-se que a boa estimação dos pontos permitiu gerar uma zona plana cujascaracterísticas são bastante próximas das reais. Apesar de todos os pontos pertencerem ao mesmoplano real, o valor da distância mínima de cada ponto estimado ao plano é considerado um valorbastante conservador, pois só assim se obtém um plano com a máxima precisão. Desta forma, porobservação da figura 6.32, os pontos considerados como pertencendo ao plano, a verde,correspondem apenas aos pontos 1, 2, 5, 7, 8, 9, 11, 12, 13, 14, 15 e 20, o que não implica que adistância dos outros pontos ao plano varie mais do que 0.005m para além do limite estipulado. Osângulos θ e ϕ do plano obtido através do algoritmo correspondem a 4.4811o e 0.3402o,respectivamente. Comparando estes valores com os valores reais, é possível ver que o erro associadoé muito baixo, sendo da ordem de grandeza do erro real de medição. Por sua vez, o valorcorrespondente a ρ é de 1.1454m, permitindo uma variação na profundidade dos pontos pertencentesao plano entre 1.135m e 1.16m, valores considerados óptimos, tendo em conta a distância real a que oplano se encontrava. A partir da análise feita, é possível afirmar que o plano obtido corresponde aomelhor plano possível tendo em conta os pontos obtidos e representa com grande aproximação oplano real, dando segurança suficiente para ser considerado um bom plano de aterragem.

66

6.5 Discussão dos resultados obtidos

Através da análise dos resultados apresentados na secção 6.3, claramente se considera que os resul-tados obtidos no segundo ensaio experimental foram melhores que os do primeiro, em grande partedevido ao maior deslocamento efectuado pela câmara. Isto evidencia a dificuldade em determinara profundidade a que pontos se encontram, sendo necessário sempre ângulos de paralaxe grandes,que permitam obter valores estimados bastante aproximados. Estes ensaios permitiram assim testar ométodo de localização e mapeamento em simultâneo, onde se considera que os resultados obtidos sãobastante bons, cujas estimações se aproximam bastante dos valores reais, principalmente no segundocaso considerado. A boa estimação de parâmetros, nomeadamente a localização correcta dos pontosno espaço, é a principal base para que o algoritmo de detecção de zonas planas consiga gerar boaszonas de aterragem, com uma grande precisão.

A obtenção de zonas planas depende do valor estimado dos pontos no instante de menor incerteza.Como tal, se esse valor de incerteza não for baixo o suficiente para que a estimação seja consideradaóptima, os planos obtidos poderão não corresponder inteiramente ao caso real, como aconteceu noprimeiro caso. Por sua vez, no segundo caso o valor de incerteza associado à estimação dos pontospermitiu que o plano gerado se aproximasse bastante do plano real, sendo que neste caso o plano seapresentava numa posição aproximadamente vertical, ou perpendicular, em relação à câmara.

Os ensaios realizados na secção 6.4 permitiram desta forma testar o algoritmo de detecção de planose considera-se que o teste foi bem sucedido, com erros associados muito baixos. O algoritmo emquestão é capaz de detectar zonas planas no mundo, que possam ser consideradas ou não zonas deaterragem, dependendo dos pontos que definem cada uma dessas zonas.

Estes resultados permitem afirmar com alguma segurança que os algoritmos implementadosproduzem bons resultados, sendo que um sistema deste tipo fornece bastante informação ao veículoonde se encontra incorporado. Mais uma vez se salienta que não havia outros sensores associados aeste sistema, pelo que os erros de estimação, ainda que baixos, poderiam ser ainda mais reduzidoscom a sua introdução.

67

Capítulo 7

Conclusões

Nesta tese implementou-se, com sucesso, um sistema que consegue determinar, com alguma precisão,o movimento tridimensional efectuado por uma única câmara, assim como a localização dos pontos dereferência utilizados por esta como forma de localização. Contudo, ficou bem patente a dificuldadeem determinar a profundidade a que pontos se encontram, a partir da análise das imagens obtidascom uma única câmara. O deslocamento efectuado por esta é muito importante na medida em quese não for suficiente para produzir bons ângulos de paralaxe, não é possível reduzir a incerteza naestimação dos pontos a valores considerados óptimos. Em alguns sistemas, o uso de visão stereo,com a utilização de duas câmaras cuja distância e ângulo entre elas são conhecidos, permite que aprofundidade de pontos dentro de um certo raio de visão seja bem determinada à partida, sem sernecessário um deslocamento do sistema. Esta certeza na estimação inicial da profundidade forneceassim informação correcta e segura que permite determinar bem o seu deslocamento desde o inicio,pois a incerteza associada aos pontos iniciais é praticamente nula.

Implementou-se ainda um outro algoritmo, também este com êxito, que utiliza esses mesmos pontosde referência para determinar zonas planas no mundo que possam ser consideradas possíveis zonasde aterragem para veículos aerospaciais não tripulados. A capacidade do sistema de detectar estaszonas sem qualquer tipo de intervenção humana fornece autonomia suficiente para que este possaoperar em zonas desconhecidas sem correr o risco de necessitar aterrar e não poder, por não terinformação suficiente sobre o terreno.

Apesar de os resultados obtidos serem bastante bons, este sistema foi testado apenas em ambientescontrolados, onde o movimento da câmara era conhecido e considerado lento. O sistema não foi aindatestado num caso real, nomeadamente aplicado a um quadrirotor, onde a variação entre imagens éconsideravelmente maior. Algumas adaptações e implementações teriam de ser feitas para que aresposta do sistema, nessas condições, fosse a melhor possível. Desta forma considera-se que otrabalho efectuado, ainda que que contendo um factor de sucesso elevado para os objectivospropostos até ao ponto considerado, não se encontra de todo completo.

7.1 Trabalho futuro

O trabalho, a ser melhorado, não incide no melhoramento do sistema em si, ou dos algoritmos, quecomo já foi demonstrado, produzem respostas bastante boas. Incide sim na implementação destesistema que forneça respostas em tempo real, pois de outra forma não seria possível aplicá-lo a umquadrirotor, que necessita de informação constante em cada instante. O sistema apresentado deter-mina zonas de aterragem com alguma precisão, mas essa informação necessita também ela de ser

68

incorporada num controlador, por forma a que o quadrirotor consiga aterrar de forma adequada. Odesenvolvimento desse controlador, assim como a implementação deste sistema em tempo real noquadrirotor são pontos importantes a ser desenvolvidos, caso se pretenda uma aplicação completa ereal do sistema em causa.

69

Anexo I

Neste anexo é apresentado o conceito de quaternião, ferramenta matemática normalmente usada comosubstituta dos ângulos de Euler para descrever orientações tridimensionais de corpos rígidos no espaçoe usada no desenvolvimento desta tese. É também apresentada a relação entre os ângulos de Euler eos quaterniões.

Quaterniões

Normalmente, a determinação e representação da orientação tridimensional de um corpo rígido é feitaatravés dos ângulos de Euler, obtidos a partir das velocidades angulares do corpo. Apesar de con-seguirem descrever bem a sua orientação, a verdade é que para serem obtidos, recorre-se a funçõestrigonométricas que, em comparação com operações aritméticas usuais como adição e multiplicação,são muito lentas em termos computacionais. Desta forma, os quaterniões surgem como uma alterna-tiva segura que permite obter a matriz de rotação do corpo com uma maior rapidez computacional. Osquaterniões podem ser vistos como uma extensão dos números complexos, sendo que a forma usualde representar um quaternião encontra-se descrita na equação seguinte:

q = q0 + i.qx + j.qy + k.qz (7.1)

Um quaternião é assim constituído por uma parte real, q0, e por outras três componentes que formamum vector no espaço imaginário ijk, qx, qy e qz.

Uma das restrições impostas a um quaternião é a de ter sempre norma unitária, ou seja:

||q|| =√

(q20 + q2x + q2y + q2z) = 1 (7.2)

Assim, é possível definir um quaternião como a representação da rotação tridimensional de um corporígido de um ângulo θ em torno de um vector unitário a, com componentes ax, ay e az, tal como mostraa figura 7.1, sendo que:

q0 = cos(θ

2

)(7.3)

qx = ax. sin(θ

2

)(7.4)

qy = ay. sin(θ

2

)(7.5)

qz = az. sin(θ

2

)(7.6)

70

Figura 7.1: Rotação de um corpo rigido (cubo a tracejado) de um angulo θ (a azul) em torno de umvector a, unitário (a amarelo).

O ângulo θ é definido a partir da rotação do corpo em torno dos três eixos principais. Sejam rotx, rotye rotz as rotações sofridas pelo corpo em torno dos três eixos principais. Então, o ângulo θ é definidopor:

θ =√

(rotx)2 + (roty)2 + (rotz)2 (7.7)

Assim como o ângulo θ, também o vector a em torno do qual será aplicada a rotação do corpodepende das rotações deste em torno dos eixos. Desta forma as suas componentes são dadas por:

ax = rotx (7.8)

ay = roty (7.9)

az = rotz (7.10)

Excepto no caso em que as rotações em torno de cada um dos eixos principais é igual a 1√3

rad, estevector não terá norma unitária, e por isso, é necessário adaptar estas equações para que a restriçãoacima descrita seja sempre respeitada. Assim, teremos que normalizar sempre cada componente dovector a, o que faz com que as componentes do quaternião tomem a seguinte forma:

q0 = cos(θ

2

)(7.11)

qx =ax||a||

. sin(θ

2

)(7.12)

qy =ay||a||

. sin(θ

2

)(7.13)

qz =az||a||

. sin(θ

2

)(7.14)

71

com:

||a|| =√a2x + a2

y + a2z (7.15)

Assim garante-se sempre que a restrição é respeitada, pois a norma do quaternião será sempreunitária, independentemente do vector a considerado. Tendo todas as componentes do quaterniãobem definidas, é possível obter por fim a matriz de rotação Rq que descreve a rotação efectuada pelocorpo no espaço tridimensional:

Rq =

q20 + q2x − q2y − q2z −2q0qz + 2qxqy 2qxqz + 2q0qy2qxqy + 2q0qz q20 − q2x + q2y − q2z −2q0qx + 2qyqz−2q0qy + 2qxqz 2qyqz + 2q0qx q20 − q2x − q2y + q2z

(7.16)

Esta matriz é análoga à matriz de rotação Re obtida a partir dos ângulos de Euler, dada por:

Re =

cos θ cosψ − cosφ sinψ + sinφ sin θ cosψ sinφ sinψ + cosφ sin θ cosψcos θ sinψ cosφ cosψ + sinφ sin θ sinψ − sinφ cosψ + cosφ sin θ sinψ− sin θ sinφ cos θ cosφ cos θ

(7.17)

onde φ corresponde ao ângulo de rotação em torno do eixo x, θ corresponde ao ângulo de rotação emtorno do eixo y e ψ corresponde ao ângulo de rotação em torno do eixo z, e descreve a orientação docorpo rígido num referencial fixo.

Relação entre Quaterniões e Ângulos de Euler

Apesar de permitirem uma maior rapidez na determinação da matriz de rotação, a verdade é que porinspecção directa das componentes de um quaternião não é possivel perceber qual a rotação sofridapelo corpo. Os angulos de Euler têm a particularidade de permitirem perceber com maior facilidadequal a rotação efectuada sobre cada um dos eixos principais, bastando para isso analisar os valores deφ, θ e ψ. Sendo que as matrizes 7.16 e 7.17 são análogas, é possivel estabelecer uma correspondenciaentre os valores de φ, θ e ψ e as componentes do quaternião. Assim, os angulos de Euler são dadospor:

ψ = arctan(Rq (2, 1)Rq (1, 1)

)(7.18)

θ = arctan(

−Rq (3, 1)cosψ.Rq (1, 1) + sinψ.Rq (2, 1)

)(7.19)

φ = arctan(

sinψ.Rq (1, 3)− cosψ.Rq (2, 3)cosψ.Rq (2, 2)− sinψ.Rq (1, 2)

)(7.20)

72

Anexo II

A informação contida neste anexo é complementar à fornecida no Capítulo 4, pelo que algumas equaçõesaqui referidas, por se encontrarem definas nesse capítulo, não são definidas novamente.

Determinação da jacobiana ∂yi∂xv

Com excepção dos parâmetros xWci, yWci

e zWci, todos os outros parâmetros do vector de estado do

ponto não dependem directamente dos parâmetros que constituem o vector de estado da câmara,sendo que o parâmetro ρi não depende de nenhum directa ou indirectamente. Por esse motivo énecessário determinar derivadas intermédias. O primeiro passo na determinação da jacobiana ∂yi

∂xv

consiste em obter a variação dos parâmetros θi e φi com as coordenadas do ponto no referêncialprincipal:

∂θi∂nWud

=[∂θi∂nWx

,∂θi∂nWy

,∂θi∂nWz

](7.21)

∂φi∂nWud

=[∂φi∂nWx

,∂φi∂nWy

,∂φi∂nWz

](7.22)

De seguida determinam-se as derivadas parciais da matriz de rotação RWC , em relação a cadacomponente do quaternião qWC :

∂RWC

∂qWC0

=

−2q0 −2qz 2qy2qz 2q0 −2qx−2qy 2qx 2q0

(7.23)

∂RWC

∂qWCx

=

2qx 2qy 2qz2qy −2qx −2q02qz 2q0 −2qx

(7.24)

∂RWC

∂qWCy

=

−2qy 2qx 2q02qx 2qy 2qz−2q0 2qz −2qy

(7.25)

∂RWC

∂qWCz

=

−2qz −2q0 2qx2q0 −2qz 2qy2qx 2qy 2qz

(7.26)

Utilizando as derivadas definidas em 7.23, 7.24, 7.25 e 7.26 e as coordenadas do ponto no referêncialda câmara é possível determinar a variação das coordenadas do ponto no referêncial principal, nWudcom as componentes do quaternião qWC :

73

∂nWud∂qWC

=[

∂RW C

∂q0hCud

∂RW C

∂qxhCud

∂RW C

∂qyhCud

∂RW C

∂qzhCud

](7.27)

Assim, é possível obter a variação dos parâmetros θi e φi com as componentes do quaternião qWC ,combinando as derivadas determinadas em 7.21, 7.22 e 7.27:

∂θi∂qWC

=∂θi∂nWud

∂nWud∂qWC

(7.28)

∂φi∂qWC

=∂φi∂nWud

∂nWud∂qWC

(7.29)

Directa ou indirectamente, apenas os parâmetros θi e φi do vector de estado do ponto variam com ascomponentes do quaternião, pelo que a derivada parcial do vector de estado yi em relação aoquaternião tem entradas todas nulas excepto para estes dois parâmetros, cujas derivadas são asdeterminadas em 7.28 e 7.29:

∂yi∂qWC

=

0 0 0 00 0 0 00 0 0 0

· · · ∂θi

∂qW C · · ·· · · ∂φi

∂qW C · · ·0 0 0 0

(7.30)

A variação do vector de estado do ponto yi com os parâmetros do vector de estado da câmara quedefinem a sua posição, rW , é directa e é dada por:

∂yi∂rW

=

1 0 00 1 00 0 10 0 00 0 00 0 0

(7.31)

Todas as outras derivadas são nulas, ou seja, nenhum dos parâmetros do vector de estado do pontovaria directa ou indirectamente com a velocidade linear ou angular da câmara, pelo que a jacobiana∂yi

∂xvvem finalmente dada por:

∂yi∂xv

=

...∂yi

∂rW

...

...∂yi

∂qW C

...

0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

(7.32)

74

Determinação da jacobiana ∂yi∂hid

Considere-se um vector de estado y′i, que contém todos os parâmetros do vector de estado do ponto,yi, excepto o inverso da profundidade ρi, pois este não varia, directa ou inderectamente, com asmedições efectuadas do ponto. Como os parâmetros do vector de estado do ponto não variamdirectamente com a medição efectuada, ou seja, com as coordenadas do ponto no plano da imagem,com distorção, hid, é necessário recorrer novamente a derivadas intermédias. Assim, o primeiro passoconsiste em determinar a variação do vector de estado y′i com as coordenadas do ponto no referêncialprincipal, obtidas sem distorção:

∂y′i∂nWud

=

0 0 00 0 00 0 0· · · ∂θi

∂nWud

· · ·· · · ∂φi

∂nWud

· · ·

(7.33)

onde ∂θi

∂nWud

e ∂φi

∂nWud

são dadas nas equações 7.21 e 7.22.

Por sua vez, a variação das coordenadas do ponto no referêncial principal com as coordenadas doponto no referêncial da câmara, sem distorção, é dada por:

∂nWud∂hCud

= RWC (7.34)

A variação das coordenadas do ponto no referêncial da câmara com as mesmas coordenadas noplano da imagem, sem distorção, são dadas por:

∂hCud∂hiud

=

[−1fku

0 0

0 −1fkv

0

](7.35)

Por último, a variação das coordenadas do ponto no plano da imagem sem distorção com as mesmascoordenadas com distorção é dada por:

∂hiud∂hid

=

[∂uud

∂ud

∂uud

∂vd∂vud

∂ud

∂vud

∂vd

](7.36)

onde ∂uud

∂ud, ∂uud

∂vd, ∂vud

∂ude ∂vud

∂vdcorrespondem às derivadas das equações apresentadas no Capítulo 2,

definidas por:

∂uud∂ud

=(1 + k1r

2d + k2r

4d

)+ (ud − u0)

(k1 + 2k2r

2d

)(2 (ud − u0)k2u

)(7.37)

∂vud∂vd

=(1 + k1r

2d + k2r

4d

)+ (vd − v0)

(k1 + 2k2r

2d

)(2 (vd − v0)k2v

)(7.38)

∂uud∂vd

= (ud − u0)(k1 + 2k2r

2d

)(2 (vd − v0)k2v

)(7.39)

∂vud∂ud

= (vd − v0)(k1 + 2k2r

2d

)(2 (ud − u0)k2u

)(7.40)

com os parâmetros de calibração da câmara e rd definidos no mesmo capítulo.

75

Então, a variação do vector de estado y′i com as coordenadas do ponto no plano da imagem, comdistorção, é dada a partir da combinação das derivadas 7.33, 7.34, 7.35 e 7.36:

∂y′i∂hid

=∂y′i∂nWud

× ∂nWud∂hCud

× ∂hCud∂hiud

× ∂hiud∂hid

(7.41)

Desta forma, é então possível definir a jacobiana ∂yi

∂hid

, tendo em conta que, pelo facto do parâmetro deyi correspondente ao inverso da profundidade, ρi, não depender das coordenadas do ponto no planoda imagem, com distorção, as suas entradas na matriz vêm a zero:

∂yi∂hid

=

... 0

... 0∂y′i∂hi

d

0... 0... 0

0 0 1

(7.42)

Determinação da jacobiana(∂f∂x

)|x(k|k)

Tal como em casos anteriores, a determinação da jacobiana(∂f∂x

)|x(k|k)

implica a obtenção de outras

derivadas que serão posteriormente combinadas, pois alguns parâmetros não dependemdirectamente de outros. A variação da função de transição de estado com os parâmetros do vectorestimado do estado da câmara xv, rW , vW e wC é directamente obtida, assim como a variação destacom os parâmetros do vector estimado do estado dos pontos yi, que corresponde à matriz identidadeI com dimensão proporcional ao número de pontos do sistema. Contudo para o caso do quaternião, arelação não é tão directa. Tendo em conta que o angulo θ, parâmetro do quaternião, pode ser dado emfunção das componentes da velocidade angular wC como:

θ = ‖wC‖∆t (7.43)

então a variação das componentes do quaternião qWC (k | k), com as componentes da velocidadeangular wC (k | k), ∂q

W C

∂wC é dada por:

∂qWC (k | k)∂wC (k | k)

=

∂qW C0

∂wCx

∂qW C0

∂wCy

∂qW C0

∂wCz

∂qW Cx

∂wCx

∂qW Cx

∂wCy

∂qW Cx

∂wCz

∂qW Cy

∂wCx

∂qW Cy

∂wCy

∂qW Cy

∂wCz

∂qW Cz

∂wCx

∂qW Cz

∂wCy

∂qW Cz

∂wCz

(7.44)

onde:

∂qWC0

∂wCx=(−∆t

2

)(wCx‖wC‖

)sin(‖wC‖∆t

2

)(7.45)

com ∂qW C0

∂wCy

e ∂qW C0

∂wCz

definidos de forma análoga a 7.45;

76

∂qWCx

∂wCx=(

∆t2

)((wCx)2

‖wC‖2

)cos(‖wC‖∆t

2

)+(

1‖wC‖

)(1−

(wCx)2

‖wC‖2

)sin(‖wC‖∆t

2

)(7.46)

com ∂qW Cy

∂wCy

e ∂qW Cz

∂wCz

definidos de forma análoga a 7.46;

∂qWCx

∂wCy=(

∆t2

)(wCx w

Cy

‖wC‖

)cos(‖wC‖∆t

2

)−(

1‖wC‖

)sin(‖wC‖∆t

2

)(7.47)

com ∂qW Cx

∂wCz

, ∂qW Cy

∂wCx

, ∂qW Cy

∂wCz

, ∂qW Cz

∂wCx

e ∂qW Cz

∂wCy

definidos de forma análoga a 7.47.

A variação da predição do quaternião qWC (k + 1 | k) com os quaterniões qWC (k | k), presente novector de estado da câmara e q

′WC (k | k), é dada por:

∂qWC (k + 1 | k)∂q′WC (k | k)

=

q0WC (k) −qxWC (k) −qyWC (k) −qzWC (k)

qxWC (k) q0

WC (k) −qzWC (k) qyWC (k)

qyWC (k) qz

WC (k) q0WC (k) −qxWC (k)

qzWC (k) −qyWC (k) qx

WC (k) q0WC (k)

(7.48)

∂qWC (k + 1 | k)∂qWC (k | k)

=

q′WC0 (k) −q′WC

x (k) −q′WCy (k) −q′WC

z (k)q′WCx (k) q

′WC0 (k) q

′WCz (k) −q′WC

y (k)q′WCy (k) −q′WC

z (k) q′WC0 (k) q

′WCx (k)

q′WCz (k) q

′WCy (k) −q′WC

x (k) q′WC0 (k)

(7.49)

Desta forma é possível definir finalmente a jacobiana(∂f∂x

)|x(k|k)

como:

(∂f

∂x

)|x(k|k)

=

[∂fv

∂xvzeros (13× dim)

zeros (dim× 13) ∂fy

∂y

](7.50)

onde dim corresponde ao número total de parâmetros que caracterizam os vectores de estado dospontos do sistema, ∂fv

∂xvé dado a partir das equações 7.44, 7.48 e 7.49 por:

∂fv∂xv

=

I (3× 3) zeros (3× 4) I (3× 3) ∆t zeros (3× 3)

zeros (4× 3) ∂qW C(k+1|k)∂q′W C(k|k) zeros (4× 3) ∂qW C(k+1|k)

∂qW C(k|k)∂qW C(k|k)∂wC(k|k)

· · · zeros (6× 7) · · · · · · I (6× 6) · · ·

(7.51)

e ∂fy

∂y vem corresponde simplesmente à matriz identidade, pois a predição feita para os pontos dosistema consiste em considerar que estes se mantém na mesma posição:

∂fy∂y

= I (dim× dim) (7.52)

Determinação da jacobiana(∂h∂x

)|x(k|k)pred

A jacobiana(∂h∂x

)|x(k|k)pred

corresponde à variação da função de medição h, linearizada em torno dovalor da predição do estado do sistema, para o instante seguinte.

A variação desta função em relação a cada uma das predições dos parâmetros vem assim dada por:

77

(∂h

∂x

)|x(k|k)pred

=

∂h1∂xv

∂h1∂y1

0 · · · 0...

......

......

∂hi

∂xv0 · · · 0 ∂hi

∂yi

(7.53)

Considere-se então o ponto característico i, e as componentes da jacobiana correspondentes à últimafila da matriz 7.53, para termos de simplificação, tendo em conta que o processo de determinação dasoutras componentes da jacobiana é análogo:(

∂hi∂x

)|x(k|k)pred

=[

∂h1∂xv

0 · · · 0 ∂hi

∂yi

](7.54)

A função de medição do ponto i, hi só depende da predição do vector de estado da câmara e do pontoem análise, razão pela qual todas as outras entradas na matriz vêm a zero. Assim, a determinação de(∂hi

∂x

)|x(k|k)pred

necessita apenas da obtenção de ∂hi

∂xve ∂hi

∂yi. Como hi não varia directamente com os

parâmetros de xv, assim como com os parâmetros de yi, é necessário determinar algumas derivadasintermédias que possam ser combinadas posteriormente para obter as derivadas desejadas.

A variação da função hi com os parâmetros de xv é dada por:

∂hi∂xv

=[

∂hi

∂rW∂hi

∂qW C zeros (2× 6)]

(7.55)

com ∂hi

∂rW e ∂hi

∂qW C definidas a partir da combinação de derivadas intermédias:

∂hi∂rW

=∂hi

∂hiudi

×∂hiudi

∂hCi× ∂hCi∂rW

(7.56)

∂hi∂qWC

=∂hi

∂hiudi

×∂hiudi

∂hCi× ∂hCi∂qWC

(7.57)

O primeiro passo na determinação das derivadas intermédias corresponde a obter a variação de hicom a predição das coordenadas do ponto i no plano da imagem, ∂hi

∂hiudi

, o que corresponde à inversa

da matriz definida em 7.36, cujas derivadas parciais são as definidas em 7.37, 7.38, 7.39 e 7.40, maspara (uudi , vudi) e (udi , vdi):

∂hi

∂hiudi

=

∂uudi

∂udi

∂vudi

∂vdi∂vudi

∂udi

∂vudi

∂vdi

−1

(7.58)

Por sua vez, a variação das coordenadas do ponto i no plano da imagem com as coordenadas do

mesmo no referêncial da câmara,∂hi

udi

∂hCi

, corresponde a:

∂hiudi

∂hCi=

− fku

hCzi

0hC

xifku

(hCzi

)2

0 − fkv

hCzi

hCyifku

(hCzi

)2

(7.59)

A variação das coordenadas do ponto no referêncial da câmara com a predição de rW , ∂hCi

∂rW , dependeda matriz de rotação RWC e é dada por:

78

∂hCi∂rW

= −(RWC

)−1

ρi (7.60)

Por último, a variação das coordenadas do ponto no referêncial da câmara com a predição de qWC ,∂hC

i

∂qW C , é dada por:

∂hCi∂qWC

=[

∂RW C

∂qW C0

hWi∂RW C

∂qW Cx

hWi∂RW C

∂qW Cy

hWi∂RW C

∂qW Cz

hWi

]

1 0 0 00 −1 0 00 0 −1 00 0 0 −1

(7.61)

onde ∂RW C

∂qW C0

, ∂RW C

∂qW Cx

, ∂RW C

∂qW Cy

e ∂RW C

∂qW Cz

correspondem às derivadas definidas em 7.23, 7.24, 7.25 e 7.26

para a matriz de rotação RWC .

Desta forma, a variação da função hi com os parâmetros de xv, apresentada em 7.55, fica totalmentedefinida.

A variação da função hi com os parâmetros de yi, ∂hi

∂yié igualmente obtida a partir da combinação de

derivadas intermédias, e é dada por:

∂hi∂yi

=∂hi

∂hiudi

×∂hiudi

∂hCi× ∂hCi

∂yi(7.62)

Sendo que ∂hi

∂hiudi

e∂hi

udi

∂hCi

se encontram definidas nas equações 7.58 e 7.59, só é necessário

determinar a variação das coordenadas do ponto no referêncial da câmara com os parâmetros de yi,∂hC

i

∂yi, e é dada por:

∂hCi∂yi

=

λi ×(RWC

)−1∂mi

∂θi

∂mi

∂φi

(RWC

)−1

×

xWci

yWci

zWci

− rW (7.63)

onde ∂mi

∂θie ∂mi

∂φisão dadas por:

∂mi

∂θi=(RWC

)−1

×[

cos φi cos θi 0 − cos φi sin θi]

(7.64)

∂mi

∂φi=(RWC

)−1

×[− sin φi sin θi − cos φi − sin φi cos θi

](7.65)

Desta forma, a variação da função hi com os parâmetros de yi, ∂hi

∂yi, definida na equação 7.62, fica

completa, assim como as componentes da jacobiana(∂h∂x

)|x(k|k)pred

correspondentes ao ponto i,definidas em 7.54. O processo de determinação das restantes componentes da jacobiana(∂h∂x

)|x(k|k)pred

é análogo, pelo que esta fica completamente definida.

79

Bibliografia

[1] J.-Y. Bouguet. (2008) Camera calibration toolbox for matlab. 9

[2] M. S. Chris Harris, “A combined corner and edge detector,” The Plessey Company, p. 147U152,1988. 15

[3] J. M. M. M. Javier Civera, Andrew J. Davison, “Inverse depth parametrization for monocular slam,”IEEE TRANSACTIONS ON ROBOTICS, vol. VOL. 24, pp. 932–945, 2008. 27

[4] P. G. F. Tarsha-Kurdi, T. Landes, “Hough-transform and extended ransac algorithms for automaticdetection of 3d building roof planes from lidar data,” IAPRS, vol. XXXVI, p. 6, 2007. 41

[5] N. M. O. S. Andrew Davison, Ian Reid, “Monoslam: Real-time single camera slam,” IEEE, vol. 29,p. 16, 2007.

[6] D. W. M. Andrew J. Davison, “Simultaneous localization and map-building using active vision,” IEEE,vol. Vol. 24, pp. 965–880, 2002.

[7] J. Aulinas, “3d visual slam applied to large-scale underwater scenarios,” Master’s thesis, Institute ofInformatics and Applications, University of Girons, 2008.

[8] D. S. Darya Frolova, “Matching with invariant features,” Master’s thesis, The Weizmann Institute ofScience, 2004.

[9] A. J. Davison, “Real-time simultaneous localisation and mapping with a single camera,” p. 8.

[10] ——, “Slam with a single camera,” Master’s thesis, Robotics Research Group, Department of Engi-neering Science, University of Oxford, 2002.

[11] ——, “Mobile robot navigation using active vision,” Master’s thesis, Robotics Research Group, De-partment of Engineering Science, University of Oxford, 1998.

[12] K. G. Derpanis, “The harris corner detector,” 2004.

[13] E. B. Joaquim Salvi, Yvan Petillot, “Visual slam for 3d large-scale seabed acquisition employingunderwater vehicles,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.22–26, 2008.

[14] M. G. Lucas Teixeira, Waldemar Celes, “Accelerated corner-detector algorithms,” Master’s thesis,Tecgraf - Computer Science Department.

[15] J. D. Luiz Mirisola, “Uma metodologia de odometria visual/inercial e slam 3d com um vant,” Mas-ter’s thesis, Divisão de Robótica e Visão Computacional - DRVC, CTI - Centro de Tecnologia daInformação, Brasil; Instituto de Sistemas e Robótica, Universidade de Coimbra, Portugal.

80

[16] S. S. Mitch Bryson, “Bearing-only slam for an airborne vehicle,” Master’s thesis, ARC Centre forExcellence in Autonomous Systems, Australian Centre for Field Robotics, University of Sydney.

[17] J. Neira, “Pure visual slam,” p. 53, 2008.

[18] M. I. Ribeiro, “Kalman and extended kalman filters: Concept, derivation and properties,” Master’sthesis, Institute for Systems and Robotics, Instituto Superior Técnico, 2004.

[19] S. Rotenberg, “Orientation & quaternions,” 2005.

[20] I. M. Simon Baker, “Lucas-kanade 20 years on: A unifying framework,” International Journal ofComputer Vision.

[21] D. M. Somkiat Wangsiripitak, “Avoiding moving outliers in visual slam by tracking moving objects,”IEEE, 2009.

[22] J. S. D. C. Zhenyu Yu, Kenzo Nonami, “3d vision based landing control of a small scale autonomoushelicopter,” International Journal of Advanced Robotic Systems, vol. Vol. 4, No. 1, p. 51U56, 2007.

81