Merge branch 'remote'
This commit is contained in:
unknown
+197
-211
@@ -292,26 +292,6 @@ class Sensor:
|
||||
return result
|
||||
|
||||
|
||||
# def get_depth(self, denoise_sigma=15):
|
||||
# return getFilteredDensity(self.vnormal, use_cuda=self.cuda)
|
||||
# self.depth = getFilteredDensity(self.vnormal, use_cuda=self.cuda)
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
Convenience method: get_img + compute_deformation in one call.
|
||||
|
||||
Returns:
|
||||
dict: All sensor data
|
||||
"""
|
||||
self.get_img()
|
||||
self.compute_deformation()
|
||||
return {
|
||||
'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:
|
||||
"""
|
||||
Get sensor data by info type.
|
||||
@@ -374,88 +354,6 @@ class Sensor:
|
||||
print("Sensor disconnected.")
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Async Pipeline
|
||||
# =========================================================================
|
||||
|
||||
def enable_async(self, queue_size=2, compute_contact=False):
|
||||
"""
|
||||
Enable 3-stage async pipeline: Capture → Flow → HHD.
|
||||
|
||||
Each stage runs in parallel for maximum throughput.
|
||||
"""
|
||||
if self._async_enabled:
|
||||
return
|
||||
self._compute_contact_enabled = compute_contact
|
||||
|
||||
self._capture_queue = Queue(maxsize=queue_size)
|
||||
self._flow_queue = Queue(maxsize=queue_size)
|
||||
self._result_queue = Queue(maxsize=queue_size)
|
||||
if compute_contact:
|
||||
self._contact_queue = Queue(maxsize=queue_size)
|
||||
self._stop_event = Event()
|
||||
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)
|
||||
if compute_contact:
|
||||
self._contact_thread = Thread(target=self._contact_worker, daemon=True)
|
||||
|
||||
self._capture_thread.start()
|
||||
self._flow_thread.start()
|
||||
self._compute_thread.start()
|
||||
if compute_contact:
|
||||
self._contact_thread.start()
|
||||
|
||||
if self.verbose:
|
||||
if compute_contact:
|
||||
print("Async pipeline enabled: Capture → Flow → HHD → Contact")
|
||||
else:
|
||||
print("Async pipeline enabled: Capture → Flow → HHD")
|
||||
|
||||
def get_async(self, timeout=0.05):
|
||||
"""
|
||||
Get latest result from async pipeline (non-blocking).
|
||||
|
||||
Returns:
|
||||
dict or None: Sensor data if available
|
||||
"""
|
||||
compute_contact = self._compute_contact_enabled
|
||||
if not self._async_enabled:
|
||||
return None
|
||||
try:
|
||||
result = self._result_queue.get(timeout=timeout)
|
||||
t = time.perf_counter()
|
||||
result['fps'] = 1.0 / (t - self.last_log_time) if self.last_log_time else 0
|
||||
self.last_log_time = t
|
||||
|
||||
# Sync instance variables
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
if compute_contact:
|
||||
for k in ['img', 'flow', 'normal', 'shear', 'depth', 'centroid', 'centroid_found', 'contour', 'fps', 'fnormal', 'fshearx', 'fsheary', 'ftor']:
|
||||
setattr(self, k, result.get(k))
|
||||
else:
|
||||
for k in ['img', 'flow', 'normal', 'shear', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
setattr(self, k, result.get(k))
|
||||
self.vupdate = True
|
||||
return result
|
||||
except Empty:
|
||||
return None
|
||||
|
||||
def disable_async(self):
|
||||
"""Stop async pipeline."""
|
||||
if not self._async_enabled:
|
||||
return
|
||||
self._stop_event.set()
|
||||
for t in [self._capture_thread, self._flow_thread, self._compute_thread]:
|
||||
if t: t.join(timeout=2.0)
|
||||
self._async_enabled = False
|
||||
self._capture_thread = self._flow_thread = self._compute_thread = None
|
||||
self._capture_queue = self._flow_queue = self._result_queue = self._stop_event = None
|
||||
if self.verbose:
|
||||
print("Async pipeline disabled")
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Configuration & Calibration
|
||||
# =========================================================================
|
||||
@@ -478,16 +376,19 @@ class Sensor:
|
||||
self.cal_path = "./config/cal.npy"
|
||||
|
||||
if os.path.exists(self.cal_path):
|
||||
print("Loading calibration from", self.cal_path)
|
||||
if self.verbose:
|
||||
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()
|
||||
else:
|
||||
print("Creating new calibration...")
|
||||
if self.verbose:
|
||||
print("Creating new calibration...")
|
||||
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)
|
||||
if self.verbose:
|
||||
print("Calibration saved to", self.cal_path)
|
||||
return img
|
||||
|
||||
# =========================================================================
|
||||
@@ -508,8 +409,6 @@ class Sensor:
|
||||
|
||||
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}")
|
||||
return True
|
||||
|
||||
def _open_video_source(self, override, box):
|
||||
@@ -588,110 +487,6 @@ class Sensor:
|
||||
self.__stitcher.extract_tiles()
|
||||
return True
|
||||
|
||||
# =========================================================================
|
||||
# Private - Async Workers
|
||||
# =========================================================================
|
||||
|
||||
def _queue_put(self, q, item):
|
||||
"""Put item in queue, dropping old if full."""
|
||||
if q.full():
|
||||
try: q.get_nowait()
|
||||
except Empty: pass
|
||||
q.put(item)
|
||||
|
||||
def _capture_worker(self):
|
||||
"""Stage 1: Capture and preprocess frames by reusing get_img()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
# Reuse get_img() to avoid code duplication
|
||||
if self.get_img() is None:
|
||||
continue
|
||||
|
||||
# Put the processed image in the queue
|
||||
# get_img() updates self.__img, so we use that
|
||||
if self.__img is not None:
|
||||
self._queue_put(self._capture_queue, self.__img.copy())
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Capture error: {e}")
|
||||
|
||||
def _flow_worker(self):
|
||||
"""Stage 2: Compute optical flow by reusing __compute_flow()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
img = self._capture_queue.get(timeout=0.1)
|
||||
# Set __img to reuse __compute_flow()
|
||||
self.__img = img
|
||||
self.__compute_flow()
|
||||
flow = self.__flow.copy() if self.__flow is not None else None
|
||||
self._queue_put(self._flow_queue, (img, flow))
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Flow error: {e}")
|
||||
|
||||
def _hhd_worker(self):
|
||||
"""Stage 3: HHD decomposition by reusing __compute_hhd()."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
img, flow = self._flow_queue.get(timeout=0.1)
|
||||
|
||||
# Set __flow to reuse __compute_hhd()
|
||||
self.__flow = flow
|
||||
self.__compute_hhd()
|
||||
# Get results from internal variables set by __compute_hhd()
|
||||
result = {
|
||||
'img': img, 'flow': flow,
|
||||
'normal': self.__vnormal.copy() if self.__vnormal is not None else None,
|
||||
'shear': self.__vshear.copy() if self.__vshear is not None else None,
|
||||
# 'depth': getFilteredDensity(flow, use_cuda=self.cuda),
|
||||
'fnormal': self.__fnormal, 'fshearx': self.__fshearx,
|
||||
'fsheary': self.__fsheary, 'ftor': self.__ftor,
|
||||
}
|
||||
|
||||
# Always include contact fields to ensure consistent result structure
|
||||
# When compute_contact is True, contact worker will update these values
|
||||
# When compute_contact is False, they remain None
|
||||
result.update({
|
||||
'centroid': None,
|
||||
'centroid_found': False,
|
||||
'contour': None,
|
||||
'depth_map': None,
|
||||
'depth': None, # Alias for depth_map for compatibility
|
||||
})
|
||||
|
||||
self._queue_put(self._result_queue, result)
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"HHD error: {e}")
|
||||
|
||||
def _contact_worker(self):
|
||||
"""Stage 4: Contact computation using vnormal from HHD results."""
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
# Get results from HHD worker which includes vnormal
|
||||
hhd_result = self._result_queue.get(timeout=0.1)
|
||||
vnormal = hhd_result.get('normal') # This is vnormal from HHD
|
||||
|
||||
if vnormal is not None:
|
||||
# Compute contact using vnormal directly
|
||||
contact_result = self.compute_contact(vnormal=vnormal)
|
||||
# Add contact results to the existing HHD result
|
||||
depth_map = contact_result.get('divergence_map')
|
||||
hhd_result.update({
|
||||
'centroid': contact_result.get('centroid'),
|
||||
'centroid_found': contact_result.get('centroid_found'),
|
||||
'contour': contact_result.get('contour'),
|
||||
'depth_map': depth_map,
|
||||
'depth': depth_map, # Alias for depth_map for compatibility
|
||||
})
|
||||
|
||||
# Put updated result back to queue
|
||||
self._queue_put(self._result_queue, hhd_result)
|
||||
except Empty:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self.verbose: print(f"Contact error: {e}")
|
||||
|
||||
# =========================================================================
|
||||
# Private - Utilities
|
||||
@@ -705,3 +500,194 @@ class Sensor:
|
||||
print(f" {k}: {v:7.2f} ms")
|
||||
print(f" ─────────────────────────")
|
||||
print(f" TOTAL: {timings['total']:7.2f} ms ({1000/timings['total']:.1f} FPS)")
|
||||
|
||||
|
||||
|
||||
|
||||
# =========================================================================
|
||||
# Public API - Async Pipeline
|
||||
# =========================================================================
|
||||
|
||||
# def enable_async(self, queue_size=2, compute_contact=False):
|
||||
# """
|
||||
# Enable 3-stage async pipeline: Capture → Flow → HHD.
|
||||
|
||||
# Each stage runs in parallel for maximum throughput.
|
||||
# """
|
||||
# if self._async_enabled:
|
||||
# return
|
||||
# self._compute_contact_enabled = compute_contact
|
||||
|
||||
# self._capture_queue = Queue(maxsize=queue_size)
|
||||
# self._flow_queue = Queue(maxsize=queue_size)
|
||||
# self._result_queue = Queue(maxsize=queue_size)
|
||||
# if compute_contact:
|
||||
# self._contact_queue = Queue(maxsize=queue_size)
|
||||
# self._stop_event = Event()
|
||||
# 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)
|
||||
# if compute_contact:
|
||||
# self._contact_thread = Thread(target=self._contact_worker, daemon=True)
|
||||
|
||||
# self._capture_thread.start()
|
||||
# self._flow_thread.start()
|
||||
# self._compute_thread.start()
|
||||
# if compute_contact:
|
||||
# self._contact_thread.start()
|
||||
|
||||
# if self.verbose:
|
||||
# if compute_contact:
|
||||
# print("Async pipeline enabled: Capture → Flow → HHD → Contact")
|
||||
# else:
|
||||
# print("Async pipeline enabled: Capture → Flow → HHD")
|
||||
|
||||
# def get_async(self, timeout=0.05):
|
||||
# """
|
||||
# Get latest result from async pipeline (non-blocking).
|
||||
|
||||
# Returns:
|
||||
# dict or None: Sensor data if available
|
||||
# """
|
||||
# compute_contact = self._compute_contact_enabled
|
||||
# if not self._async_enabled:
|
||||
# return None
|
||||
# try:
|
||||
# result = self._result_queue.get(timeout=timeout)
|
||||
# t = time.perf_counter()
|
||||
# result['fps'] = 1.0 / (t - self.last_log_time) if self.last_log_time else 0
|
||||
# self.last_log_time = t
|
||||
|
||||
# # Sync instance variables
|
||||
# # for k in ['img', 'flow', 'normal', 'shear', 'depth', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
# if compute_contact:
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'depth', 'centroid', 'centroid_found', 'contour', 'fps', 'fnormal', 'fshearx', 'fsheary', 'ftor']:
|
||||
# setattr(self, k, result.get(k))
|
||||
# else:
|
||||
# for k in ['img', 'flow', 'normal', 'shear', 'fnormal', 'fshearx', 'fsheary', 'ftor', 'fps']:
|
||||
# setattr(self, k, result.get(k))
|
||||
# self.vupdate = True
|
||||
# return result
|
||||
# except Empty:
|
||||
# return None
|
||||
|
||||
# def disable_async(self):
|
||||
# """Stop async pipeline."""
|
||||
# if not self._async_enabled:
|
||||
# return
|
||||
# self._stop_event.set()
|
||||
# for t in [self._capture_thread, self._flow_thread, self._compute_thread]:
|
||||
# if t: t.join(timeout=2.0)
|
||||
# self._async_enabled = False
|
||||
# self._capture_thread = self._flow_thread = self._compute_thread = None
|
||||
# self._capture_queue = self._flow_queue = self._result_queue = self._stop_event = None
|
||||
# if self.verbose:
|
||||
# print("Async pipeline disabled")
|
||||
|
||||
# =========================================================================
|
||||
# Private - Async Workers
|
||||
# =========================================================================
|
||||
|
||||
# def _queue_put(self, q, item):
|
||||
# """Put item in queue, dropping old if full."""
|
||||
# if q.full():
|
||||
# try: q.get_nowait()
|
||||
# except Empty: pass
|
||||
# q.put(item)
|
||||
|
||||
# def _capture_worker(self):
|
||||
# """Stage 1: Capture and preprocess frames by reusing get_img()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# # Reuse get_img() to avoid code duplication
|
||||
# if self.get_img() is None:
|
||||
# continue
|
||||
|
||||
# # Put the processed image in the queue
|
||||
# # get_img() updates self.__img, so we use that
|
||||
# if self.__img is not None:
|
||||
# self._queue_put(self._capture_queue, self.__img.copy())
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Capture error: {e}")
|
||||
|
||||
# def _flow_worker(self):
|
||||
# """Stage 2: Compute optical flow by reusing __compute_flow()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# img = self._capture_queue.get(timeout=0.1)
|
||||
# # Set __img to reuse __compute_flow()
|
||||
# self.__img = img
|
||||
# self.__compute_flow()
|
||||
# flow = self.__flow.copy() if self.__flow is not None else None
|
||||
# self._queue_put(self._flow_queue, (img, flow))
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Flow error: {e}")
|
||||
|
||||
# def _hhd_worker(self):
|
||||
# """Stage 3: HHD decomposition by reusing __compute_hhd()."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# img, flow = self._flow_queue.get(timeout=0.1)
|
||||
|
||||
# # Set __flow to reuse __compute_hhd()
|
||||
# self.__flow = flow
|
||||
# self.__compute_hhd()
|
||||
# # Get results from internal variables set by __compute_hhd()
|
||||
# result = {
|
||||
# 'img': img, 'flow': flow,
|
||||
# 'normal': self.__vnormal.copy() if self.__vnormal is not None else None,
|
||||
# 'shear': self.__vshear.copy() if self.__vshear is not None else None,
|
||||
# # 'depth': getFilteredDensity(flow, use_cuda=self.cuda),
|
||||
# 'fnormal': self.__fnormal, 'fshearx': self.__fshearx,
|
||||
# 'fsheary': self.__fsheary, 'ftor': self.__ftor,
|
||||
# }
|
||||
|
||||
# # Always include contact fields to ensure consistent result structure
|
||||
# # When compute_contact is True, contact worker will update these values
|
||||
# # When compute_contact is False, they remain None
|
||||
# result.update({
|
||||
# 'centroid': None,
|
||||
# 'centroid_found': False,
|
||||
# 'contour': None,
|
||||
# 'depth_map': None,
|
||||
# 'depth': None, # Alias for depth_map for compatibility
|
||||
# })
|
||||
|
||||
# self._queue_put(self._result_queue, result)
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"HHD error: {e}")
|
||||
|
||||
# def _contact_worker(self):
|
||||
# """Stage 4: Contact computation using vnormal from HHD results."""
|
||||
# while not self._stop_event.is_set():
|
||||
# try:
|
||||
# # Get results from HHD worker which includes vnormal
|
||||
# hhd_result = self._result_queue.get(timeout=0.1)
|
||||
# vnormal = hhd_result.get('normal') # This is vnormal from HHD
|
||||
|
||||
# if vnormal is not None:
|
||||
# # Compute contact using vnormal directly
|
||||
# contact_result = self.compute_contact(vnormal=vnormal)
|
||||
# # Add contact results to the existing HHD result
|
||||
# depth_map = contact_result.get('divergence_map')
|
||||
# hhd_result.update({
|
||||
# 'centroid': contact_result.get('centroid'),
|
||||
# 'centroid_found': contact_result.get('centroid_found'),
|
||||
# 'contour': contact_result.get('contour'),
|
||||
# 'depth_map': depth_map,
|
||||
# 'depth': depth_map, # Alias for depth_map for compatibility
|
||||
# })
|
||||
|
||||
# # Put updated result back to queue
|
||||
# self._queue_put(self._result_queue, hhd_result)
|
||||
# except Empty:
|
||||
# continue
|
||||
# except Exception as e:
|
||||
# if self.verbose: print(f"Contact error: {e}")
|
||||
|
||||
|
||||
Reference in New Issue
Block a user