orisys-tech/orisys-sdk
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add private variable and copy when return by readinfo

Browse Source
This commit is contained in:
unknown
2025-12-30 14:52:06 +08:00
parent 2a739ed27c
commit 398d2b1706
8 changed files with 134 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/orisys/sensor.py
+110 -105
View File
@@ -18,7 +18,6 @@ from queue import Queue, Empty
from .Stitcher import Stitcher
from .tracker import Tracker
from .nHHD import nHHD
# from .getDensity import getFilteredDensity
from . import configs
from .get_depth import Contact3D
@@ -48,6 +47,7 @@ class Sensor:
CENTROID = auto()
CONTOUR = auto()
DEPTH = auto()
IMG = auto()
# =========================================================================
# Initialization
@@ -94,25 +94,24 @@ class Sensor:
self.stitch_calibration = []
# Data Variables
self.img = None
self.img_size_raw = None
self.img_filtered = None
self.flow = None
self.vnormal = None
self.vshear = None
# self.depth = None
self.n_boxes = None
self.fnormal = self.fshearx = self.fsheary = self.ftor = 0.0
self.__img = None
self.__img_size_raw = None
self.__img_filtered = None
self.__flow = None
self.__vnormal = None
self.__vshear = None
self.__n_boxes = None
self.__fnormal = self.__fshearx = self.__fsheary = self.__ftor = 0.0
# Contact Data Variables
self.centroid = None
self.centroid_found = False
self.contour = None
self.depth_map = None
self.__centroid = None
self.__centroid_found = False
self.__contour = None
self.__depth_map = None
# Timing
self.last_log_time = time.perf_counter()
self.fps = 0
self._fps = 0
# Async pipeline state
self._async_enabled = False
@@ -125,17 +124,17 @@ class Sensor:
self._stop_event = None
# Load configuration
self.config_cam = None
self.config_stitcher = None
self.config_contact = None
self.__config_cam = None
self.__config_stitcher = None
self.__config_contact = None
self._load_config()
# Initialize components
self._stitcher = Stitcher(isstitch=isstitch, config_stitcher=self.config_stitcher, verbose=self.verbose)
self.tracker = Tracker(adaptive=True, cuda=cuda, verbose=self.verbose)
self.contact3d = Contact3D(config_contact=self.config_contact, verbose=self.verbose)
self.hhd = None
self.__stitcher = Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose)
self.__tracker = Tracker(adaptive=True, cuda=cuda, verbose=self.verbose)
self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose)
self.__hhd = None
# Setup video capture
self._open_video_source(override, box)
@@ -174,35 +173,35 @@ class Sensor:
# Extract and stitch tiles
if log_timing: t0 = time.perf_counter()
self._stitcher.get_stitch_unit(gray)
self.__stitcher.get_stitch_unit(gray)
self._extract_tiles(gray)
if log_timing: timings['get_tiles'] = (time.perf_counter() - t0) * 1000
if log_timing: t0 = time.perf_counter()
img = np.ascontiguousarray(self._stitcher.stitch_rt_optimized())
img = np.ascontiguousarray(self.__stitcher.stitch_rt_optimized())
if log_timing: timings['stitch'] = (time.perf_counter() - t0) * 1000
self.img_size_raw = img.shape
self.__img_size_raw = img.shape
# Resize
img = cv2.resize(img, tuple(self.config_resolution_stitch), interpolation=cv2.INTER_CUBIC)
img = cv2.resize(img, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC)
# Temporal filtering (noise reduction)
alpha = 0.7
if self.img_filtered is None:
self.img_filtered = img.astype(np.float32)
if self.__img_filtered is None:
self.__img_filtered = img.astype(np.float32)
else:
self.img_filtered = alpha * img + (1 - alpha) * self.img_filtered
self.__img_filtered = alpha * img + (1 - alpha) * self.__img_filtered
self.img = self.img_filtered.astype(np.uint8)
self.imgupdate = True
self.__img = self.__img_filtered.astype(np.uint8)
self.__imgupdate = True
if log_timing:
timings['total'] = (time.perf_counter() - t_start) * 1000
self._print_timings("get_img", timings)
if self.mirror:
self.img = cv2.flip(self.img, 1)
self.__img = cv2.flip(self.__img, 1)
return self.img
return True
def compute_deformation(self, log_timing=False):
"""
@@ -217,27 +216,27 @@ class Sensor:
# Optical flow
if log_timing: t0 = time.perf_counter()
flow = self.tracker.track(self.img)
flow = self.__tracker.track(self.__img)
if hasattr(flow, 'download'):
flow = flow.download()
if log_timing: timings['optical_flow'] = (time.perf_counter() - t0) * 1000
# Initialize HHD solver
if self.hhd is None:
self.hhd = nHHD(grid=flow.shape[:2], spacings=(18, 18))
if self.__hhd is None:
self.__hhd = nHHD(grid=flow.shape[:2], spacings=(18, 18))
# Force computation
if log_timing: t0 = time.perf_counter()
if self.cuda and self.CUPY_AVAILABLE:
_, forces = self.hhd.get_force_gpu_v2(flow)
_, forces = self.__hhd.get_force_gpu_v2(flow)
else:
_, forces = self.hhd.get_force(flow)
self.fnormal, self.fshearx, self.fsheary, self.ftor = forces
_, forces = self.__hhd.get_force(flow)
self.__fnormal, self.__fshearx, self.__fsheary, self.__ftor = forces
if log_timing: timings['hhd_decompose'] = (time.perf_counter() - t0) * 1000
# Get components
self.vnormal = self.hhd.get_normal()
self.vshear = self.hhd.get_shear()
self.__vnormal = self.__hhd.get_normal()
self.__vshear = self.__hhd.get_shear()
# # Depth computation
# if log_timing: t0 = time.perf_counter()
@@ -245,40 +244,37 @@ class Sensor:
# if log_timing: timings['get_density'] = (time.perf_counter() - t0) * 1000
# Update state
self.flow = flow
self.__flow = flow
self.imgupdate = False
self.vupdate = True
# FPS calculation
self.fps = 1.0 / (time.perf_counter() - self.last_log_time)
self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
self.last_log_time = time.perf_counter()
if log_timing:
timings['total'] = (time.perf_counter() - t_start) * 1000
self._print_timings("compute_deformation", timings)
return self.flow
return True
def compute_contact(self, multiple_foe=False, min_contour_area=3000, min_magnitude=1.0, expansion_threshold=0.01, downsample_factor=1, kernel_size=(5,5), denoise_sigma=15):
if multiple_foe:
# result = compute_multiple_foe(self.vnormal, min_contour_area=min_contour_area, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
result = self.contact3d.compute_multiple_foe(self.vnormal)
result = self.__contact3d.compute_multiple_foe(self.__vnormal)
else:
if self.cuda:
# result = compute_foe_gpu(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
result = self.contact3d.compute_foe_gpu(self.vnormal)
result = self.__contact3d.compute_foe_gpu(self.__vnormal)
else:
# result = compute_foe(self.vnormal, min_magnitude=min_magnitude, expansion_threshold=expansion_threshold, downsample_factor=downsample_factor, kernel_size=kernel_size, denoise_sigma=denoise_sigma)
result = self.contact3d.compute_foe(self.vnormal)
result = self.__contact3d.compute_foe(self.__vnormal)
self.centroid = result['centroid']
self.centroid_found = result['centroid_found']
self.__centroid = result['centroid']
self.__centroid_found = result['centroid_found']
if 'contour' in result:
self.contour = result['contour']
self.depth_map = result['divergence_map']
self.__contour = result['contour']
self.__depth_map = result['divergence_map']
return result
return True
# def get_depth(self, denoise_sigma=15):
@@ -295,10 +291,10 @@ class Sensor:
self.get_img()
self.compute_deformation()
return {
'img': self.img, 'flow': self.flow,
'normal': self.vnormal, 'shear': self.vshear, 'depth': self.depth,
'fps': self.fps, 'fnormal': self.fnormal,
'fshearx': self.fshearx, 'fsheary': self.fsheary, 'ftor': self.ftor,
'img': self.__img, 'flow': self.__flow,
'normal': self.__vnormal, 'shear': self.__vshear, 'depth': self.__depth_map,
'fps': self._fps, 'fnormal': self.__fnormal,
'fshearx': self.__fshearx, 'fsheary': self.__fsheary, 'ftor': self.__ftor,
}
def read_info(self, *args) -> Tuple:
@@ -315,17 +311,27 @@ class Sensor:
print("Warning: Call compute_deformation() first.")
mapping = {
self.info.VRAW: lambda: self.flow if self.flow is not None else np.array([]),
self.info.VNORMAL: lambda: self.vnormal if self.vnormal is not None else np.array([]),
self.info.VSHEAR: lambda: self.vshear if self.vshear is not None else np.array([]),
self.info.FNORMAL: lambda: self.fnormal,
self.info.FSHEARX: lambda: self.fshearx,
self.info.FSHEARY: lambda: self.fsheary,
self.info.FPS: lambda: self.fps,
# Return copies of arrays to prevent external modification of internal state
self.info.VRAW: lambda: self.__flow.copy() if self.__flow is not None else np.array([]),
self.info.VNORMAL: lambda: self.__vnormal.copy() if self.__vnormal is not None else np.array([]),
self.info.VSHEAR: lambda: self.__vshear.copy() if self.__vshear is not None else np.array([]),
# Scalar values are safe to return directly
self.info.FNORMAL: lambda: self.__fnormal,
self.info.FSHEARX: lambda: self.__fshearx,
self.info.FSHEARY: lambda: self.__fsheary,
self.info.FPS: lambda: self._fps,
self.info.TIMESTAMP: lambda: time.strftime('%H:%M:%S'),
self.info.DEPTH: lambda: self.depth_map if self.depth_map is not None else np.array([]),
self.info.CENTROID: lambda: self.centroid if self.centroid is not None else np.array([]),
self.info.CONTOUR: lambda: self.contour if self.contour is not None else np.array([]),
# Return copies of arrays to prevent external modification
self.info.DEPTH: lambda: self.__depth_map.copy() if self.__depth_map is not None else np.array([]),
# Centroid: can be tuple, list, or array - return a copy to prevent modification
# Return None if centroid is not found or empty
self.info.CENTROID: lambda: (
np.array(self.__centroid).copy()
if self.__centroid is not None and len(self.__centroid) > 0
else None
),
self.info.CONTOUR: lambda: self.__contour.copy() if self.__contour is not None else np.array([]),
self.info.IMG: lambda: self.__img.copy() if self.__img is not None else np.array([]),
}
return tuple(mapping.get(arg, lambda: None)() for arg in args if isinstance(arg, self.info))
@@ -365,8 +371,8 @@ class Sensor:
self._async_enabled = True
self._capture_thread = Thread(target=self._capture_worker, daemon=True)
self._flow_thread = Thread(target=self._flow_worker, daemon=True)
self._compute_thread = Thread(target=self._hhd_worker, daemon=True)
self._flow_thread = Thread(target=self.__flow_worker, daemon=True)
self._compute_thread = Thread(target=self.__hhd_worker, daemon=True)
self._contact_thread = Thread(target=self._contact_worker, daemon=True)
self._capture_thread.start()
@@ -419,7 +425,7 @@ class Sensor:
def export_config(self, path="./config/exported_config.json"):
"""Return current configuration dict."""
configs.export_config(self.config, path)
configs.export_config(self._config, path)
if self.verbose:
print(f"Config exported to {path}")
return True
@@ -437,11 +443,11 @@ class Sensor:
if os.path.exists(self.cal_path):
print("Loading calibration from", self.cal_path)
self.stitch_calibration = np.load(self.cal_path, allow_pickle=True)
self._stitcher.stitch_calibration = self.stitch_calibration
return self._stitcher.stitch_rt_optimized()
self.__stitcher.stitch_calibration = self.stitch_calibration
return self.__stitcher.stitch_rt_optimized()
else:
print("Creating new calibration...")
img, self.stitch_calibration = self._stitcher.stitch_cal()
img, self.stitch_calibration = self.__stitcher.stitch_cal()
os.makedirs("./config", exist_ok=True)
np.save(self.cal_path, self.stitch_calibration)
print("Calibration saved to", self.cal_path)
@@ -453,30 +459,29 @@ class Sensor:
def _load_config(self):
"""Load sensor configuration from JSON file."""
self.config = configs.load_config(self.config_name)
cam = self.config["camera"]
self.config_resolution_raw = cam["resolution_raw"]
self.config_fps = cam["fps"]
self.config_units = cam["units"]
self.config_resolution_stitch = cam["resolution_stitch"]
# self.config_index = cam["index"]
# self.config_pos_default = cam["pos_default"]
self._config = configs.load_config(self.config_name)
cam = self._config["camera"]
self._config_resolution_raw = cam["resolution_raw"]
self._config_fps = cam["fps"]
self._config_units = cam["units"]
self._config_resolution_stitch = cam["resolution_stitch"]
self.config_stitcher = self.config["stitcher"]
self.config_contact = self.config["contact"]
self.__config_stitcher = self._config["stitcher"]
self.__config_contact = self._config["contact"]
if self.verbose:
print(f"Config: {self.config_name}")
print(f" Resolution: {self.config_resolution_raw} → {self.config_resolution_stitch}")
print(f" FPS: {self.config_fps}, Units: {self.config_units}")
print(f" Resolution: {self._config_resolution_raw} → {self._config_resolution_stitch}")
print(f" FPS: {self._config_fps}, Units: {self._config_units}")
return True
def _open_video_source(self, override, box):
"""Initialize video capture and calibration."""
self.cap = cv2.VideoCapture(self.vid_src)
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
self.cap.set(cv2.CAP_PROP_FPS, self.config_fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.config_resolution_raw[0])
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config_resolution_raw[1])
self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
if self.verbose:
print("Connecting to video source...")
@@ -493,8 +498,8 @@ class Sensor:
if override:
if box is not None:
self._stitcher.isstitch = self.isstitch = False
self._stitcher.boxes = box
self.__stitcher.isstitch = self.isstitch = False
self.__stitcher.boxes = box
else:
print("Override=True but no box provided.")
else:
@@ -507,21 +512,21 @@ class Sensor:
n_boxes, v_mean0, timeout = 0, 0, 5
start_time = cv2.getTickCount()
while n_boxes < self.config_units:
while n_boxes < self._config_units:
ret, frame = self.cap.read()
v_mean = np.sum(cv2.mean(frame))
if abs(v_mean - v_mean0) < 5:
n_boxes = self._stitcher.find_stitch_boxes(frame, show=show)
n_boxes = self.__stitcher.find_stitch_boxes(frame, show=show)
if (cv2.getTickCount() - start_time) / cv2.getTickFrequency() > timeout:
print("Timeout finding vision units.")
self._stitcher.isstitch = self.isstitch = False
self.__stitcher.isstitch = self.isstitch = False
break
v_mean0 = v_mean
self.n_boxes = n_boxes
self.stitch_valid = self.isstitch and n_boxes == self.config_units
self.__n_boxes = n_boxes
self.stitch_valid = self.isstitch and n_boxes == self._config_units
status = "valid for stitching" if self.stitch_valid else "non-stitch mode"
print(f"Sensor: {status}")
@@ -529,8 +534,8 @@ class Sensor:
def _extract_tiles(self, img_src):
"""Extract tiles from source image."""
self._stitcher.get_stitch_unit(img_src)
self._stitcher.extract_tiles()
self.__stitcher.get_stitch_unit(img_src)
self.__stitcher.extract_tiles()
return True
# =========================================================================
@@ -554,15 +559,15 @@ class Sensor:
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self._stitcher.get_stitch_unit(gray)
self.__stitcher.get_stitch_unit(gray)
self._extract_tiles(gray)
img = np.ascontiguousarray(self._stitcher.stitch_rt_optimized())
img = cv2.resize(img, tuple(self.config_resolution_stitch), interpolation=cv2.INTER_CUBIC)
img = np.ascontiguousarray(self.__stitcher.stitch_rt_optimized())
img = cv2.resize(img, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC)
if self.img_filtered is None:
self.img_filtered = img.astype(np.float32)
if self.__img_filtered is None:
self.__img_filtered = img.astype(np.float32)
else:
self.img_filtered = alpha * img + (1 - alpha) * self.img_filtered
self.__img_filtered = alpha * img + (1 - alpha) * self.__img_filtered
self._queue_put(self._capture_queue, self.img_filtered.astype(np.uint8))
except Exception as e: