From 69200720726bb4599629e1eb0e2d06d3db7d7b44 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 21 Dec 2025 18:25:23 +0800 Subject: [PATCH] Signed-off-by: unknown --- example1_basic.py | 7 +- src/orisys.egg-info/PKG-INFO | 2 +- src/orisys.egg-info/SOURCES.txt | 1 + src/orisys/getDensity.py | 195 ++++++++++++++++++++++++++++++++ src/orisys/sensor.py | 11 +- 5 files changed, 211 insertions(+), 5 deletions(-) create mode 100644 src/orisys/getDensity.py diff --git a/example1_basic.py b/example1_basic.py index ebda80c..5cc1533 100644 --- a/example1_basic.py +++ b/example1_basic.py @@ -19,12 +19,17 @@ def main(): # 向量场: VRAW, VNORMAL, VSHEAR: 原始，法向，切向 # 帧率，时间戳: FPS, TIMESTAMP # 变形量总和: FNORMAL , FSHEARX, FSHEARY : 法向，切向（X)，切向（Y) - fps, fn, fx, fy, flow = sensor1.read_info(sensor1.info.FPS, sensor1.info.FNORMAL, sensor1.info.FSHEARX, sensor1.info.FSHEARY, sensor1.info.VRAW) + # 深度图: DEPTH + fps, fn, fx, fy, flow, depth = sensor1.read_info(sensor1.info.FPS, sensor1.info.FNORMAL, sensor1.info.FSHEARX, sensor1.info.FSHEARY, sensor1.info.VRAW, sensor1.info.DEPTH) print(fps,fn,fx,fy) # 显示向量场 arrows = sensor1.draw_arrows(sensor1.info.VRAW, threshold=5, grid_spacing=40, arrow_scale=1.0) cv2.imshow("Deformation", arrows) + depth_img = cv2.applyColorMap((depth*1* 255.0).astype('uint8'), cv2.COLORMAP_HOT) + + cv2.imshow('depth', depth_img) + key = cv2.waitKey(1) if key & 0xFF == ord("q"): print("\n 停止程序") diff --git a/src/orisys.egg-info/PKG-INFO b/src/orisys.egg-info/PKG-INFO index 275450a..1dfc6f0 100644 --- a/src/orisys.egg-info/PKG-INFO +++ b/src/orisys.egg-info/PKG-INFO @@ -3,7 +3,7 @@ Name: orisys Version: 0.1.0 Summary: Orisys - Optical Tactile Sensor SDK Author: Your Name -Requires-Python: >=3.7 +Requires-Python: >=3.8 Requires-Dist: numpy Requires-Dist: opencv-python Requires-Dist: scipy diff --git a/src/orisys.egg-info/SOURCES.txt b/src/orisys.egg-info/SOURCES.txt index 906a421..eb39c2c 100644 --- a/src/orisys.egg-info/SOURCES.txt +++ b/src/orisys.egg-info/SOURCES.txt @@ -3,6 +3,7 @@ src/orisys/Stitcher.py src/orisys/StructuredGrid.py src/orisys/UnstructuredGrid.py src/orisys/__init__.py +src/orisys/getDensity.py src/orisys/nHHD.py src/orisys/nPoisson.py src/orisys/sensor.py diff --git a/src/orisys/getDensity.py b/src/orisys/getDensity.py new file mode 100644 index 0000000..effb282 --- /dev/null +++ b/src/orisys/getDensity.py @@ -0,0 +1,195 @@ +import cupy as cp +import cv2 +import numpy as np + +zero_level = None + +def getFilteredDensity(flow, use_cuda): + sigma = 3.0 + r_max = 9.0 + # tmp = time.time() + if use_cuda: + density = getDensityCuda(flow, sigma, r_max) + else: + density = getDensity(flow, sigma, r_max) + # print("Density frequency: ", float(1.0 / (time.time() - tmp))) + # tmp = time.time() + density = correctInvalidArea(density) + # print("Correct frequency: ", float(1.0 / (time.time() - tmp))) + # return density[15:-15, 15:-15] + return density + +def correctInvalidArea(density): + # density = guided_filter(density, density, 5, 0.001, s=4) + # invalid_ind = np.zeros_like(density, dtype='uint8') + global zero_level + if zero_level is None: + zero_level = np.mean(density[60:-60, 60:-60]) + # threshold = zero_level - (density.max() - zero_level) / 120 + threshold = zero_level + invalid_ind = twoClustering(density, threshold) + # density = cv2.inpaint(density * 1e4, invalid_ind, 20, + # cv2.INPAINT_TELEA) / 1e4 + density[np.where(invalid_ind > 0)] = zero_level ## type 1 repair + # density = guided_filter(density, density, 7, 0.005, s=4) + density = cv2.ximgproc.guidedFilter(density, density, 7, 0.005) + return density + # return invalid_ind + + +def twoClustering(density, threshold): + cluster = np.zeros_like(density, dtype='uint8') + cluster[np.where(density < threshold)] = 1 + # kernel = np.ones((3, 3), np.uint8) + # cluster = cv2.dilate(cluster, kernel, iterations=4) + return cluster + + + +def getDensity(flow, sigma, r_max): + """ + Compute Gaussian density map from optical flow vectors. + Pure NumPy implementation without freud dependency. + Vectorized for better performance. + + Args: + flow: Optical flow field with shape (height, width, 2) + sigma: Gaussian kernel standard deviation + r_max: Maximum radius for Gaussian contribution + + Returns: + Negative density map with same shape as flow field (height, width) + """ + # Pre-compute constants + two_sigma2 = 2.0 * sigma * sigma + gaussian_norm = 1.0 / (two_sigma2 * np.pi) + + # Get flow dimensions + height, width = flow.shape[:2] + flow_x = flow[:, :, 0].astype(np.float32) + flow_y = flow[:, :, 1].astype(np.float32) + + # Create coordinate grids + x_coords = np.arange(width, dtype=np.float32) + y_coords = np.arange(height, dtype=np.float32) + grid_x, grid_y = np.meshgrid(x_coords, y_coords) + + # Compute destination positions after flow displacement + dest_x = grid_x + flow_x + dest_y = grid_y + flow_y + + # Initialize result array + result = np.zeros((height, width), dtype=np.float32) + + # Vectorized computation: for each source pixel, compute contributions to nearby grid points + for y in range(height): + for x in range(width): + dest_x_val = dest_x[y, x] + dest_y_val = dest_y[y, x] + + # Determine the bounding box of affected grid points + min_x = max(0, int(np.floor(dest_x_val - r_max))) + max_x = min(width, int(np.ceil(dest_x_val + r_max)) + 1) + min_y = max(0, int(np.floor(dest_y_val - r_max))) + max_y = min(height, int(np.ceil(dest_y_val + r_max)) + 1) + + if min_x >= max_x or min_y >= max_y: + continue + + # Create local grid for this window + local_x = np.arange(min_x, max_x, dtype=np.float32) + local_y = np.arange(min_y, max_y, dtype=np.float32) + local_grid_x, local_grid_y = np.meshgrid(local_x, local_y) + + # Compute distances squared + dx = local_grid_x - dest_x_val + dy = local_grid_y - dest_y_val + distance_sq = dx * dx + dy * dy + + # Filter by r_max (only include points within radius) + mask = distance_sq <= (r_max * r_max) + + # Compute Gaussian contributions + gaussian_vals = np.exp(-distance_sq / two_sigma2) * gaussian_norm + gaussian_vals[~mask] = 0 + + # Accumulate negative density + result[min_y:max_y, min_x:max_x] -= gaussian_vals + + return result + + +class CudaArrayInterface: + def __init__(self, gpu_mat): + w, h = gpu_mat.size() + type_map = { + cv2.CV_8U: "u1", + cv2.CV_8S: "i1", + cv2.CV_16U: "u2", + cv2.CV_16S: "i2", + cv2.CV_32S: "i4", + cv2.CV_32F: "f4", + cv2.CV_64F: "f8", + } + self.__cuda_array_interface__ = { + "version": 2, + "shape": (h, w), + "data": (gpu_mat.cudaPtr(), False), + "typestr": type_map[gpu_mat.type()], + "strides": (gpu_mat.step, gpu_mat.elemSize()), + } + + +func_string = r""" + extern "C"{ + __device__ float gaussian(float twoSigma2, float dx, float dy) + { + float pi = 3.1415926535897932384626; + float distance = dx*dx+dy*dy; + float result = expf(-distance/twoSigma2); + result = result/(twoSigma2*pi); + return result; + } + __global__ void density(const float *flowX, const float *flowY, float *result, int x_size, int y_size, float twoSigma2, float r_max) + { + int x = threadIdx.x; + int y = blockIdx.x; + float dest_x = float(x)+flowX[y*x_size+x]; + float dest_y = float(y) + flowY[y*x_size+x]; + for(int ind_x=ceilf(dest_x-r_max);ind_x= x_size || ind_x<0 || ind_y >= y_size || ind_y < 0) + { + continue; + } + float dx = float(ind_x)-dest_x; + float dy = float(ind_y)-dest_y; + atomicAdd(result+ind_y*x_size+ind_x, -gaussian(twoSigma2, dx,dy)); + } + } + } + """ +funcupy = cp.RawModule(code=func_string).get_function("density") + + +def getDensityCuda(flow, sigma, r_max): + twoSigma2 = cp.float32(2.0 * sigma**2) + r_max = cp.float32(r_max) + if type(flow).__module__ == np.__name__: + flowx = cp.array(flow[:, :, 0], dtype=cp.float32) + flowy = cp.array(flow[:, :, 1], dtype=cp.float32) + else: + flowx, flowy = cv2.cuda.split(flow) + flowx = cp.array(CudaArrayInterface(flowx), + dtype=cp.float32, + copy=True) + flowy = cp.array(CudaArrayInterface(flowy), + dtype=cp.float32, + copy=True) + result = cp.zeros((flowx.shape[0], flowx.shape[1]), dtype=cp.float32) + block_dim = (flowx.shape[1], ) + grid = (flowx.shape[0], ) + funcupy(grid, block_dim, (flowx, flowy, result, cp.int32( + flowx.shape[1]), cp.int32(flowx.shape[0]), twoSigma2, r_max)) + return result.get() diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index e9e033b..a3f2250 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -12,6 +12,7 @@ import time from scipy.interpolate import griddata from enum import Enum, auto from typing import Optional, Tuple, Union, Any +from .getDensity import getFilteredDensity class Sensor: """ @@ -83,7 +84,7 @@ class Sensor: self.stitch_valid = False self.n_boxes = None self.stitch_calibration = [] - self.img = self.flow = self.normal = self.shear = self.force = None + self.img = self.flow = self.normal = self.shear = self.force = self.depth = None self.fnormal=self.fshearx=self.fsheary=self.ftor=0.0 self.imgupdate = self.vupdate = False self.pos_default = [(260, 56), (577, 62), (897, 69), (1214, 75), (1530, 81), (256, 391), (570, 397), (889, 403), (1208, 409), (1523, 416), (248, 724), (563, 730), (882, 738), (1201, 743), (1516, 751)] @@ -109,6 +110,7 @@ class Sensor: FNORMAL = auto() FSHEARX = auto() FSHEARY = auto() + DEPTH = auto() def get_img(self): @@ -161,9 +163,10 @@ class Sensor: self.normal = self.hhd.get_normal() self.shear = self.hhd.get_shear() flag, [self.fnormal,self.fshearx,self.fsheary,self.ftor] = self.hhd.get_force(flow) - + self.depth = getFilteredDensity(self.flow, use_cuda=self.cuda) self.fps = 1/(time.time() - self.last_log_time) self.last_log_time = time.time() + return self.flow @@ -183,6 +186,7 @@ class Sensor: - FSHEAR: Shear force magnitude (float) - FPS: Current frames per second (float) - TIMESTAMP: Current timestamp string (str) + - DEPTH: Depth image (np.ndarray) Example: normal, shear, fps = sensor.read_info(sensor.info.VNORMAL, sensor.info.VSHEAR, sensor.info.FPS) @@ -215,7 +219,8 @@ class Sensor: results.append(self.fps) elif arg == self.info.TIMESTAMP: results.append(time.strftime('%H:%M:%S')) - + elif arg == self.info.DEPTH: + results.append(self.depth if self.depth is not None else np.array([])) return tuple(results) if results else () def disconnect(self):