UPDATE Vision 新增Nan值判断

Vision/tool/utils.py

@@ -13,14 +13,82 @@ import cv2
 import psutil
 from psutil._common import bytes2human
 
-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:
+
+def out_bounds_dete(pm_y, pm_x, piont_y, piont_x):
+    if piont_y>pm_y:
+        piont_y = pm_y
+        print('四坐标点超出点云大小')
+    if piont_y<0:
+        piont_y=0
+        print('四坐标点超出点云大小')
+    if piont_x>pm_x:
+        piont_x = pm_x
+        print('四坐标点超出点云大小')
+    if piont_x<0:
+        piont_x=0
+        print('四坐标点超出点云大小')
+    return piont_y, piont_x
+
+def remove_nan_mean_value(pm, y, x, iter_max=15):
+    point_x, point_y, point_z = pm[y, x]
+    if np.isnan(point_x):
+        print('Nan值去除')
+        point_x_list = []
+        point_y_list = []
+        point_z_list = []
+        iter_current = 1
+        pm_shape_y = pm.shape[0]
+        pm_shape_x = pm.shape[1]
+        remove_nan_isok = False
+        while iter_current < iter_max:
+            # 计算开始点
+            if y - iter_current > 0:
+                y_start = y - iter_current
+            else:
+                y_start = 0
+
+            if x - iter_current > 0:
+                x_start = x - iter_current
+            else:
+                x_start = 0
+
+            for idx_y in range(iter_current*2 + 1):
+                y_current = y_start + idx_y
+                if y_current > pm_shape_y-1:
+                    continue
+                for idx_x in range(iter_current*2 + 1):
+                    x_current = x_start + idx_x
+                    if x_current > pm_shape_x-1:
+                        continue
+                    elif np.isnan(pm[y_current, x_current][0]) == False:
+                        point_x_list.append(pm[y_current, x_current][0])
+                        point_y_list.append(pm[y_current, x_current][1])
+                        point_z_list.append(pm[y_current, x_current][2])
+
+            len_point_x = len(point_x_list)
+            if len_point_x>0:
+                point_x = sum(point_x_list)/len_point_x
+                point_y = sum(point_y_list)/len_point_x
+                point_z = sum(point_z_list)/len_point_x
+                remove_nan_isok = True
                 break
-    return piont_x, piont_y, piont_z
+            iter_current += 1
+    else:
+        remove_nan_isok = True
+    if remove_nan_isok == True:
+        return point_x, point_y, point_z
+    else:
+        print('在15*15范围中未找到有效值，所有点云值为无效值')
+        return np.nan, np.nan, np.nan
+
+def remove_nan(pm, y, x):
+    point_x, point_y, point_z = pm[y, x]
+    if np.isnan(point_x):
+        for i in range(10):
+            point_x, point_y, point_z = pm[y+i, x]
+            if np.isnan(point_x)==False:
+                break
+    return point_x, point_y, point_z
 
 def get_disk_space(path='C:'):