【单目测距】单目相机测距(三)

文章目录

一、前言

  • 上篇博客【单目测距】单目相机测距(二) 有讲到当相机不是理想状态,实际情况如相机安装时候有角度偏差,需要对相机进行标定。同时也分析影响测距误差的多个因素以及各个因素影响权重。
  • 上述都是基于地面与自身平行,当地面存在坡度尤其是上下坡度的时候。此时测距误差会非常之大。如果有 1° 坡度,那么目标在 10 m 处测距就有 20 cm 误差。
  • 如果我们提前已知到地面的坡度 sigma ,我们就应该实时去修正相机外参,此片博客提供传入地面角度,实时修正相机外参的思路与代码。

二、测距代码

python 复制代码
import numpy as np

h = 1.5  # 相机离地面1.5m高
pitch = -0.023797440420123328  # 弧度
pixe_x, pixe_y = 888, 700  # 图像像素点,接地点
CameraMat = np.array([[1008, 0, 945],
                      [0, 1009, 537],
                      [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, 0, -1.5])

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)

2.1、地面有坡度

  • 根据前面分析,如果地面有坡度,我们应该实时去修正相机外参。具体怎么做,也很简单。就是实时去更新我们的 pitch 角与相机的外参。
  • 我们前提是需要知道地面坡度是多少,关于如何获取地面坡度,以后有机会再谈。

2.2、python代码

python 从旋转矩阵转化到角度、从角度到转化矩阵,主要用到 scipy 库中的 Rotation。

2.2.1、旋转矩阵转角度

python 复制代码
import numpy as np
from scipy.spatial.transform import Rotation

r = np.array([-0.0517, -0.0611, 0.9968, 0.9987, 0.0011, 0.0519, -0.0042, 0.9981, 0.0609]).reshape(3, 3)
euler_r = Rotation.from_matrix(r).as_euler('zxy', degrees=False)  # zxy 是 外旋顺序。degrees False 显示弧度,True 显示角度
print(euler_r)

# [ 1.56967277 -0.0518037   1.50976086]

2.2.2、角度转旋转矩阵

python 复制代码
from scipy.spatial.transform import Rotation

euler_r = [1.56967277, -0.0518037, 1.50976086]
new_r = Rotation.from_euler("zxy", [euler_r[0], euler_r[1], euler_r[2]], degrees=False).as_matrix()

2.2.3、三维旋转原理 (Rotation 原理)

python 复制代码
import numpy as np
from scipy.spatial.transform import Rotation


def get_r_matrix(str, alpha):
    sin = -np.sin(alpha)
    cos = np.cos(alpha)
    res = np.eye(3)
    if str == "z":
        res = np.array([[cos, sin, 0],
                        [-sin, cos, 0],
                        [0, 0, 1]])
    elif str == "y":
        res = np.array([[cos, 0, -sin],
                        [0, 1, 0],
                        [sin, 0, cos]])
    elif str == "x":
        res = np.array([[1, 0, 0],
                        [0, cos, sin],
                        [0, -sin, cos]])
    return res


euler_r = [1.56967277, -0.0518037, 1.50976086]
a, b, c = euler_r[0], euler_r[1], euler_r[2]

z = get_r_matrix("z", a)
x = get_r_matrix("x", b)
y = get_r_matrix("y", c)
mtx = y @ x @ z
mtx_1 = Rotation.from_euler("zxy", [a, b, c], degrees=False).as_matrix()
print(mtx, mtx_1)  # 结果完全一致

2.2.4、完整代码

综上所述,可得

python 复制代码
import numpy as np
from scipy.spatial.transform import Rotation

diff_pitch = -0.01  # 假设当前地面坡度为 -0.01 弧度
h = 1.5  # 相机离地面1.5m高
pitch = -0.023797440420123328  # 弧度
pitch = pitch + diff_pitch
pixe_x, pixe_y = 888, 700  # 图像像素点,接地点
CameraMat = np.array([[1008, 0, 945],
                      [0, 1009, 537],
                      [0, 0, 1]])  # 相机内参

original_r = np.array([[-0.0330564609, 0.0238237337, 0.999169505],
                       [0.999452124, -0.000862625046, 0.0330863791],
                       [0.00165014972, 0.999715802, -0.0237821659]])  # 旋转矩阵
euler_r = Rotation.from_matrix(original_r).as_euler('zxy', degrees=False)
R = Rotation.from_euler("zxy", [euler_r[0], euler_r[1], euler_r[2] + diff_pitch], degrees=False).as_matrix()

T = np.array([0, 0, -1.5])  # 平移矩阵

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)

2.3、c++ 代码

知道了 2.2.3 中的三维旋转原理,那我们利用矩阵乘法就可以轻松获得新外参啦

c++ 复制代码
  double pitchDiff = -0.01;
  cv::Mat initR = (cv::Mat_<double>(3,3) << -0.0330564609, 0.0238237337, 0.999169505,
                                             0.999452124, -0.000862625046, 0.0330863791, 
                                             0.00165014972, 0.999715802, -0.0237821659); // 相机初始外参
  
  cv::Mat pitchR = (cv::Mat_<double>(3, 3) << cos(pitchDiff), 0, sin(pitchDiff), 0, 1, 0, -sin(pitchDiff), 0, cos(pitchDiff));

  cv::Mat curR = pitchR * initR;
相关推荐
地平线开发者6 小时前
征程 6 | 征程 6 工具链如何支持 Matmul/Conv 双 int16 输入量化?
算法·自动驾驶
柔情的菜刀6 小时前
基于偏振相机---太阳子午线计算技术
数码相机
OAFD.8 小时前
YOLOv3 详解:核心改进、网络架构与目标检测实践
网络·yolo·目标检测
AI浩9 小时前
基于信息保留与细粒度特征聚合的无人机目标检测
人工智能·目标检测·无人机
地平线开发者10 小时前
新版 perf 文件解读与性能分析
算法·自动驾驶
ReinaXue11 小时前
大模型【进阶】(六)QWen2.5-VL视觉语言模型详细解读
图像处理·人工智能·神经网络·目标检测·计算机视觉·语言模型·transformer
格林威12 小时前
近红外相机在半导体制造领域的应用
大数据·人工智能·深度学习·数码相机·视觉检测·制造·工业相机
CoookeCola14 小时前
Google Landmarks Dataset v2 (GLDv2):面向实例级识别与检索的500万图像,200k+类别大规模地标识别基准
图像处理·人工智能·学习·目标检测·计算机视觉·视觉检测
尤超宇16 小时前
YOLOv3 目标检测算法核心技术
算法·yolo·目标检测
qq_5260991316 小时前
工业级图像采集卡:工业智能化进程中的核心视觉中枢
数码相机