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)