[Python] How to properly handle vehicle turns when calculating the direction of motion based...

I'am working on a program that analyzes video from a car's front-facing camera to display the vehicle's direction of motion using an arrow drawn on the video. The program uses lane marking coordinates extracted from the video (stored in data.log) to calculate the angle of motion and draw the arrow accordingly.

The data.log file contains lane marking coordinates for each frame. Here’s an example of how the data looks: [[{'lane_class': 2, 'lane_points': [[218, 580], [236, 570], [253, 560]...

To calculate the angle of motion, I use the following function: This function calculates the slope (m) of the lane markings using linear regression and converts it to an angle in degrees:

def calculate_lane_angle(lane_points, image_height, min_points=2):
if len(lane_points) < min_points:
return None

points = np.array(lane_points)
x = points[:, 0]
y = image_height - points[:, 1] # Invert Y-axis

if np.var(x) < 1e-6:
return 90.0 if y[0] < y[-1] else -90.0

A = np.vstack([x, np.ones(len(x))]).T
try:
m, _ = np.linalg.lstsq(A, y, rcond=None)[0]
except np.linalg.LinAlgError:
return None

angle = np.degrees(np.arctan(m))
return angle


Here’s how I process the video frames to calculate the angle and draw the arrow:

def process_video(video_path, logs, output_path):
cap = cv2.VideoCapture(video_path)
if not cap.isOpened():
raise ValueError("Failed to open video")

width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(cap.get(cv2.CAP_PROP_FPS))
fourcc = cv2.VideoWriter_fourcc(*'avc1')
out = cv2.VideoWriter(output_path, fourcc, fps, (width, height))

kf = KalmanFilter()
last_angle = 0.0
angle_history = []
max_change = 3 # Maximum change in angle per frame
median_window = 5

frame_idx = 0
while cap.isOpened():
ret, frame = cap.read()
if not ret or frame_idx >= len(logs):
break

frame_data = logs[frame_idx][0] # First list — lane data
car_center_x = width // 2

# Filter lanes into left and right
left_lane = None
right_lane = None
for lane in frame_data:
if "lane_points" in lane:
pts = np.array(lane["lane_points"])
x_coords = pts[:, 0]
avg_x = np.mean(x_coords)

if avg_x < car_center_x:
if left_lane is None or abs(avg_x - car_center_x) < abs(left_lane["avg_x"] - car_center_x):
left_lane = {"lane": lane, "avg_x": avg_x}
else:
if right_lane is None or abs(avg_x - car_center_x) < abs(right_lane["avg_x"] - car_center_x):
right_lane = {"lane": lane, "avg_x": avg_x}

# Calculate angle
angles = []
if left_lane and right_lane:
for lane in [left_lane, right_lane]:
pts = lane["lane"]["lane_points"]
angle = calculate_lane_angle(pts, height)
if angle is not None:
angles.append(angle)
raw_angle = np.mean(angles) if angles else last_angle
elif left_lane or right_lane:
lane = left_lane if left_lane else right_lane
pts = lane["lane"]["lane_points"]
raw_angle = calculate_lane_angle(pts, height) or last_angle
if abs(raw_angle) > 5:
raw_angle = np.sign(raw_angle) * 5
else:
if len(angle_history) > 1:
trend = angle_history[-1] - angle_history[-2]
raw_angle = angle_history[-1] + trend
else:
raw_angle = last_angle


filtered_angle = kf.update(raw_angle)

# Limit rate of change
delta = filtered_angle - last_angle
if abs(delta) > max_change:
filtered_angle = last_angle + np.sign(delta) * max_change

# Median filter
angle_history.append(filtered_angle)
if len(angle_history) > median_window:
angle_history.pop(0)
final_angle = np.median(angle_history[-median_window:])

adjust_angle = (final_angle + 90) % 360
if adjust_angle > 180:
adjust_angle -= 360
dx = int(arrow_length * np.cos(np.radians(adjust_angle)))
dy = int(arrow_length * np.sin(np.radians(adjust_angle)))
start = (width // 2, height - 50)
end = (start[0] + dx, start[1] - dy)

if 0 <= end[0] < width and 0 <= end[1] < height:
cv2.arrowedLine(frame, start, end, arrow_color, arrow_thickness, tipLength=0.3)

out.write(frame)
frame_idx += 1

cap.release()
out.release()
cv2.destroyAllWindows()


What Works: The arrow correctly shows the direction during straight-line motion and The Kalman filter and median smoothing help reduce noise in the angle calculation.

What Doesn’t Work: During turns (e.g., at intersections), the arrow does not respond smoothly or accurately. For example: When the car turns right, the arrow should point to the right, but it remains mostly unchanged. After completing the turn, the arrow should return to pointing straight ahead.

What I’ve Tried: I implemented a Kalman filter to smooth the angle values, but it doesn’t handle sharp changes well. I applied a median filter to reduce noise, but it also delays the response to turns. I limited the maximum change in angle per frame to avoid abrupt jumps, but this makes the arrow unresponsive during turns.

Expected Behavior: During a turn, the arrow should smoothly rotate to reflect the vehicle's changing direction. After completing the turn, the arrow should smoothly return to pointing straight ahead.

Problem visualisation on video: https://youtu.be/WoHWuMinlgc

Continue reading...