计算相机与标记物之间的距离

下面的代码首先加载一个包含ArUco标记的图像，检测图像中的ArUco标记，并计算每个标记到相机中心的距离。

第二段Python代码将计算图像中检测到的ArUco标记之间的成对距离。

我们将使用一张包含两个ArUco标记的图像。这两个标记的中心之间相距约19厘米，相机与图像的距离约为60厘米。

计算相机到某些标记的距离

以下是计算相机到某些标记距离的示例。

1import cv2
2import numpy as np
3
4def postprocessDistToCam(ids, tvec):
5    if ids is not None:
6        for i, tvec in enumerate(tvecs):
7            marker_id = ids[i][0]
8            distance = np.linalg.norm(tvec)
9            print(f"标记 {marker_id} 到相机中心的距离: {distance:.2f} 米")
10
11image_file = './resources/marker_images_dist/calibrate_20230704_134548.png'
12image = cv2.imread(image_file)
13calibration_data_file = "./resources/calibration_data.npz"
14camera_matrix, distortion_coeffs = loadCalibrationData(calibration_data_file)
15
16height, width = image.shape[:2]
17new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(camera_matrix, distortion_coeffs, (width, height), 0, (width, height))
18image = cv2.undistort(image, camera_matrix, distortion_coeffs, None, new_camera_matrix)
19  
20processed_image = preprocessImage(image)
21
22marker_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_250)
23detector_params = cv2.aruco.DetectorParameters_create()
24detections, ids = detectMarkers(processed_image, marker_dict, detector_params)
25
26marker_length = .05 # 单位：米
27rvecs, tvecs = poseEstimation(detections, marker_length, camera_matrix, distortion_coeffs)
28
29postprocessDistToCam(ids, tvecs)
30
31result = visualization(image, detections, ids, camera_matrix, distortion_coeffs, rvecs, tvecs, marker_length)
32
33#cv2.imwrite(f"./output/marker_detection.png", result)

第1–2行： 首先导入 cv2（OpenCV）和 numpy。OpenCV 是用于计算机视觉的库，numpy 是科学计算包，pandas 是数据分析和处理工具。

第4–9行： 定义辅助函数：代码定义了多个用于特定任务的函数：

• postprocessDistToCam()：计算每个检测到的标记到相机中心的距离。距离通过每个标记的平移向量（tvec）的弗罗贝尼乌斯范数（即欧几里得范数）来计算。弗罗贝尼乌斯范数，也称为欧几里得范数，是衡量矩阵或向量大小（长度）的一种方式。其计算方法为各元素绝对值平方和的平方根。

• 其他在代码示例中使用的辅助函数与前一课相同。

第11–14行： 首先从指定路径加载图像，并从 calibration_data.npz 文件加载校准数据。

第17行： 接着使用加载的校准数据创建一个 new_camera_matrix，以最小化图像畸变。然后使用原始的 camera_matrix、distortion_coeffs 和 new_camera_matrix 对图像进行去畸变处理。

第20行： 然后使用 preprocessImage() 函数对去畸变后的图像进行预处理（转换为灰度图）。

第22–24行： 创建一个 ArUco 字典和检测参数。使用 cv2.aruco.detectMarkers() 函数在预处理后的图像中检测标记。

第27行： 接着使用 poseEstimation() 函数估计每个检测到的标记的姿态。

第29行： 使用 postprocessDistToCam() 辅助函数计算并打印每个检测到的标记到相机中心的距离。

第31行： 最后，使用 visualization() 函数将检测到的标记及其估计姿态绘制在去畸变图像上。结果图像保存到输出目录。

第33行： 若要查看可视化结果，需取消注释最后一行代码。

此外，请注意计算出的距离会打印到控制台。

计算出的标记与相机之间的距离相当精确，确实应接近60厘米。

计算标记之间的距离

以下代码与上述基本相同，但使用了不同的后处理辅助函数。

1import cv2
2import numpy as np
3import pandas as pd
4
5def postprocessDistBetween(ids, tvecs):
6    num_ids = len(ids)
7    true_ids = []
8    if num_ids > 1:
9        distances = np.zeros((num_ids, num_ids))
10        for i, tvec_i in enumerate(tvecs):
11            marker_id = ids[i][0]
12            true_ids.append(marker_id)
13            for j, tvec_j in enumerate(tvecs):
14                if i != j:
15                    distances[i,j] = np.linalg.norm(tvec_i - tvec_j)
16    distances = pd.DataFrame(distances, columns=true_ids, index=true_ids)
17    print("标记间距离（单位：米）:\n")
18    print(distances.to_html())
19
20image_file = './resources/marker_images_dist/calibrate_20230704_134548.png'
21image = cv2.imread(image_file)
22    
23calibration_data_file = "./resources/calibration_data.npz"
24camera_matrix, distortion_coeffs = loadCalibrationData(calibration_data_file)
25height, width = image.shape[:2]
26new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(camera_matrix, distortion_coeffs, (width, height), 0, (width, height))
27image = cv2.undistort(image, camera_matrix, distortion_coeffs, None, new_camera_matrix)
28  
29processed_image = preprocessImage(image)
30
31marker_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_250)
32detector_params = cv2.aruco.DetectorParameters_create()
33detections, ids = detectMarkers(processed_image, marker_dict, detector_params)
34
35marker_length = .05 # 单位：米
36rvecs, tvecs = poseEstimation(detections, marker_length, camera_matrix, distortion_coeffs)
37
38postprocessDistBetween(ids, tvecs)
39
40result = visualization(image, detections, ids, camera_matrix, distortion_coeffs, rvecs, tvecs, marker_length)
41
42cv2.imwrite(f"./output/marker_detection.png", result)

第5–18行： 定义 postprocessDistBetween() 函数以计算标记之间的距离。该函数计算所有标记之间的成对距离，并将其存入一个 pandas DataFrame 中。随后以 HTML 表格形式打印该表格。

其他辅助函数已在前一课中说明。

流程与之前一致：

• 加载包含 ArUco 标记的图像。
• 从文件加载校准数据。
• 使用校准数据对图像进行去畸变处理。
• 对图像进行预处理（转为灰度图）。
• 检测图像中的 ArUco 标记。
• 估计检测到的标记的姿态。

第38行： 然后调用 postprocessDistBetween() 函数计算每对检测到的标记之间的距离。这些距离将以 HTML 表格形式打印到控制台。

第40行： 最后，使用 visualization() 函数对标记进行可视化。生成的结果图像将被保存为输出文件。

在输出中可以看到，两个标记中心之间的距离约为19厘米，这正是我们预期的结果。