Given a pair of stereo cameras, my goal is to find the real speed of a detected object.
The distance between the two cameras is about 5 meters.
However, the estimated speed is 3~4 times of the actual speed.
(If the ground truth is 60kph, the prediction is 240kph)
I have verified that K
and R
are correct (judging by the visualization)
Any help will be appreciated!
import cv2
import numpy as np
# Camera 1
P1, K1, R1, t1
# Camera 2
P2, K2, R2, t2
import numpy as np
obj_3d_coordinates = []
for left_coordinate, right_coordinate in zip(left_coordinates, right_coordinates):
obj_coords_image1 = np.array([*left_coordinate, 1]).T
obj_coords_image2 = np.array([*right_coordinate, 1]).T
# Convert image points to normalized coordinates
p1_norm = np.linalg.inv(K1) @ obj_coords_image1
p2_norm = np.linalg.inv(K2) @ obj_coords_image2
# Formulate the system of equations
A = np.array([
p1_norm[0] * R1[2, :] - R1[0, :],
p1_norm[1] * R1[2, :] - R1[1, :],
p2_norm[0] * R2[2, :] - R2[0, :],
p2_norm[1] * R2[2, :] - R2[1, :]
])
b = np.array([
t1[0] - p1_norm[0] * t1[2],
t1[1] - p1_norm[1] * t1[2],
t2[0] - p2_norm[0] * t2[2],
t2[1] - p2_norm[1] * t2[2]
]).flatten()
# Solve the system using least squares
P_world, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
obj_3d_coordinates.append(P_world)
c = np.array([255,0,0])
c = c / 255
c = np.tile(c, (len(obj_3d_coordinates), 1))
# It's a 500 fps camera
time_interval = 1.0 / 500 # Time interval between frames in seconds
camera_distance_meters = 5 # Actual distance between the two cameras in meters
# # Calculate the scale factor to adjust the 3D coordinates
scale_factor = camera_distance_meters / np.linalg.norm(t2 - t1)
obj_3d_coordinates_scaled = [scale_factor * point for point in obj_3d_coordinates]
# Calculate the distances traveled between consecutive frames
distances = [np.linalg.norm(obj_3d_coordinates_scaled[i+1] - obj_3d_coordinates_scaled[i]) for i in range(len(obj_3d_coordinates_scaled) - 1)]
distances = np.array(distances)
avg_distance = np.mean(distances)
avg_speed = avg_distance / time_interval
print("Speeds (m/s):", avg_speed)
# m/s to km/h
avg_speed_kmh = avg_speed * 3.6
print("Speeds (km/h):", avg_speed_kmh)