Computing the Camera Ray’s Pitch Angle in different coordinate frames

I would like to calculate the pitch angle of a camera ray vector that corresponds to an image point (u,v). This vector is moved to plant coordinate frame and named as Reflected_Ray vector.

How to calculate the angle of the Reflected_Ray vector component that lies on XZ plane?

The Reflected_Ray vector has the origin in the plant coordinate frame and its end point coincides with the XY plane of the world coordinate frame. This end point is the 3D point of the image point which is (u,v).

MRE is given as following:

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")

If you successfully run the MRE, you would obtain this vectors and their defined coordinate system, the scale is centimeter:

If I understand the problem correctly, then Reflected_Ray is the vector that points from plant coordinate frame origin towards image point (u,v). To calculate the angle that the Reflected_Ray components make in the world coordinate frame, we transform the vector components of Reflected_Ray from plant coordinate frame (PCF) to world coordinate frame (WCF). We can then simply calculate the angle formed by the X and Z components, w.r.t to the +ve x axis (or otherwise as desired).

1. Transform coordinates

   WCF(u', v', w') = A*PCF(u, v, 0)

   where A represents the Euler angle rotation matrix product to rotate the PCF unit vectors into the WCF coordinate system. The linear shift is not important here, as we are only interested in the magnitude of the unit vectors.

2. Calculate projected angle

   theta = arctan( WCF(w') / WCF(u') )

Apologies for the terrible presentation, I'm not sure if it's possible to use MathJax in SO answers? Also, apologies if I have misinterpreted your question...