哪个网站推广好,中国最新的国内军事新闻,wordpress的后台,蚌埠市做网站文章目录 一、前言二、测距代码2.1、地面有坡度2.2、python代码2.2.1、旋转矩阵转角度2.2.2、角度转旋转矩阵2.2.3、三维旋转原理 (Rotation 原理)2.2.4、完整代码 2.3、c 代码 一、前言 上篇博客【单目测距】单目相机测距#xff08;二#xff09; 有讲到当相机不是理想状态… 文章目录 一、前言二、测距代码2.1、地面有坡度2.2、python代码2.2.1、旋转矩阵转角度2.2.2、角度转旋转矩阵2.2.3、三维旋转原理 (Rotation 原理)2.2.4、完整代码 2.3、c 代码 一、前言 上篇博客【单目测距】单目相机测距二 有讲到当相机不是理想状态实际情况如相机安装时候有角度偏差需要对相机进行标定。同时也分析影响测距误差的多个因素以及各个因素影响权重。上述都是基于地面与自身平行当地面存在坡度尤其是上下坡度的时候。此时测距误差会非常之大。如果有 1° 坡度那么目标在 10 m 处测距就有 20 cm 误差。如果我们提前已知到地面的坡度 sigma 我们就应该实时去修正相机外参此片博客提供传入地面角度实时修正相机外参的思路与代码。 二、测距代码 先回顾一下往期测距代码【单目测距】单目相机测距二输入相机内参、外参、相机高度是提前标定完成目标像素点由目标检测 bbox 求出。 import numpy as nph 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、旋转矩阵转角度
import numpy as np
from scipy.spatial.transform import Rotationr 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, degreesFalse) # zxy 是 外旋顺序。degrees False 显示弧度True 显示角度
print(euler_r)# [ 1.56967277 -0.0518037 1.50976086]2.2.2、角度转旋转矩阵
from scipy.spatial.transform import Rotationeuler_r [1.56967277, -0.0518037, 1.50976086]
new_r Rotation.from_euler(zxy, [euler_r[0], euler_r[1], euler_r[2]], degreesFalse).as_matrix()2.2.3、三维旋转原理 (Rotation 原理)
import numpy as np
from scipy.spatial.transform import Rotationdef 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 reseuler_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], degreesFalse).as_matrix()
print(mtx, mtx_1) # 结果完全一致2.2.4、完整代码
综上所述可得
import numpy as np
from scipy.spatial.transform import Rotationdiff_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, degreesFalse)
R Rotation.from_euler(zxy, [euler_r[0], euler_r[1], euler_r[2] diff_pitch], degreesFalse).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 中的三维旋转原理那我们利用矩阵乘法就可以轻松获得新外参啦 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;
