Como fiz um sistema de rastreamento óptico

Foi em 2015. Óculos de realidade virtual Oculus DK2 acabaram de aparecer à venda, o mercado de jogos de realidade virtual estava ganhando popularidade rapidamente.



As oportunidades do jogador em tais jogos eram limitadas. Foram monitorados apenas 6 graus de liberdade dos movimentos da cabeça - rotação (com inercial nos óculos) e movimento em pequeno volume no campo de visão de uma câmera infravermelha fixada no monitor. O processo do jogo consistia em sentar em uma cadeira com um gamepad nas mãos, girar a cabeça em diferentes direções e lutar contra a náusea.



Não parecia muito legal, mas eu vi isso como uma oportunidade de fazer algo interessante, usando minha experiência em desenvolvimento de eletrônicos e sede de novos projetos. Como esse sistema pode ser melhorado?



Claro, livre-se do gamepad, dos fios, permita que o jogador se mova livremente no espaço, veja suas mãos e pés, interaja com o ambiente, outros jogadores e objetos interativos reais.



Eu vi assim:

1. Pegamos vários jogadores, colocamos óculos de realidade virtual, um laptop e sensores em seus braços, pernas e torso.
2. Pegamos uma sala composta por várias salas, corredores, portas, equipamos com sistema de rastreamento, penduramos sensores e travas magnéticas nas portas, adicionamos vários objetos interativos e criamos um jogo em que a geometria de um local virtual repete exatamente a geometria de uma sala real.
3. Criamos um jogo. O jogo é uma missão multiplayer em que vários jogadores colocam equipamentos e se encontram em um mundo virtual. Nele, eles se veem, se veem, podem circular pelo local, abrir portas e resolver conjuntamente os problemas do jogo.

Contei essa ideia ao meu amigo, que inesperadamente a aceitou com grande entusiasmo e se ofereceu para assumir as questões organizacionais. Portanto, decidimos confundir a inicialização.



Para implementar a funcionalidade declarada, foi necessário criar duas tecnologias principais:

1. um traje que consiste em sensores nos braços, pernas e torso que rastreia a posição das partes do corpo do jogador
2. um sistema de rastreamento que rastreia jogadores e objetos interativos no espaço 3D.

O desenvolvimento da segunda tecnologia será discutido neste artigo. Talvez mais tarde eu escreva sobre o primeiro.

Sistema de rastreamento.

Claro, não tínhamos um orçamento para tudo isso, então tínhamos que fazer de tudo, desde materiais de sucata. Para a tarefa de rastrear jogadores no espaço, decidi usar câmeras ópticas e marcadores LED acoplados a óculos de realidade virtual. Não tinha experiência nesse tipo de desenvolvimento, mas já ouvi algo sobre OpenCV, Python, e pensei que poderia fazer.



Conforme planejado, se o sistema sabe onde a câmera está localizada e como está orientada, então pela posição da imagem do marcador no quadro, você pode determinar a linha reta no espaço 3D em que esse marcador está localizado. A interseção de duas dessas linhas fornece a posição final do marcador.







Assim, as câmeras tiveram que ser fixadas no teto para que cada ponto no espaço fosse visualizado por pelo menos duas câmeras (melhor mais para evitar obstruir a visão pelos corpos dos jogadores). Para cobrir o suposto local com uma área de cerca de 100 metros quadrados com rastreamento, foram necessárias cerca de 60 câmeras. Escolhi as primeiras webcams usb baratas disponíveis naquela época.







Essas webcams precisam estar conectadas a algo. Experimentos mostraram que ao usar cabos de extensão USB (pelo menos baratos), as câmeras começaram a apresentar falhas. Portanto, decidi dividir as webcams em grupos de 8 peças e colocá-las nas unidades do sistema montadas no teto. Havia apenas 10 portas USB no meu computador doméstico, então é hora de começar a desenvolver uma bancada de teste.



A arquitetura que criei é a seguinte:

Uma bola de acrílico fosco de uma guirlanda com um LED RGB colado dentro é pendurada em cada copo. Vários jogadores deveriam estar no jogo ao mesmo tempo, então, para identificação, decidi separá-los por cores - R, G, B, RG, RB, GB, RB. É assim que parecia:







A primeira tarefa que precisa ser feita é escrever um programa para encontrar a bola no quadro.



Encontrando a bola na moldura



Tive que procurar as coordenadas do centro da bola e sua cor para identificação em cada quadro que saía da câmera. Parece fácil. Eu faço o download do OpenCV para Python, ligo a câmera no USB e escrevo um script. Para minimizar o efeito de objetos desnecessários no quadro, defino a exposição e a velocidade do obturador da câmera no mínimo, e o brilho do LED é alto para obter pontos brilhantes em um fundo escuro. Na primeira versão, o algoritmo era o seguinte:

2. ( , , – ).
3. .
4. ( )

Parece funcionar, mas existem nuances.



Em primeiro lugar, em uma câmera barata, a matriz é bastante barulhenta, o que leva a constantes flutuações dos contornos dos aglomerados binarizados e, consequentemente, a movimentos bruscos do centro. É impossível para os jogadores distorcerem a imagem em óculos de realidade virtual, então esse problema teve que ser resolvido. As tentativas de usar outros tipos de binarização adaptativa com parâmetros diferentes não surtiram muito efeito.



Em segundo lugar, a resolução da câmera é de apenas 640 * 480, então a alguma distância (não muito grande) a bola é visível como alguns pixels no quadro e o algoritmo de pesquisa de borda para de funcionar normalmente.



Tive que criar um novo algoritmo. A seguinte ideia veio à minha mente:

1. Converta a imagem em tons de cinza
2. Gaussian blur – ,
3. ,

Isso funciona muito melhor, as coordenadas do centro ficam paradas quando a bola está parada, e funciona mesmo a uma grande distância da câmera.



Para ter certeza de que tudo isso funcionará com 8 câmeras em um computador, você precisa realizar um teste de estresse.



Teste de carga Eu



conecto 8 câmeras ao meu desktop, organizo-as de forma que cada uma veja pontos brilhantes e executo um script onde o algoritmo descrito funciona em 8 processos independentes (graças à biblioteca de multiprocessamento Python) e processa todos os streams de uma vez.



E ... imediatamente me deparei com uma falha. As imagens da câmera aparecem e desaparecem, a taxa de quadros salta de 0 para 100, um pesadelo. A investigação mostrou que algumas das portas USB do meu computador estão conectadas ao mesmo barramento por meio de um hub interno, motivo pelo qual a velocidade do barramento é dividida entre várias portas e não é mais suficiente para a taxa de bits da câmera. Conectar as câmeras em diferentes portas do computador em diferentes combinações mostrou que tenho apenas 4 barramentos USB independentes. Tive que encontrar uma placa-mãe com 8 ônibus, o que foi uma missão bastante difícil.





Eu continuo o teste de carga. Desta vez, todas as câmeras estão conectadas e transmitem streams normais, mas imediatamente me deparo com o próximo problema - fps baixo. O processador está 100% carregado e só consegue processar 8 a 10 quadros por segundo de cada uma das oito webcams.







Parece que o código precisa ser otimizado. O gargalo acabou sendo um borrão gaussiano (não é surpreendente, porque você precisa convolver com uma matriz 9 * 9 para cada pixel do quadro). A redução do kernel não salvou a situação. Tive de procurar outro método para encontrar os centros das manchas nas molduras.



A solução foi encontrada repentinamente na função SimpleBlobDetector embutida no OpenCV. Ela faz exatamente o que preciso e muito rapidamente. A vantagem é alcançada devido à binarização sequencial da imagem com diferentes limiares e à busca de contornos. O resultado é um máximo de 30 fps com uma carga de CPU de menos de 40%. Teste de carga aprovado!



Classificação de cor



A próxima tarefa é classificar o marcador por sua cor. O valor médio da cor sobre os pixels pontuais fornece componentes RGB que são muito instáveis ​​e variam muito dependendo da distância até a câmera e do brilho do LED. Mas há uma grande solução: tradução do espaço RGB com HSV (matiz, saturação, valor). Nesta representação, o pixel em vez de "vermelho", "azul", "verde" é decomposto nos componentes "matiz", "saturação", "brilho". Nesse caso, a saturação e o brilho podem ser simplesmente excluídos e classificados apenas por matiz.









E assim, no momento, aprendi como encontrar e identificar marcadores em quadros de um grande número de câmeras. Agora você pode ir para a próxima fase - rastrear no espaço.

Rastreamento

Usei um modelo de câmera pinhole em que todos os raios incidem na matriz por meio de um ponto localizado na distância focal da matriz.







Este modelo irá transformar as coordenadas bidimensionais de um ponto no quadro em equações tridimensionais de uma linha reta no espaço.



Para rastrear as coordenadas 3D do marcador, você precisa obter pelo menos duas linhas de intersecção no espaço de câmeras diferentes e encontrar o ponto de sua intersecção. Não é difícil ver o marcador com duas câmeras, mas para construir essas linhas, você precisa que o sistema saiba tudo sobre as câmeras conectadas: onde elas ficam, em quais ângulos, a distância focal de cada lente. O problema é que nada disso é conhecido. O cálculo dos parâmetros requer algum tipo de procedimento de calibração.



Calibração de rastreamento



Na primeira versão, decidi tornar a calibração de rastreamento o mais primitiva possível.

1. Eu penduro o primeiro bloco de oito câmeras no teto, conecto-as a uma unidade de sistema que fica pendurada no mesmo lugar, direciono as câmeras para que cubram o volume máximo do jogo.
2. Usando um nível de laser e telêmetro, eu meço as coordenadas XYZ de todas as câmeras em um único sistema de coordenadas
3. Para calcular as orientações e distâncias focais das câmeras, eu medi as coordenadas de adesivos especiais. Eu penduro os adesivos da seguinte maneira:
   
   
   • Na interface de exibição de uma imagem da câmera, desenho dois pontos. Um no centro do quadro, outro 200 pixels à direita do centro
     
     
   • Se você olhar para a moldura, esses pontos caem em algum lugar na parede, no chão ou em qualquer outro objeto dentro da sala. Penduro adesivos de papel nos locais apropriados e desenho pontos neles com um marcador
   • Eu meço as coordenadas XYZ desses pontos usando o mesmo nível e telêmetro. No total, para um bloco de oito câmeras, você precisa medir as coordenadas das próprias câmeras e mais dois pontos para cada uma. Essa. 24 trigêmeos de coordenadas. E deve haver cerca de dez desses blocos. Acontece um trabalho longo e enfadonho. Mas nada, vou automatizar a calibração mais tarde.
   • Eu começo o processo de cálculo com base nos dados medidos
   
   
   

Existem dois sistemas de coordenadas: um global, associado à sala, e outro local para cada câmera. Em meu algoritmo, o resultado para cada câmera deve ser uma matriz 4 * 4, contendo sua localização e orientação, permitindo que você converta as coordenadas de local em global.







A ideia é a seguinte:

1. Pegamos a matriz original com rotações e deslocamento zero.
2. , .
3. , .
4. , . , . . 200 . , .
5. (, 200 ).

Certamente esse problema poderia ser resolvido analiticamente, mas para simplificar, usei uma solução numérica na descida do gradiente. Não é assustador, porque os cálculos serão realizados uma vez após a instalação das câmeras.



Para visualizar os resultados da calibração, fiz uma interface 2D com um mapa, no qual o script desenha os rótulos das câmeras e as direções em que eles veem os marcadores. Os triângulos representam as orientações da câmera e os ângulos de visão.







Testando o rastreamento



Você pode começar a executar a visualização, que mostrará se as orientações da câmera foram identificadas corretamente e se os quadros foram interpretados corretamente. Idealmente, as linhas provenientes dos ícones da câmera devem se cruzar em um ponto.



Eis o que aconteceu:





Parece verdade, mas a precisão pode ser claramente maior. O primeiro motivo de imperfeição que veio à mente é a distorção nas lentes das câmeras. Isso significa que essas distorções precisam ser compensadas de alguma forma.



Calibração da câmera

A câmera ideal tem apenas um parâmetro importante para mim - a distância focal. A curva real da câmera também precisa levar em consideração a distorção da lente e o deslocamento do centro da matriz.

Para medir esses parâmetros, existe um procedimento de calibração padrão, durante o qual um conjunto de fotografias de um tabuleiro de damas é tirado com uma câmera medida, na qual os ângulos entre os quadrados são reconhecidos com precisão de subpixel.







O resultado da calibração é uma matriz contendo as distâncias focais ao longo de dois eixos e o deslocamento da matriz em relação ao centro óptico. Tudo isso é medido em pixels.







Bem como um vetor de coeficientes de distorção







que permite compensar as distorções da lente usando transformações de coordenadas de pixel.



Ao aplicar transformações com esses coeficientes às coordenadas do marcador no quadro, você pode trazer o sistema para um modelo de câmera pinhole ideal.



Executando um novo teste de rastreamento:







muito melhor! Parece tão bom que até parece funcionar.



Mas o processo de calibração acaba sendo muito enfadonho: meça diretamente as coordenadas de cada câmera, comece a exibir a imagem de cada câmera, pendure adesivos, meça as coordenadas de cada adesivo, escreva os resultados em uma tabela, calibre as lentes. Tudo isso levou alguns dias e um quilo de nervos. Decidi lidar com rastreamento e escrever algo mais automatizado.



Calculando as coordenadas do marcador



E então, eu tenho um monte de linhas retas, espalhadas no espaço, nas interseções das quais deveria haver marcadores. Apenas as linhas retas no espaço não se cruzam de fato, mas se cruzam, ou seja, passam a alguma distância um do outro. Minha tarefa é encontrar o ponto o mais próximo possível das duas linhas retas. Falando formalmente, você precisa encontrar o ponto médio do segmento que é perpendicular a ambas as linhas.







O comprimento do segmento AB também é útil, porque reflete a "qualidade" do resultado obtido. Quanto mais curto for, quanto mais próximas as linhas retas estiverem umas das outras, melhor será o resultado.



Em seguida, escrevi um algoritmo de rastreamento que calcula as interseções de linhas em pares (dentro da mesma cor, de câmeras que estão a uma distância suficiente uma da outra), procura a melhor e a usa como coordenadas de marcador. Nos próximos frames, ele tenta usar o mesmo par de câmeras para evitar um salto nas coordenadas ao trocar para rastreamento com outras câmeras.



Paralelamente, ao desenvolver um traje com sensores, descobri um estranho fenômeno. Todos os sensores apresentaram valores diferentes de ângulo de guinada (direção no plano horizontal), como se cada um tivesse seu próprio norte. Em primeiro lugar, foi útil verificar se me enganei nos algoritmos de filtragem de dados ou no layout da placa, mas não encontrei nada. Então decidi olhar os dados brutos do magnetômetro e vi o problema.

O campo magnético em nossa sala foi direcionado VERTICALMENTE PARA BAIXO! Aparentemente, isso se deve ao ferro na estrutura do edifício.



Mas os óculos VR também usam um magnetômetro. Por que eles não têm esse efeito? Eu vou verificar. Acontece que ele também tem óculos ... Se você ficar parado, poderá ver como o mundo virtual gira lenta mas seguramente ao seu redor em uma direção aleatória. Em 10 minutos, ele sai quase 180 graus. Em nosso jogo, isso levará inevitavelmente à falta de sincronia entre as realidades virtual e real e vidros quebrados contra as paredes.



Parece que além das coordenadas dos óculos, você terá que determinar sua direção no plano horizontal. A solução se apresenta - colocar não um, mas dois marcadores idênticos nos óculos. Ele permitirá que você determine a direção com uma precisão de 180 graus, mas levando em consideração a presença de sensores inerciais embutidos, isso é o suficiente.







O sistema como um todo funcionou, embora com pequenas ombreiras. Mas foi tomada a decisão de lançar a missão, que estava prestes a ser concluída por nosso desenvolvedor do gamedev que se juntou à nossa mini-equipe. Toda a área de jogo foi destruída, portas com sensores e travas magnéticas foram instaladas e dois objetos interativos foram confeccionados:







Os jogadores colocaram óculos, roupas e mochilas de computador e entraram na área de jogo. As coordenadas de rastreamento foram enviadas a eles via wi-fi e usadas para posicionar o personagem virtual. Tudo funcionou bem, os visitantes estão felizes. O mais agradável foi assistir ao horror e aos gritos de visitantes especialmente impressionáveis ​​nos momentos em que fantasmas virtuais os atacaram da escuridão =)

Dimensionamento

De repente, recebemos um pedido de um grande atirador de realidade virtual para 8 jogadores com armas nas mãos. E são 16 objetos que precisam ser agitados. Por sorte o cenário assumiu a possibilidade de dividir o rastreamento em duas zonas de 4 jogadores cada, então decidi que não haveria problemas, você poderia anotar o pedido e não se preocupar com nada. Era impossível testar o sistema em casa. exigia uma grande área e muitos equipamentos que seriam adquiridos pelo cliente, por isso, antes da instalação, decidi gastar tempo automatizando a calibração de rastreamento.



Autocalibração



Era incrivelmente inconveniente dirigir as câmeras, pendurar todos aqueles adesivos, medir as coordenadas manualmente. Eu queria me livrar de todos esses processos - desligar as câmeras do bulldozer, andar aleatoriamente com o marcador no espaço e executar o algoritmo de calibração. Em teoria, isso deveria ser possível, mas como abordar a escrita de um algoritmo não está claro.



O primeiro passo foi centralizar todo o sistema. Em vez de dividir a área de jogo em blocos de 8 câmeras, fiz um único servidor, que recebia as coordenadas dos pontos nos frames de todas as câmeras de uma só vez.



A ideia é a seguinte:

1. Eu penduro câmeras e as direciono para a área de jogo a olho nu
2. Eu inicio o modo de gravação no servidor, no qual todos os pontos 2D vindos das câmeras são salvos em um arquivo
3. Eu ando em um local escuro de jogo com um marcador em minhas mãos
4. Eu paro a gravação e começo o cálculo dos dados de calibração, que calcula as localizações, orientações e distâncias focais de todas as câmeras.
5. como resultado do parágrafo anterior, um único espaço preenchido com câmeras é obtido. Porque este espaço não está vinculado a coordenadas reais, ele tem um deslocamento e rotação aleatórios, que eu subtraio manualmente.

Tive que vasculhar uma grande quantidade de material sobre álgebra linear e escrever muitas centenas de linhas de código Python. Tanto que mal me lembro como funciona.











Esta é a aparência de um bastão de calibração especial impresso em uma impressora.

Testando um grande projeto

Os problemas começaram durante os testes nas instalações, algumas semanas antes do lançamento do projeto. A identificação de 8 cores de marcadores diferentes funcionou muito bem, os jogadores de teste teletransportaram-se constantemente uns para os outros, algumas cores não diferiam em nada dos destaques externos no shopping center. As vãs tentativas de consertar algo a cada noite sem dormir me deixavam cada vez mais desesperada. Tudo isso foi agravado pela falta de desempenho do servidor ao calcular dezenas de milhares de linhas retas por segundo.



Quando o nível de cortisol no sangue excedeu o máximo teórico, decidi olhar para o problema de um ângulo diferente. Como você pode reduzir o número de pontos coloridos sem reduzir o número de marcadores? Torne o rastreamento ativo. Que cada jogador, por exemplo, sempre tenha um chifre vermelho à esquerda. E o segundo às vezes acende verde na chegada de um comando do servidor para que em um momento seja aceso apenas por um jogador. Acontece que a luz verde parece pular de um jogador para outro, atualizando a ligação de rastreamento para a luz vermelha e redefinindo o erro de orientação do magnetômetro.



Para fazer isso, tive que correr até o chipidip mais próximo, comprar LEDs, fios, transistores, um ferro de solda, fita isolante e pendurar a funcionalidade de controle do LED na placa do traje, que não foi projetada para isso, no ranho. É bom que, ao conectar a placa, apenas para garantir, eu pendurei algumas pernas de stm-ki livres nas almofadas de contato.







Os algoritmos de rastreamento tinham que ser consideravelmente complicados, mas no final funcionaram! O teletransporte de jogadores uns para os outros desapareceu, a carga no processador caiu, os sinalizadores pararam de interferir.





O projeto foi lançado com sucesso, primeiro fiz novas pranchas de ternos com suporte para rastreamento ativo e fizemos uma atualização de hardware.

Como isso acabou?

Nos últimos 3 anos, abrimos muitos canais de entretenimento ao redor do mundo, mas o coronavírus fez seus próprios ajustes, o que nos deu a oportunidade de mudar a direção do trabalho para uma direção mais socialmente útil. Agora temos muito sucesso no desenvolvimento de simuladores médicos em RV. Nossa equipe ainda é pequena e nos esforçamos ativamente para expandir nosso quadro de funcionários. Se houver desenvolvedores experientes em UE4 procurando trabalho entre os leitores da Habr, escreva para mim.

Momento engraçado tradicional no final do artigo:

Periodicamente, durante os testes com um grande número de jogadores, surgia uma falha em que o jogador era repentinamente teletransportado por um curto período de tempo para uma altura de vários metros, o que causava uma reação correspondente. Acontece que meu modelo de câmera assumiu a interseção da matriz com uma linha infinita saindo do marcador. Mas ela não levou em consideração que a câmera tem frente e verso, então o sistema buscou o cruzamento de linhas infinitas, mesmo que o ponto esteja atrás da câmera. Portanto, houve situações em que duas câmeras diferentes viram dois marcadores diferentes, mas o sistema pensou que era um marcador a uma altura de vários metros.







O sistema funcionou literalmente pela bunda.