UPDATE Vision 新增Nan值判断

Vision/tool/CameraPe.py

@@ -145,7 +145,7 @@ class camera():
         :return: ret , img, point_map
         '''
         if self.caminit_isok == False or self.event.IsOffline():
-            return 0, None
+            return 0, None, None
         else:
             image_list = self.cl.DeviceStreamRead(self.handle, 2000)
             if len(image_list) == 2:
@@ -153,19 +153,18 @@ class camera():
                     frame = image_list[i]
                     if frame.streamID == PERCIPIO_STREAM_DEPTH:
                         img_depth = frame
-                        img_depth2p3d = frame
                     if frame.streamID == PERCIPIO_STREAM_COLOR:
                         img_color = frame
 
-                self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height,
-                                                              self.scale_unit, img_depth, self.color_calib, img_color.width,
-                                                              img_color.height, self.img_registration_depth)
-
+                # self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height,
+                #                                               self.scale_unit, img_depth, self.color_calib, img_color.width,
+                #                                               img_color.height, self.img_registration_depth)
+                #
                 # self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render)
                 # mat_depth_render = self.img_registration_render.as_nparray()
                 # cv2.imshow('registration', mat_depth_render)
 
-                self.cl.DeviceStreamMapDepthImageToPoint3D(self.img_registration_depth, self.depth_calib, self.scale_unit,
+                self.cl.DeviceStreamMapDepthImageToPoint3D(img_depth, self.depth_calib, self.scale_unit,
                                                       self.pointcloud_data_arr)
 
                 # show p3d arr data

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:'):