Calibração e Correção de Distorção em Robôs Humanoides: Um Guia Prático
A seguir, apresentamos um exemplo prático focado em aplicações centrais do sistema visual de robôs humanoides, demonstrando o processo completo de calibração de câmera e correção de distorção. Neste exemplo, geramos programaticamente imagens de tabuleiro de xadrez centralizado com pontos de canto precisos, utilizamos o método de Zhang para calcular a matriz de parâmetros intrínsecos da câmera e os coeficientes de distorção, evitando problemas de detecção de falhas em cenas reais; simulamos os efeitos de distorção da lente da câmera, comparando visualmente as diferenças antes e após a correção do objeto de agarre (copo); além disso, implementamos a projeção de coordenadas 3D do mundo para o plano 2D da imagem, marcando a posição espacial do objeto de interesse para o robô.
Exemplo: Calibração de Câmera e Correção de Distorção para Robôs Humanoides (código-fonte: codigos\3\CalibracaoRobo.py)
O fluxo principal de implementação do arquivo CalibracaoRobo.py é descrito a seguir.
(1) O código a seguir configura o ambiente para exibição de caracteres chineses no Matplotlib. Ele força o uso do backend TkAgg, seleciona o caminho da fonte apropriado para o sistema operacional (Windows usa Microsoft YaHei, macOS usa PingFang, Linux usa WenQuanYi Micro Hei), tenta carregar a fonte e configurá-la como global, garantindo que o sinal de negativo seja exibido corretamente. Se o carregamento da fonte falhar, retorna automaticamente para a fonte sans-serif padrão.
# ===================== 1. Configuração de Fonte para Chinês =====================
matplotlib.use('TkAgg', force=True)
plt.rcParams['axes.unicode_minus'] = False # Garante exibição correta do sinal negativo
sistema = platform.system()
# Especifica caminho da fonte por sistema (fallback)
if sistema == "Windows":
caminho_fonte = r"C:\Windows\Fonts\msyh.ttc" # Microsoft YaHei
elif sistema == "Darwin":
caminho_fonte = "/System/Library/Fonts/PingFang.ttc" # PingFang
else: # Linux
caminho_fonte = "/usr/share/fonts/truetype/wqy/wqy-microhei.ttc" # WenQuanYi
try:
prop = fm.FontProperties(fname=caminho_fonte)
plt.rcParams['font.family'] = prop.get_name()
except Exception as e:
print(f"Falha ao carregar fonte: {e}, usando padrão")
plt.rcParams['font.family'] = ['sans-serif']
(2) O código a seguir define os parâmetros essenciais para a calibração da câmera. Inclui o número de pontos de canto internos do tabuleiro de xadrez, o tamanho físico de cada quadrado, a resolução da câmera, e inicializa manualmente uma matriz de parâmetros intrínsecos DEFAULT_K razoável para evitar explosão de valores e erros anômalos de reprojetação durante a calibração simulada.
# ===================== 2. Parâmetros Essenciais (baseado em câmeras reais) =====================
PONTOS_TABULEIRO = (7, 5) # Número de pontos de canto internos (colunas, linhas)
TAMANHO_QUADRADO_M = 0.02 # Tamanho real de cada quadrado (metros)
RESOLUCAO_IMAGEM = (640, 480) # Resolução da câmera
# Inicialização manual de parâmetros intrínsecos (evita explosão numérica)
DEFAULT_K = np.array([[360, 0, 320],
[0, 360, 240],
[0, 0, 1]], dtype=np.float32)
(3) O código a seguir gera uma imagem de tabuleiro de xadrez centralizado e simula coordenadas de pontos de canto internos. A função gerar_tabuleiro_xadrez desenha um tabuleiro de xadrez com quadrados pretos e brancos alternados em um branco de fundo, calcula manualmente o centro de cada quadrado como ponto de canto e marca com pontos vermelhos, garantindo que processos posteriores de calibração ou projeção não falhem na detecção de pontos.
# ===================== 3. Geração de Imagem de Tabuleiro Simulada + Pontos de Canto =====================
def gerar_tabuleiro_xadrez(tamanho_img=(640,480), pontos=PONTOS_TABULEIRO, grid_px=60):
"""
Gera imagem de tabuleiro centralizada + pontos de canto simulados
:param tamanho_img: Resolução da imagem
:param pontos: Número de pontos de canto internos (colunas, linhas)
:param grid_px: Tamanho em pixels de cada quadrado
:return: Imagem do tabuleiro, coordenadas 2D dos pontos
"""
linhas_grid = pontos[1] + 1 # Linhas do grid = pontos de canto + 1
colunas_grid = pontos[0] + 1 # Colunas do grid = pontos de canto + 1
# Cria fundo branco puro
img = np.ones((tamanho_img[1], tamanho_img[0], 3), np.uint8) * 255
# Calcula coordenadas de início centralizado
inicio_x = (tamanho_img[0] - colunas_grid * grid_px) // 2
inicio_y = (tamanho_img[1] - linhas_grid * grid_px) // 2
# Desenha tabuleiro de xadrez com padrão alternado
for r in range(linhas_grid):
for c in range(colunas_grid):
if (r + c) % 2 == 0:
x1 = inicio_x + c * grid_px
y1 = inicio_y + r * grid_px
x2 = x1 + grid_px
y2 = y1 + grid_px
cv2.rectangle(img, (x1, y1), (x2, y2), (0, 0, 0), -1)
# Geração manual de pontos de canto (centros dos quadrados)
pontos_img = []
for r in range(pontos[1]):
for c in range(pontos[0]):
px = inicio_x + (c + 0.5) * grid_px # Centro X do quadrado
py = inicio_y + (r + 0.5) * grid_px # Centro Y do quadrado
pontos_img.append([px, py])
cv2.circle(img, (int(px), int(py)), 4, (0, 0, 255), -1) # Marca ponto
pontos_img = np.array(pontos_img, dtype=np.float32).reshape(-1, 1, 2)
return img, pontos_img
(4) O código a seguir gera pontos de coordenadas 3D no mundo real e coleta dados de calibração simulados. Usando o método de Zhang, as posições dos pontos de canto internos do tabuleiro no sistema de coordenadas do mundo são inicializadas no plano Z=0. Em seguida, geramos três imagens de tabuleiro em ângulos diferentes, adicionando os pontos 3D correspondentes e os pontos de canto 2D da imagem às listas, preparando para a calibração da câmera, enquanto mantemos uma imagem demonstrativa para visualização.
# ===================== 4. Construção de Pontos 3D no Mundo (método de Zhang, Z=0) =====================
pontos_mundo = [] # Lista de pontos 3D no mundo
pontos_imagem = [] # Lista de pontos 2D da imagem
img_demo_tabuleiro = None
# Geração de pontos 3D de referência
objp = np.zeros((np.prod(PONTOS_TABULEIRO), 3), np.float32)
objp[:, :2] = np.mgrid[0:PONTOS_TABULEIRO[0], 0:PONTOS_TABULEIRO[1]].T.reshape(-1, 2) * TAMANHO_QUADRADO_M
# Geração de 3 tabuleiros simulados (diferentes ângulos)
for i in range(3):
img, pontos_p = gerar_tabuleiro_xadrez(RESOLUCAO_IMAGEM)
pontos_mundo.append(objp)
pontos_imagem.append(pontos_p)
if img_demo_tabuleiro is None:
img_demo_tabuleiro = img.copy()
print(f"Tabuleiro simulado {i+1} gerado ✅")
(5) O código a seguir executa a calibração da câmera nos pontos de canto simulados. Ele chama cv2.calibrateCamera, passando a matriz de parâmetros intrínsecos DEFAULT_K inicializada manualmente e os pontos 2D/3D gerados, obtendo a matriz de parâmetros intrínsecos K, coeficientes de distorção e vetores de rotação e translação. Em seguida, calcula o erro médio de rperojetação para avaliar a precisão da calibração e exibe os resultados.
# ===================== 5. Calibração da Câmera (otimizando parâmetros intrínsecos) =====================
# Inicialização manual de parâmetros intrínsecos para melhorar a calibração
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
pontos_mundo, pontos_imagem, RESOLUCAO_IMAGEM, DEFAULT_K, None
)
# Cálculo do erro de reprojetação (avaliação da precisão)
erro_total = 0
for i in range(len(pontos_mundo)):
pontos_proj, _ = cv2.projectPoints(pontos_mundo[i], rvecs[i], tvecs[i], K, dist)
erro = cv2.norm(pontos_imagem[i], pontos_proj, cv2.NORM_L2) / len(pontos_proj)
erro_total += erro
erro_medio = erro_total / len(pontos_mundo)
# Exibição dos resultados da calibração
print("\n=== 🤖 Resultados da Calibração da Câmera do Robô Humanoide ===")
print(f"Número de imagens calibradas: {len(pontos_mundo)}")
print(f"Matriz de parâmetros intrínsecos K:\n{np.round(K, 2)}")
print(f"Coeficientes de distorção [k1,k2,p1,p2,k3]:\n{np.round(dist[0], 4) if dist is not None else 'N/A'}")
print(f"Erro médio de reprojetação: {erro_medio:.4f} pixels (ideal <0.5 pixels)")
(6) O código a seguir gera uma imagem de teste e simula a distorção da lente com correção. Cria uma imagem de fundo branco representando o objeto de agarre do robô (como um copo), usa PIL para desenhar texto em chinês, evitando problemas de codificação com caracteres chineses no OpenCV; em seguida, usa cv2.undistort processando inversamente para simular distorção, gerando uma imagem com distorção, e então aplica o undistort direto para obter a imagem corrigida, demonstrando o efeito da correção.
# ===================== 6. Simulação de Distorção + Correção =====================
# Geração de imagem de teste (objeto de agarre: copo)
img_teste = np.ones((RESOLUCAO_IMAGEM[1], RESOLUCAO_IMAGEM[0], 3), np.uint8) * 255
# Usando PIL para desenhar texto chinês (evitando problemas de codificação)
try:
img_pil = Image.fromarray(img_teste)
draw = ImageDraw.Draw(img_pil)
fonte = ImageFont.truetype(caminho_fonte, 20)
draw.text((220, 250), "Objeto de Agarre (Copo)", font=fonte, fill=(0, 255, 0))
img_teste = np.array(img_pil)
except Exception as e:
print(f"Falha ao desenhar texto chinês: {e}, usando inglês")
cv2.putText(img_teste, "Grasp Target (Cup)", (220, 250),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
# Simulação de distorção da lente (processo inverso)
def adicionar_distorcao(img, K, dist):
"""Simula distorção da câmera (para demonstração)"""
return cv2.undistort(img, K, -dist) # Coeficientes negativos = adicionar distorção
img_distorcida = adicionar_distorcao(img_teste, K, dist) # Imagem com distorção
img_corrigida = cv2.undistort(img_distorcida, K, dist) # Imagem corrigida
(7) O código a seguir projeta coordenadas 3D do mundo real para o plano 2D da imagem. Dadas as coordenadas 3D do alvo (como a posição do objeto no sistema de coordenadas do robô), usa os vetores de rotação, translação e matriz de parâmetros intrínsecos obtidos na calibração para projetar, obtendo a posição em pixels na imagem. Em seguida, marca o ponto projetado na imagem corrigida e usa PIL para desenhar o rótulo "Ponto 3D Projetado", demonstrando o efeito de projeção 3D→2D.
# ===================== 7. Projeção 3D→2D (modelo da câmera) =====================
# Simulação de coordenadas 3D do objeto (x=0.2m, y=0.1m, z=0.6m)
alvo_3d = np.array([[0.2, 0.1, 0.6]], dtype=np.float32)
# Projeção de coordenadas 3D para plano 2D da imagem
alvo_2d, _ = cv2.projectPoints(alvo_3d, rvecs[0], tvecs[0], K, dist)
alvo_2d = alvo_2d.squeeze().astype(int)
# Marca ponto projetado na imagem corrigida
cv2.circle(img_corrigida, (alvo_2d[0], alvo_2d[1]), 8, (0, 0, 255), -1)
try:
img_pil_corrigida = Image.fromarray(img_corrigida)
draw2 = ImageDraw.Draw(img_pil_corrigida)
fonte2 = ImageFont.truetype(caminho_fonte, 15)
draw2.text((alvo_2d[0]+10, alvo_2d[1]), "Ponto 3D Projetado", fonte2, fill=(0, 0, 255))
img_corrigida = np.array(img_pil_corrigida)
except:
cv2.putText(img_corrigida, "Ponto 3D Projetado", (alvo_2d[0]+10, alvo_2d[1]),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 1)
(8) O código a seguir visualiza os resultados da calibração e projeção. Cria um layout de 2x2 subplots: superior esquerdo mostra o tabuleiro e marcação de pontos, superior direito mostra o objeto de agarre com distorção, inferior esquerdo exibe a imagem corrigida com pontos de projeção 3D, e inferior direito exibe a matriz de parâmetros intrínsecos, coeficientes de distorção e erro médio de reprojetação. Todo o texto usa a configuração de fonte para garantir legibilidade, formando uma interface completa de demonstração de calibração e projeção.
# ===================== 8. Visualização dos Resultados (4 subplots) =====================
fig, eixos = plt.subplots(2, 2, figsize=(14, 10))
fonte_titulo = {'family': plt.rcParams['font.family'], 'size': 12, 'weight': 'bold'}
fonte_texto = {'family': plt.rcParams['font.family'], 'size': 10}
# Título principal
fig.suptitle("🤖 Sistema Visual do Robô Humanoide - Calibração e Correção de Distorção", **fonte_titulo)
# Subplot 1: Tabuleiro + pontos
eixos[0,0].imshow(cv2.cvtColor(img_demo_tabuleiro, cv2.COLOR_BGR2RGB))
eixos[0,0].set_title("1. Tabuleiro de calibração + pontos marcados", **fonte_titulo)
eixos[0,0].axis('off')
# Subplot 2: Objeto com distorção
eixos[0,1].imshow(cv2.cvtColor(img_distorcida, cv2.COLOR_BGR2RGB))
eixos[0,1].set_title("2. Objeto com distorção (simulação de câmera real)", **fonte_titulo)
eixos[0,1].axis('off')
# Subplot 3: Imagem corrigida + projeção 3D
eixos[1,0].imshow(cv2.cvtColor(img_corrigida, cv2.COLOR_BGR2RGB))
eixos[1,0].set_title("3. Imagem corrigida + pontos de projeção 3D", **fonte_titulo)
eixos[1,0].axis('off')
# Subplot 4: Parâmetros da calibração
eixos[1,1].axis('off')
eixos[1,1].set_title("4. Parâmetros e precisão da calibração", **fonte_titulo)
K_str = '\n'.join([' '.join([f"{num:.2f}" for num in row]) for row in K])
dist_str = ', '.join([f"{d:.4f}" for d in dist[0]]) if dist is not None else "N/A"
eixos[1,1].text(0.05, 0.95, "Resultados principais", **fonte_titulo)
eixos[1,1].text(0.05, 0.80, f"Matriz K:\n{K_str}", **fonte_texto)
eixos[1,1].text(0.05, 0.45, f"Coeficientes de distorção:\n[{dist_str}]", **fonte_texto)
eixos[1,1].text(0.05, 0.25, f"Erro médio: {erro_medio:.4f} pixels", **fonte_texto)
plt.tight_layout()
plt.show()
A execção gera uma visualziação com 4 subplots mostrando o tabuleiro de calibração, efeitos de distorção, resultados da correção e parâmetros principais, ilustrando logicamente a aplicação do modelo de câmera na percepção visual de robôs humanoides, com alta robustez e clareza.