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 !