UPDATE Vision 新增box[]输出

Vision/tool/utils.py

@@ -12,6 +12,47 @@ import numpy as np
 import cv2
 import psutil
 from psutil._common import bytes2human
+from sklearn.decomposition import PCA
+
+def pca(points):
+    # 计算PCA
+    mean = np.mean(points, axis=0)
+    cov = np.cov(points, rowvar=False)
+    eigenvalues, eigenvectors = np.linalg.eigh(cov)
+
+    # 对特征值进行排序并找到对应的特征向量
+    sorted_indices = np.argsort(eigenvalues)[::-1]
+    sorted_eigenvalues = eigenvalues[sorted_indices]
+    sorted_eigenvectors = eigenvectors[:, sorted_indices]
+
+    # # 打印PCA结果
+    # print("PCA Mean:", mean)
+    # print("PCA Eigenvalues:", sorted_eigenvalues)
+    # print("PCA Eigenvectors:\n", sorted_eigenvectors)
+
+    # 通常，最小特征值对应的特征向量是平面的法线
+    normal_vector = sorted_eigenvectors[:, 0] if sorted_eigenvalues[0] < sorted_eigenvalues[
+        1] else sorted_eigenvectors[:, 1]
+    # print("法向量：", normal_vector)
+    return normal_vector
+
+
+def compute_pca_direction(points):
+    # points 是物体点云的坐标
+    pca = PCA(n_components=3)
+    pca.fit(points)
+    primary_direction = pca.components_[0]  # 第一主成分对应的方向向量
+    return primary_direction
+
+def remove_nan(pm, y, x):
+    piont_x, piont_y, piont_z = pm[y, x]
+    if np.isnan(piont_x):
+        for i in range(10):
+            piont_x, piont_y, piont_z = pm[y+i, x]
+            if np.isnan(piont_x)==False:
+                break
+    return piont_x, piont_y, piont_z
+
 def get_disk_space(path='C:'):
 
     usage = psutil.disk_usage(path)