Signed-off-by: unknown <ykcheungab@MEE1965.ust.hk>

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

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

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<ceilf(dest_x+r_max); ++ind_x)
+            for(int ind_y=ceilf(dest_y-r_max);ind_y<ceilf(dest_y+r_max); ++ind_y)
+            {
+                if(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()

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