Stereo Vision depth calculation (not calibration!)

Python
1

Hello,
I have a stereo vision system that was already calibrated.
I’m trying to get 3D depth data out of this system using single corresponding pixel value pairs.
So the main goal is to get a Z-value out of two x-y pairs + calibration data.

I copy pasted some python code together and made it actually work (somehow).

This is the calibration information I got (intrinsic and extrinsic)

This is Camera 2 (Camera 1 is follows the same format):

Translation: 188.36732 8.202 17.56879
Rotation: 0.97323 -0.021649 -0.228776 0.0202371 0.999758 -0.0085198 0.228906 0.003662 0.97344
Distortion: -0.08698 0.3207 -0.00673 -0.00274 -32.80
Intrinsic: 721.33 0.0 152.310.0 393.79 74.44 0.0 0.0 1.0

This is the example python code I’m using:

import numpy as np
import cv2

# Function to calculate depth and distance with distortion correction
def calculate_depth_with_distortion(pixel_loc_cam1, pixel_loc_cam2, translation_vector, rotation_matrix, baseline, focal_length, camera_matrix1, dist_coeffs1, camera_matrix2, dist_coeffs2):
    # Convert pixel locations to homogeneous coordinates
    x1, y1 = pixel_loc_cam1
    x2, y2 = pixel_loc_cam2

    # Undistort the points
    undistorted_pixel_loc_cam1 = cv2.undistortPoints(np.array([[x1, y1]]), camera_matrix1, dist_coeffs1, P=camera_matrix1)
    undistorted_pixel_loc_cam2 = cv2.undistortPoints(np.array([[x2, y2]]), camera_matrix2, dist_coeffs2, P=camera_matrix2)

    # Extract undistorted coordinates
    x1_undistorted, y1_undistorted = undistorted_pixel_loc_cam1[0][0]
    x2_undistorted, y2_undistorted = undistorted_pixel_loc_cam2[0][0]

    # Create projection matrices
    P1 = np.hstack((camera_matrix1, np.zeros((3, 1))))
    P2 = np.hstack((np.dot(camera_matrix2, rotation_matrix), np.reshape(translation_vector, (3, 1))))

    # Create matrices for calculation
    A = np.array([
        x1_undistorted * P1[2] - P1[0],
        y1_undistorted * P1[2] - P1[1],
        x2_undistorted * P2[2] - P2[0],
        y2_undistorted * P2[2] - P2[1]
    ])

    # Perform SVD
    _, _, V = np.linalg.svd(A)

    # Retrieve 3D point
    X = V[-1, :3] / V[-1, 3]

    # Calculate disparity
    disparity = np.abs(x1_undistorted - x2_undistorted)

    # Calculate depth and distance
    depth = np.linalg.norm(X)
    distance = baseline * focal_length / disparity

    return X, depth, distance

# Sample input values
pixel_loc_cam1 = (10.0, 15.0)  # Example pixel location for camera 1
pixel_loc_cam2 = (12.0, 17.0)  # Example pixel location for camera 2
translation_vector = np.array([1, 2, 3])  # Example translation vector
rotation_matrix = np.array([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6], [0.7, 0.8, 0.9]])  # Example rotation matrix
baseline = 100  # Example baseline in millimeters
focal_length = 50  # Example focal length in millimeters
camera_matrix1 = np.array([[1000, 0, 320], [0, 1000, 240], [0, 0, 1]])  # Example camera matrix for camera 1
dist_coeffs1 = np.array([-0.2, 0.1, -0.001, 0.001, 0.0])  # Example distortion coefficients for camera 1
camera_matrix2 = np.array([[1100, 0, 340], [0, 1100, 260], [0, 0, 1]])  # Example camera matrix for camera 2
dist_coeffs2 = np.array([-0.3, 0.2, -0.002, 0.002, 0.0])  # Example distortion coefficients for camera 2

# Calculate depth and distance with distortion correction
depth, actual_depth, distance = calculate_depth_with_distortion(pixel_loc_cam1, pixel_loc_cam2, translation_vector, rotation_matrix, baseline, focal_length, camera_matrix1, dist_coeffs1, camera_matrix2, dist_coeffs2)
print("Depth information:", depth)
print("Actual depth in mm:", actual_depth)
print("Distance to the target in mm:", distance)

After giving the code some X-Y Values and my calibration data I’m getting distance values that are far off. e.g. instead of 600mm (calculated with some other trustworthy black box code that I’m trying to recreate) I’m getting 2mm output.

Is there any guidiance how to debug the code and to find an efficient way to calculate the correct distance values out of xyxy ?

Thank you very much!