Transform a image perspective given Euler Angles of a drone

I am working on a transmission line following algorithm using quadcopters. To do so, I need to calculate the line position on the image I receive from the UAV in order to determine a pitch velocity so the lines can be kept at the center of the image. The problem is that, when I apply a velocity on x-axis to move the UAV to the desired setpoint (left/right moviment), the image plane tilts along with the UAV which increases the positional error incorrectly. The images below exemplify the issue.

I tried something similar to this post since the UAV euler angles is known. This approach reduced the distortion caused by the frame tilting, but I couldn’t eliminate it.

(c++ - Transform a frame to be as if it was taken from above using OpenCV - Stack Overflow)

The code:

f = 692.81 # Focal Length

cx = width
cy = height

#Euller Angles 

roll_ = 0
pitch_ = pitch
yaw_ = 0

dx = 0
dy = 0
dz = 1

A2 = np.array([[f,0,cx,0],[0,f, cy,0],[0,0,1,0]])

A1 = np.array([[1/f,0,-cx/f],[0,1/f,-cy/f],[0,0,0],[0,0,dz]])


RX = np.array([[1,0,0,0],[0,np.cos(roll_),-(np.sin(roll_)),0],[0, np.sin(roll_), np.cos(roll_),0],[0,0,0,1]])
RY = np.array([[np.cos(pitch_),  0, -np.sin(pitch_),0],[0,1,0,0],[(np.sin(pitch_)), 0, np.cos(pitch_),0],[0,0,0,1]])
RZ = np.array([[np.cos(yaw_), -(np.sin(yaw_)), 0,0],[np.sin(yaw_),  np.cos(yaw_), 0,0],[0,0,1,0],[0,0,0,1]]) 

T = np.array([[1, 0, 0, dx],[0, 1, 0, dy],[0, 0, 1, dz],[0, 0, 0, 1]])    

R = np.dot(np.dot(RX, RY), RZ)

H = np.dot(A2, np.dot(T, np.dot(R, A1)))

#The output frame

linha_bw = cv2.warpPerspective(linha_bw, H,(frame.shape[1],frame.shape[0]),None,cv2.INTER_LINEAR)

The results from this transformation can be seen on the graph below. The blue curve is the controller without the image rectification, while the red one is the controller with the code above.

I’m not sure if there is mistakes on my code or there is a better approach to solve my problem through image processing techniques. Any help is highly appreciated !

so… you need to involve an IMU so that you can know the tilt of the vehicle, allowing you to rectify the picture?

ok then.

Euler angles have downsides. use whatever works. you need a rotation matrix.

once you have the rotation matrix, you compose that, with a camera matrix and its inverse (H = K \cdot R \cdot K^{-1}). that gives you a homography. you use that to “warp” the camera picture.

check your intrinsics. cx is NOT equal to width. if you don’t have specific knowledge of cx, assume cx = (width-1) / 2. same for cy.

DO NOT manually calculate an inverse of a camera matrix. that’s either silly or wrong. just use an inversion function. np.linalg.inv is fine to use.

use the @ operator (infix) instead of np.dot. makes the expression readable.

use synthetic values (and pictures) and experiment with those, so you know what the result is supposed to look like.

a focal length and image size giving a 90 degree field of view may be easiest to experiment with.

Thank you for your response ! I’ll test it soon and give a feedback.