So berechnen Sie den Tonhöhewinkel eines Kamerastrahlvektors in einem bestimmten Koordinatenrahmen ⇐ Python

[Anonymous]

Ich möchte den Tonhöhenwinkel eines Kamerahrahtvektors berechnen, der einem Bildpunkt (u, v) entspricht. Dieser Vektor wird in den Anlagenkoordinatenrahmen bewegt und als reflected_ray Vektor bezeichnet. Die XY -Ebene der Weltkoordinatenrahmen . Dieser Endpunkt ist der 3D-Punkt des Bildpunkts (u, v) .import numpy as np
from scipy.spatial.transform import Rotation as R
import os, sys, cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection

np.set_printoptions(precision=4, suppress=True)
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
sys.path.append(os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))))

# define intrinsics
K = np.array([[2870.7255, 0. , 2131.0571],
[ 0. , 2876.4684, 1392.7799],
[ 0. , 0. , 1. ]])
dist = np.array([-0.0839, 0.0856, -0.001 , 0.0014, -0.0349])

# define the extrinsics
tvec = np.array([[ -2.0641],
[-17.485 ],
[109.642 ]])
R_w2c = np.array([[ 0.9988, -0.0358, -0.0337],
[ 0.0383, 0.9964, 0.0754],
[ 0.0309, -0.0766, 0.9966]])

# Start

## image point
u, v = 1926, 1958.9192

# Image Point in homogeneous coordinates
h0v0 = np.array([u,v,1], dtype=np.float32).reshape(3,1)

undist_pt1 = cv2.undistortPoints(h0v0[:2], K, dist, P=None)

h0v0_und = K @ np.array([undist_pt1.squeeze()[0],undist_pt1.squeeze()[1],1], dtype=np.float32).reshape(3,1)

# Not so important as distortion not very effective
k_c2c_und = np.linalg.inv(K) @ h0v0_und

# direction of the camera ray in the camera’s coordinate system, which is, normalized image plane where z = 1
k_c2c = np.linalg.inv(K) @ h0v0

def cameraRay(k_c2c, R_w2c, tvec, lamda):

"""
Calculate the camera ray in world coordinates.
"""
P_c2w = R_w2c.T @ (lamda*k_c2c.reshape(3,1) - tvec)
return P_c2w

n = np.array([0,0,1]).reshape(3,1)

C_o2w = - R_w2c.T @ tvec

# plant center in the world coordinate frame
V_o2w = C_o2w + np.array([0, 0, -250]).reshape(tvec.shape)

k_c2w = R_w2c.T @ k_c2c

lamda_star = - n.T @ C_o2w / (n.T @ k_c2w)

# ray of the corresponding image point, at lamda_star depth, in world coordinates
P_c2w_star = cameraRay(k_c2c, R_w2c, tvec, lamda = lamda_star)

R_w2v = R.from_matrix([[0, 0, -1], [1, 0, 0], [0, -1, 0]]).as_matrix()

# projected vector in plant coordinates
P_c2v = R_w2v @ (P_c2w_star - V_o2w)
theta_vision = np.arctan2(P_c2v[0],P_c2v[2])*180/np.pi

print(f" calculated camera ray pitch in plant coordinates: {theta_vision[0]:.4f} degrees")

# Origin of the plant beam coordinate frame
beam_origin = np.array([-50, 25, -150]).reshape(3, 1)

def visualize_camera_ray_system(C_o2w, P_c2w_star, V_o2w, R_w2c, R_w2v, P_c2v,beam_origin):
"""
Visualize the camera system, ground plane, rays, and coordinate frames in 3D.

Parameters:
-----------
C_o2w : np.ndarray
Camera center in world coordinates (3x1)
P_c2w_star : np.ndarray
Intersection point of camera ray with ground plane (3x1)
V_o2w : np.ndarray
plant center in world coordinates (3x1)
R_w2c : np.ndarray
Rotation matrix from world to camera coordinates (3x3)
R_w2v : np.ndarray
Rotation matrix from world to plant coordinates (3x3)
P_c2v : np.ndarray
Camera ray direction in plant coordinates (3x1)
"""

cm2px = 26.29 # from calibration of the camera, approximate cm to pixel conversion
W, H = 4288, 2848
# Convert variables to 1D arrays for plotting
C = C_o2w.flatten()
V = V_o2w.flatten()

# Create the figure and 3D axes
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# Plot the ground plane (world origin plane)
xx, yy = np.meshgrid(np.linspace(-(W/cm2px)//2, (W/cm2px)//2, 100), np.linspace(-(H/cm2px)//2, (H/cm2px)//2, 100))
zz = np.zeros_like(xx)
ax.plot_surface(xx, yy, zz, color='lightgray', alpha=0.5, edgecolor='none')
# name the ground plane
ax.text(0, 0, 0, "World Origin", color='black', fontsize=10)
# plot the world coordinate frame axis
axis_length = (H/cm2px)//3
ax.quiver(0, 0, 0, 1, 0, 0, length=axis_length, cmap='Reds', normalize=True, label='World X-axis')
ax.quiver(0, 0, 0, 0, 1, 0, length=axis_length, cmap='Reds', normalize=True, label='World Y-axis')
ax.quiver(0, 0, 0, 0, 0, 1, length=axis_length, cmap='Reds', normalize=True, label='World Z-axis')
# Plot the camera image plane
f = 2.4 # Distance from camera center to image plane (arbitrary scale)
w, h = 3.6, 2.4 # Width and height of the image plane rectangle
img_plane_cam = np.array([[-w/2, -h/2, f],
[ w/2, -h/2, f],
[ w/2, h/2, f],
[-w/2, h/2, f]]).T # shape (3,4)

img_plane_world = R_w2c.T @ img_plane_cam + C.reshape(3,1)
print(f" img plane world: {img_plane_world}")
verts = [list(zip(img_plane_world[0,:], img_plane_world[1,:], img_plane_world[2,:]))]
poly = Poly3DCollection(verts, alpha=0.5, facecolor='blue')
ax.add_collection3d(poly)

# Plot key points
ax.scatter(C[0], C[1], C[2], color='red', s=50, label='Camera Center')
ax.scatter(V[0], V[1], V[2], color='green', s=50, label='plant Center')
ax.scatter(0, 0, 0, color='black', s=50, label='World Origin')
ax.scatter(beam_origin.flatten()[0], beam_origin.flatten()[1], beam_origin.flatten()[2], color='purple', s=50, label='beam Origin')

# Plot the camera coordinate axes
axis_length = (H/cm2px)//3
# Plot the camera coordinate axes
ax.quiver(C[0], C[1], C[2],
R_w2c.T[0,0], R_w2c.T[1,0], R_w2c.T[2,0],
length=axis_length, color='darkred', normalize=True, label='Cam X-axis')
ax.quiver(C[0], C[1], C[2],
R_w2c.T[0,1], R_w2c.T[1,1], R_w2c.T[2,1],
length=axis_length, color='darkgreen', normalize=True, label='Cam Y-axis')
ax.quiver(C[0], C[1], C[2],
R_w2c.T[0,2], R_w2c.T[1,2], R_w2c.T[2,2],
length=axis_length, color='darkblue', normalize=True, label='Cam Z-axis')

# Plot the original camera ray
line = np.vstack((C_o2w.flatten(), P_c2w_star.flatten()))
ax.plot(line[:,0], line[:,1], line[:,2], color='magenta', linewidth=2, label='Camera Ray')

# Plot the reflected ray
P_reflected = V_o2w.flatten() + R_w2v.T @ P_c2v.flatten()
line2 = np.vstack((V_o2w.flatten(), P_reflected))
ax.plot(line2[:,0], line2[:,1], line2[:,2], color='orange', linewidth=2, label='Reflected Ray')

# Plot the plant coordinate frame
ax.quiver(V[0], V[1], V[2],
R_w2v.T[0,0], R_w2v.T[1,0], R_w2v.T[2,0],
length=axis_length, color='orange', normalize=True, label='plant X-axis')
ax.quiver(V[0], V[1], V[2],
R_w2v.T[0,1], R_w2v.T[1,1], R_w2v.T[2,1],
length=axis_length, color='purple', normalize=True, label='plant Y-axis')
ax.quiver(V[0], V[1], V[2],
R_w2v.T[0,2], R_w2v.T[1,2], R_w2v.T[2,2],
length=axis_length, color='cyan', normalize=True, label='plant Z-axis')

# plot the beam vector
line3 = np.vstack((beam_origin.flatten(), P_reflected.flatten()))
ax.plot(line3[:,0], line3[:,1], line3[:,2], color='purple', linewidth=2, label='beam Ray')

# plot the beam coordinate frame (same orientation with plant frame, translational difference only)
ax.quiver(beam_origin.flatten()[0], beam_origin.flatten()[1], beam_origin.flatten()[2],
R_w2v.T[0,0], R_w2v.T[1,0], R_w2v.T[2,0],
length=axis_length, color='gold', normalize=True, label='beam X-axis')
ax.quiver(beam_origin.flatten()[0], beam_origin.flatten()[1], beam_origin.flatten()[2],
R_w2v.T[0,1], R_w2v.T[1,1], R_w2v.T[2,1],
length=axis_length, color='pink', normalize=True, label='beam Y-axis')
ax.quiver(beam_origin.flatten()[0], beam_origin.flatten()[1], beam_origin.flatten()[2],
R_w2v.T[0,2], R_w2v.T[1,2], R_w2v.T[2,2],
length=axis_length, color='brown', normalize=True, label='beam Z-axis')
# Finalize the plot
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Visualization of Vectors, Camera Plane, and Ground Plane')
ax.legend()
plt.show()

# Call the function with the required parameters
visualize_camera_ray_system(C_o2w, P_c2w_star, V_o2w, R_w2c, R_w2v, P_c2v,beam_origin)

def calcPitch(v0, v1):
deltax = (v1.reshape(3,1) - v0.reshape(3,1))[0]
deltaz = (v1.reshape(3,1) - v0.reshape(3,1))[2]
return np.arctan2(deltax, deltaz)*180/np.pi
###

# calculate the pitch angle of the camera ray in the plant coordinate frame
theta = calcPitch(V_o2w, V_o2w.flatten() + R_w2v.T @ P_c2v.flatten())
print(f" calculated camera ray pitch in plant coordinates: {theta[0]:.4f} degrees")
< /code>

Wenn < /strong> Sie den MRE erfolgreich ausführen, erhalten Sie diese Vektoren und ihr definiertes Koordinatensystem, die Skala ist Zentimeter: < /p>