【单目测距】已知相机角度如何测距

读书猿

发布于 2024-02-05 15:19:22

1740

发布于 2024-02-05 15:19:22

文章被收录于专栏：无人驾驶感知无人驾驶感知

前言

承接上篇博客【单目测距】单目相机测距
上篇博客告诉你基本思路，现在我们结合实际情况做个测距分析，比如相机存在角度如何测距。
对此方法测距做误差分析，分析影响误差因素。

测距

已知相机高度 h，已知俯仰角 p，易求 OA。深度 OA 求得，则距离易知。

import numpy as np

h = 1.5  # 相机离地面1.5m高
pitch = -0.023797440420123328  # 弧度
pixe_x, pixe_y = 888, 700  # 图像像素点，接地点
CameraMat = np.array([[1008.79199578, 0, 945.9802333],
                      [0, 1009.03003689, 537.81211307],
                      [0, 0, 1]])  # 相机内参

R = np.array([[-0.0330564609, 0.0238237337, 0.999169505],
              [0.999452124, -0.000862625046, 0.0330863791, ],
              [0.00165014972, 0.999715802, -0.0237821659]])  # 旋转矩阵
T = np.array([0.11, -0.01, 0.13])

sigma = np.arctan((pixe_y - CameraMat[1][2]) / CameraMat[1][1])
z = h * np.cos(sigma) / np.sin(sigma + pitch)  # 深度
x_pixe, y_pixe = 2 * CameraMat[0][2] - pixe_x, 2 * CameraMat[1][2] - pixe_y  # 根据自定坐标系选择是否中心对称转换
camera_x = z * (x_pixe / CameraMat[0][0] - CameraMat[0][2] / CameraMat[0][0])
camera_y = z * (y_pixe / CameraMat[1][1] - CameraMat[1][2] / CameraMat[1][1])
camera_z = z
distance_machine_direction = R[0][0] * camera_x + R[0][1] * camera_y + R[0][2] * camera_z + T[0]  # 纵向距离
distance_transverse_direction = R[1][0] * camera_x + R[1][1] * camera_y + R[1][2] * camera_z + T[1]  # 横向距离
print(distance_machine_direction, distance_transverse_direction)