Find Distances Between the Camera and Markers

import cv2
import numpy as np
def postprocessDistToCam(ids, tvec):
    if ids is not None:
        for i, tvec in enumerate(tvecs):
            marker_id = ids[i][0]
            distance = np.linalg.norm(tvec)
            print(f"Distance from marker {marker_id} to camera center: {distance:.2f} meters")
image_file = './resources/marker_images_dist/calibrate_20230704_134548.png'
image = cv2.imread(image_file)
calibration_data_file = "./resources/calibration_data.npz"
camera_matrix, distortion_coeffs = loadCalibrationData(calibration_data_file)
height, width = image.shape[:2]
new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(camera_matrix, distortion_coeffs, (width, height), 0, (width, height))
image = cv2.undistort(image, camera_matrix, distortion_coeffs, None, new_camera_matrix)
  
processed_image = preprocessImage(image)
marker_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_250)
detector_params = cv2.aruco.DetectorParameters_create()
detections, ids = detectMarkers(processed_image, marker_dict, detector_params)
marker_length = .05 #in meters
rvecs, tvecs = poseEstimation(detections, marker_length, camera_matrix, distortion_coeffs)
postprocessDistToCam(ids, tvecs)
result = visualization(image, detections, ids, camera_matrix, distortion_coeffs, rvecs, tvecs, marker_length)
#cv2.imwrite(f"./output/marker_detection.png", result)