Fundamentos da Estimativa de Pose

A estimativa de pose da câmera envolve determinar a posição e orientação de um sistema de coordenadas da câmera em relação a um sistema de coordenadas do mundo, utilizando correspondências entre pontos 3D conhecidos no espaço e suas projeções 2D na imagem. O OpenCV fornece a função cv::solvePnP() para resolver este problema utilizando algoritmos PnP (Perspective-n-Point). Esta análise foca na aplicação prática com OpenCV 4.5.3.

Função cv::solvePnP()

A assinatura da função é:

bool cv::solvePnP(
    InputArray pontosMundo, 
    InputArray pontosImagem, 
    InputArray matrizIntrinsica, 
    InputArray coefsDistorcao, 
    OutputArray vetorRot, 
    OutputArray vetorTrans, 
    bool utilizaAproximacaoInicial = false, 
    int metodo = SOLVEPNP_ITERATIVE
)

Parâmetros principais:

• pontosMundo: Coordenadas 3D dos pontos de referência no sistema de coordenadas mundial. Formato N×3 ou 1×N/N×1 com 3 canais. Aceita std::vector<cv::Point3f> ou cv::Mat.
• pontosImagem: Coordenadas 2D das projeções correspondentes no plano da imagem. Formato N×2 ou 1×N/N×1 com 2 canais. Aceita std::vector<cv::Point2f> ou cv::Mat.
• matrizIntrinsica: Matriz 3×3 de parâmetros intrínsecos da câmera.
• coefsDistorcao: Vetor de coeficientes de distorção da lente.
• vetorRot: Vetor de rotação 3×1 retornado, representando a orientação.
• vetorTrans: Vetor de translação 3×1 retornado, representando a posição.
• utilizaAproximacaoInicial: Quando verdadeiro e usando SOLVEPNP_ITERATIVE, os valores de vetorRot e vetorTrans fornecidos são usados como inicialização.
• metodo: Algoritmo específico para resolver o problema PnP:
  • SOLVEPNP_ITERATIVE: Método iterativo que minimiza o erro de reprojeção (padrão)
  • SOLVEPNP_EPNP: Algoritmo eficiente baseado no artigo EPnP
  • SOLVEPNP_P3P: Solução clássica do problema P3P
  • SOLVEPNP_AP3P: Variante avançada de P3P
  • SOLVEPNP_IPPE: Método para pontos planares
  • SOLVEPNP_IPPE_SQUARE: Versão especializada para marcadores quadrados
  • SOLVEPNP_SQPNP: Algoritmo quadrado e robusto

Nota: As flags SOLVEPNP_DLS e SOLVEPNP_UPNP foram descontinuadas no OpenCV 4.5.3, sendo automaticamente substituídas por SOLVEPNP_EPNP.

Conversão com cv::Rodrigues()

O vetorRot retornado está no formato de vetor de rotação (eixo-ângulo). Para obter uma matriz de rotação 3×3 ou ângulos de Euler, usa-se a transformação de Rodrigues:

void cv::Rodrigues(
    InputArray entrada,
    OutputArray saida,
    OutputArray jacobiano = noArray()
)

Se entrada for um vetor de rotação 3×1, saida será a matriz de rotação 3×3, e vice-versa. O parâmetro jacobiano opcional retorna as derivadas parciais.

Inetrpretação dos Resultados

Vetor de Translação

O vetorTrans representa a translação do sistema de coordenadas do mundo em relação ao sistema da câmera. Cada componente (x, y, z) corresponde às coordenadas do origem do mundo expressas no sistema da câmera.

Considere um sistema de câmera múltipla onde a câmera secundária está posicionada à esquerda da principal. Se o vetor de trenslação da câmera secundária em relação à principal for [-120.5, 2.3, -5.8], isso significa:

• No sistema da câmera principle, a origem da câmera secundária está 120.5mm à esquerda (eixo x negativo)
• 2.3mm acima (eixo y positivo)
• 5.8mm atrás (eixo z negativo)

Esta interpretação é crucial: o vetor aponta da câmera de referência para o sistema de coordenadas alvo, expresso nas coordenadas da câmera de referência.

Matriz de Rotação

A matriz de rotação derivada de vetorRot expressa a orientação do sistema de coordenadas do mundo em relação ao sistema da câmera. Para visualizar, imagine rotacionar os eixos da câmera até que se alinhem paralelamente aos eixos do mundo.

Com um marcador planar perpendicular ao eixo óptico:

• Rotação positiva no eixo Z (anti-horário) aparece como rotação horária do marcador na imagem
• Rotação positiva no eixo X (anti-horário) corresponde ao marcador "caindo" para frente
• Rotação positiva no eixo Y (anti-horário) faz o lado esquerdo do marcador afundar em relação ao direito

Os ângulos de Euler calculados representam exatamente quanto cada eixo da câmera deve girar para se alinhar com o sistema de coordenadas do mundo.

Determinando a Posição da Câmera no Mundo

Dado que vetorRot e vetorTrans descrevem a transformação da câmera (C) para o mundo (W), podemos calcular a posição da câmera no sistema de coordenadas mundial.

Abordagem por Inversão da Transformação

A relação entre sistemas de coordenadas segue:

Alternativamente, usando a matriz de rotação completa R (3×3) e vetor de translação t:

// Transformação direta
cv::Mat pontoMundo = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat pontoCamera = R * pontoMundo + t; // Resulta em t

// Transformação inversa para origem da câmera
cv::Mat pontoCameraZero = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat pontoMundoCalculado = -R.t() * t; // Posição da câmera no mundo
</double></double>

Ambos os métodos convergem para o mesmo resultado: as coordenadas do centro óptico da câmera expressas no sistema de coordenadas mundial.

Notação e Convenções

Para transformações entre sistemas A e B:

• R<sub>A</sub><sup>B</sup>: Rotação de A para B (B em relação a A)
• T<sub>A</sub><sup>B</sup>: Translação de A para B, representando a origem de A em coordenadas de B
• Aplica-se sempre: P<sub>B</sub> = R<sub>A</sub><sup>B</sup> · P<sub>A</sub> + T<sub>A</sub><sup>B</sup>

Esta convenção mantém consistência em cadeias de transformações e evita ambiguidades em aplicações de visão computacional multi-câmera.