added dd06 test support, organized 2d viewer and 3d viewer code organization.
This commit is contained in:
ykcheungab
@@ -0,0 +1,23 @@
|
||||
"""Sensor acquisition adapters for Qt / multi-sensor viewers (no Qt widgets)."""
|
||||
|
||||
from .adapter import (
|
||||
Dd06RoiAdapter,
|
||||
FrameSnapshot,
|
||||
OrisysSensorAdapter,
|
||||
SensorAdapter,
|
||||
calibrate_normal_force,
|
||||
create_sensor_adapter,
|
||||
_snapshot_from_sensor,
|
||||
)
|
||||
from . import roi_geometry
|
||||
|
||||
__all__ = [
|
||||
"Dd06RoiAdapter",
|
||||
"FrameSnapshot",
|
||||
"OrisysSensorAdapter",
|
||||
"SensorAdapter",
|
||||
"calibrate_normal_force",
|
||||
"create_sensor_adapter",
|
||||
"roi_geometry",
|
||||
"_snapshot_from_sensor",
|
||||
]
|
||||
@@ -0,0 +1,330 @@
|
||||
"""Sensor adapters that normalize frames for Qt / multi-sensor viewers."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Optional
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import orisys
|
||||
|
||||
from .roi_geometry import (
|
||||
build_roi_warp,
|
||||
load_json_config,
|
||||
load_roi_polygon,
|
||||
open_video_source,
|
||||
rectified_output_size,
|
||||
warp_roi_to_rectangle,
|
||||
)
|
||||
|
||||
|
||||
NORMAL_FORCE_CAL_SLOPE = 3.92685554668795e-05
|
||||
NORMAL_FORCE_CAL_OFFSET = 0.0217596967278535
|
||||
|
||||
|
||||
@dataclass
|
||||
class FrameSnapshot:
|
||||
"""Normalized per-frame payload consumed by the Qt viewer."""
|
||||
|
||||
image: Optional[np.ndarray] = None
|
||||
flow: Optional[np.ndarray] = None
|
||||
depth: Optional[np.ndarray] = None
|
||||
contour: Optional[np.ndarray] = None
|
||||
centroid: Any = None
|
||||
# Raw SDK force channels used for curves (same units for FN/FX/FY).
|
||||
normal_force: float = 0.0
|
||||
shear_x: float = 0.0
|
||||
shear_y: float = 0.0
|
||||
# Optional calibrated FN in Newtons for export/display overlays.
|
||||
normal_force_calibrated: float = 0.0
|
||||
fps: float = 0.0
|
||||
timestamp: float = 0.0
|
||||
raw_image: Optional[np.ndarray] = None
|
||||
processed_image: Optional[np.ndarray] = None
|
||||
flow_overlay: Optional[np.ndarray] = None
|
||||
record_frame: Optional[np.ndarray] = None
|
||||
|
||||
|
||||
def calibrate_normal_force(raw_value: float) -> float:
|
||||
"""Convert raw normal-force reading to Newtons using y = mx + c."""
|
||||
return NORMAL_FORCE_CAL_SLOPE * float(raw_value) + NORMAL_FORCE_CAL_OFFSET
|
||||
|
||||
|
||||
class SensorAdapter(ABC):
|
||||
"""Common interface between Qt and concrete sensor backends."""
|
||||
|
||||
@abstractmethod
|
||||
def start(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def read(self) -> Optional[FrameSnapshot]:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def reset(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def close(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
def _bgr_from_gray_or_bgr(img: np.ndarray) -> np.ndarray:
|
||||
if img is None:
|
||||
return None
|
||||
if img.ndim == 2:
|
||||
return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
if img.ndim == 3 and img.shape[2] == 1:
|
||||
return cv2.cvtColor(img[:, :, 0], cv2.COLOR_GRAY2BGR)
|
||||
return img
|
||||
|
||||
|
||||
def _snapshot_from_sensor(
|
||||
sensor,
|
||||
*,
|
||||
raw_image=None,
|
||||
processed_image=None,
|
||||
) -> Optional[FrameSnapshot]:
|
||||
"""Build a FrameSnapshot with HHD/forces refreshed every frame."""
|
||||
if sensor is None or sensor.img is None:
|
||||
return None
|
||||
|
||||
# Always decompose so FN/FX/FY update every frame (motion gating freezes forces).
|
||||
sensor.compute_deformation(decompose=True, check_motion=False)
|
||||
sensor.compute_contact()
|
||||
|
||||
fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = (
|
||||
sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.VRAW,
|
||||
sensor.info.VNORMAL,
|
||||
sensor.info.IMG,
|
||||
sensor.info.CONTOUR,
|
||||
sensor.info.CENTROID,
|
||||
sensor.info.DEPTH,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
)
|
||||
|
||||
image_bgr = _bgr_from_gray_or_bgr(img)
|
||||
processed = processed_image if processed_image is not None else image_bgr
|
||||
record = None
|
||||
if getattr(sensor, "frame", None) is not None:
|
||||
record = _bgr_from_gray_or_bgr(sensor.frame)
|
||||
elif processed is not None:
|
||||
record = processed
|
||||
|
||||
flow_overlay = None
|
||||
if image_bgr is not None and flow is not None:
|
||||
flow_overlay = orisys.util.draw_arrows(
|
||||
image_bgr,
|
||||
flow,
|
||||
threshold=2,
|
||||
grid_spacing=20,
|
||||
arrow_scale=0.5,
|
||||
below_threshold_color=(160, 160, 160),
|
||||
)
|
||||
|
||||
fn_raw = float(fnormal)
|
||||
return FrameSnapshot(
|
||||
image=image_bgr,
|
||||
flow=flow,
|
||||
depth=depth_map,
|
||||
contour=contour,
|
||||
centroid=centroid,
|
||||
normal_force=fn_raw,
|
||||
shear_x=float(fshearx),
|
||||
shear_y=float(fsheary),
|
||||
normal_force_calibrated=calibrate_normal_force(fn_raw),
|
||||
fps=float(fps),
|
||||
timestamp=time.time(),
|
||||
raw_image=raw_image,
|
||||
processed_image=processed,
|
||||
flow_overlay=flow_overlay,
|
||||
record_frame=record,
|
||||
)
|
||||
|
||||
|
||||
class OrisysSensorAdapter(SensorAdapter):
|
||||
"""Direct orisys.Sensor capture path (compound-eye / single-camera)."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
video,
|
||||
config_name,
|
||||
cal_path=None,
|
||||
verbose=True,
|
||||
*,
|
||||
rotate_ccw90=False,
|
||||
motion_threshold=1,
|
||||
):
|
||||
self.video = video
|
||||
self.config_name = config_name
|
||||
self.cal_path = cal_path
|
||||
self.verbose = verbose
|
||||
self.rotate_ccw90 = bool(rotate_ccw90)
|
||||
self.motion_threshold = motion_threshold
|
||||
self.sensor = None
|
||||
|
||||
def start(self) -> None:
|
||||
src = self.video
|
||||
try:
|
||||
src = int(str(src).strip())
|
||||
except (TypeError, ValueError):
|
||||
pass
|
||||
self.sensor = orisys.Sensor(
|
||||
src,
|
||||
config_name=self.config_name,
|
||||
cal_path=self.cal_path,
|
||||
verbose=self.verbose,
|
||||
)
|
||||
|
||||
def read(self) -> Optional[FrameSnapshot]:
|
||||
if self.sensor is None:
|
||||
return None
|
||||
img = self.sensor.get_img()
|
||||
if img is None:
|
||||
return None
|
||||
|
||||
if self.rotate_ccw90:
|
||||
self.sensor.img = cv2.rotate(self.sensor.img, cv2.ROTATE_90_COUNTERCLOCKWISE)
|
||||
if self.sensor.frame is not None:
|
||||
self.sensor.frame = cv2.rotate(
|
||||
self.sensor.frame, cv2.ROTATE_90_COUNTERCLOCKWISE
|
||||
)
|
||||
|
||||
return _snapshot_from_sensor(
|
||||
self.sensor,
|
||||
raw_image=_bgr_from_gray_or_bgr(self.sensor.frame)
|
||||
if self.sensor.frame is not None
|
||||
else None,
|
||||
processed_image=_bgr_from_gray_or_bgr(self.sensor.img),
|
||||
)
|
||||
|
||||
def reset(self) -> None:
|
||||
if self.sensor is not None:
|
||||
self.sensor.reset()
|
||||
|
||||
def close(self) -> None:
|
||||
if self.sensor is not None:
|
||||
self.sensor.disconnect()
|
||||
self.sensor = None
|
||||
|
||||
|
||||
class Dd06RoiAdapter(SensorAdapter):
|
||||
"""DD06 path: external capture + polygon ROI rectify + already_processed Sensor."""
|
||||
|
||||
def __init__(self, video, config_name, verbose=True, *, motion_threshold=0):
|
||||
self.video = video
|
||||
self.config_name = config_name
|
||||
self.verbose = verbose
|
||||
self.motion_threshold = motion_threshold
|
||||
self.sensor = None
|
||||
self.cap = None
|
||||
self.roi_points = None
|
||||
self.roi_image_size = None
|
||||
self.out_size = None
|
||||
self.warp_matrix = None
|
||||
self.warp_src = None
|
||||
self.last_frame_size = None
|
||||
self._source = None
|
||||
|
||||
def start(self) -> None:
|
||||
config = load_json_config(self.config_name)
|
||||
self.roi_points, self.roi_image_size = load_roi_polygon(config)
|
||||
self.out_size = rectified_output_size(config, self.roi_points)
|
||||
self.sensor = orisys.Sensor(
|
||||
"external",
|
||||
config_name=self.config_name,
|
||||
verbose=self.verbose,
|
||||
)
|
||||
self.cap, self._source = open_video_source(self.video)
|
||||
self.warp_matrix = None
|
||||
self.last_frame_size = None
|
||||
if self.verbose:
|
||||
print(
|
||||
f"DD06 adapter: ROI points={len(self.roi_points)} "
|
||||
f"source_size={self.roi_image_size} output={self.out_size} "
|
||||
f"video={self._source}"
|
||||
)
|
||||
|
||||
def read(self) -> Optional[FrameSnapshot]:
|
||||
if self.sensor is None or self.cap is None:
|
||||
return None
|
||||
|
||||
ok, frame = self.cap.read()
|
||||
if not ok or frame is None:
|
||||
return None
|
||||
|
||||
frame_h, frame_w = frame.shape[:2]
|
||||
frame_size = (frame_w, frame_h)
|
||||
if self.warp_matrix is None or self.last_frame_size != frame_size:
|
||||
self.warp_matrix, self.warp_src, _ = build_roi_warp(
|
||||
self.roi_points,
|
||||
self.roi_image_size,
|
||||
frame_size,
|
||||
self.out_size,
|
||||
)
|
||||
self.last_frame_size = frame_size
|
||||
if self.verbose:
|
||||
print(f"Built ROI warp for frame {frame_w}x{frame_h}")
|
||||
|
||||
rectified = warp_roi_to_rectangle(frame, self.warp_matrix, self.out_size)
|
||||
self.sensor.get_img(frame=rectified, already_processed=True)
|
||||
return _snapshot_from_sensor(
|
||||
self.sensor,
|
||||
raw_image=frame,
|
||||
processed_image=rectified,
|
||||
)
|
||||
|
||||
def reset(self) -> None:
|
||||
if self.sensor is not None:
|
||||
self.sensor.reset()
|
||||
|
||||
def close(self) -> None:
|
||||
if self.cap is not None:
|
||||
self.cap.release()
|
||||
self.cap = None
|
||||
if self.sensor is not None:
|
||||
self.sensor.disconnect()
|
||||
self.sensor = None
|
||||
|
||||
|
||||
def create_sensor_adapter(
|
||||
sensor_type: str,
|
||||
*,
|
||||
video,
|
||||
config_name,
|
||||
cal_path=None,
|
||||
verbose=True,
|
||||
) -> SensorAdapter:
|
||||
"""Factory for Qt / demo viewers."""
|
||||
kind = str(sensor_type or "orisys").strip().lower()
|
||||
if kind in ("orisys", "direct", "default", "compound_eye", "single_camera"):
|
||||
# Preserve historical Wuxi viewer rotation for compound-eye demos.
|
||||
rotate = kind in ("orisys", "direct", "default", "compound_eye")
|
||||
return OrisysSensorAdapter(
|
||||
video,
|
||||
config_name,
|
||||
cal_path=cal_path,
|
||||
verbose=verbose,
|
||||
rotate_ccw90=rotate,
|
||||
motion_threshold=1,
|
||||
)
|
||||
if kind in ("dd06", "dd06-ov", "roi", "polygon_roi"):
|
||||
return Dd06RoiAdapter(
|
||||
video,
|
||||
config_name,
|
||||
verbose=verbose,
|
||||
motion_threshold=0,
|
||||
)
|
||||
raise ValueError(
|
||||
f"Unknown sensor_type={sensor_type!r}; expected 'orisys' or 'dd06'"
|
||||
)
|
||||
@@ -0,0 +1,140 @@
|
||||
"""Polygon ROI geometry helpers for rectifying tactile sensor frames."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def load_json_config(path):
|
||||
path = Path(path)
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
|
||||
|
||||
def load_roi_polygon(config):
|
||||
"""Return (points Nx2 float32, image_size [w,h]) from nested or legacy ROI fields."""
|
||||
roi = config.get("roi") if isinstance(config.get("roi"), dict) else config
|
||||
if not isinstance(roi, dict):
|
||||
raise ValueError("Config has no usable ROI section")
|
||||
|
||||
image_size = roi.get("image_size") or config.get("camera", {}).get("resolution_raw")
|
||||
if image_size is None:
|
||||
raise ValueError("ROI image_size is missing")
|
||||
|
||||
points = roi.get("points") or roi.get("polygon") or roi.get("roi_polygon")
|
||||
if points is None and roi.get("xyxy") is not None:
|
||||
x1, y1, x2, y2 = [int(v) for v in roi["xyxy"]]
|
||||
points = [[x1, y1], [x2, y1], [x2, y2], [x1, y2]]
|
||||
if not points:
|
||||
raise ValueError("ROI points are missing")
|
||||
|
||||
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2)
|
||||
if pts.shape[0] < 4:
|
||||
raise ValueError(f"ROI needs at least 4 points, got {pts.shape[0]}")
|
||||
return pts, [int(image_size[0]), int(image_size[1])]
|
||||
|
||||
|
||||
def scale_points(points, source_size, target_size):
|
||||
source_w, source_h = float(source_size[0]), float(source_size[1])
|
||||
target_w, target_h = float(target_size[0]), float(target_size[1])
|
||||
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2).copy()
|
||||
if source_w > 0 and source_h > 0 and (source_w, source_h) != (target_w, target_h):
|
||||
pts[:, 0] *= target_w / source_w
|
||||
pts[:, 1] *= target_h / source_h
|
||||
return pts
|
||||
|
||||
|
||||
def order_quad_points(points):
|
||||
"""Order 4 points as TL, TR, BR, BL for perspective transforms."""
|
||||
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2)
|
||||
if pts.shape[0] != 4:
|
||||
hull = cv2.convexHull(pts.astype(np.float32))
|
||||
rect = cv2.minAreaRect(hull)
|
||||
pts = cv2.boxPoints(rect).astype(np.float32)
|
||||
|
||||
s = pts.sum(axis=1)
|
||||
diff = np.diff(pts, axis=1).reshape(-1)
|
||||
ordered = np.zeros((4, 2), dtype=np.float32)
|
||||
ordered[0] = pts[np.argmin(s)] # top-left
|
||||
ordered[2] = pts[np.argmax(s)] # bottom-right
|
||||
ordered[1] = pts[np.argmin(diff)] # top-right
|
||||
ordered[3] = pts[np.argmax(diff)] # bottom-left
|
||||
return ordered
|
||||
|
||||
|
||||
def rectified_output_size(config, points=None):
|
||||
"""Prefer camera.resolution_stitch; fall back to ROI bounding box size."""
|
||||
stitch = config.get("camera", {}).get("resolution_stitch")
|
||||
if stitch is not None and len(stitch) >= 2:
|
||||
return int(stitch[0]), int(stitch[1])
|
||||
if points is not None:
|
||||
pts = np.asarray(points, dtype=np.float32).reshape(-1, 2)
|
||||
width = max(
|
||||
int(np.linalg.norm(pts[1] - pts[0])),
|
||||
int(np.linalg.norm(pts[2] - pts[3])),
|
||||
1,
|
||||
)
|
||||
height = max(
|
||||
int(np.linalg.norm(pts[3] - pts[0])),
|
||||
int(np.linalg.norm(pts[2] - pts[1])),
|
||||
1,
|
||||
)
|
||||
return width, height
|
||||
raise ValueError("Cannot determine rectified output size")
|
||||
|
||||
|
||||
def build_roi_warp(points, source_image_size, frame_size, output_size):
|
||||
"""
|
||||
Build a perspective transform that maps the ROI polygon in frame coords
|
||||
onto a rectangle of ``output_size`` (width, height).
|
||||
"""
|
||||
frame_w, frame_h = int(frame_size[0]), int(frame_size[1])
|
||||
scaled = scale_points(points, source_image_size, (frame_w, frame_h))
|
||||
src = order_quad_points(scaled)
|
||||
out_w, out_h = int(output_size[0]), int(output_size[1])
|
||||
dst = np.array(
|
||||
[
|
||||
[0.0, 0.0],
|
||||
[out_w - 1.0, 0.0],
|
||||
[out_w - 1.0, out_h - 1.0],
|
||||
[0.0, out_h - 1.0],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
matrix = cv2.getPerspectiveTransform(src, dst)
|
||||
return matrix, src, (out_w, out_h)
|
||||
|
||||
|
||||
def warp_roi_to_rectangle(frame, matrix, output_size):
|
||||
out_w, out_h = int(output_size[0]), int(output_size[1])
|
||||
return cv2.warpPerspective(
|
||||
frame,
|
||||
matrix,
|
||||
(out_w, out_h),
|
||||
flags=cv2.INTER_LINEAR,
|
||||
borderMode=cv2.BORDER_REPLICATE,
|
||||
)
|
||||
|
||||
|
||||
def open_video_source(video):
|
||||
"""Open a camera index or video file. Returns (cap, resolved_source)."""
|
||||
text = str(video).strip()
|
||||
if text.isdigit():
|
||||
source = int(text)
|
||||
cap = cv2.VideoCapture(source, cv2.CAP_DSHOW)
|
||||
if not cap.isOpened():
|
||||
cap.release()
|
||||
cap = cv2.VideoCapture(source)
|
||||
else:
|
||||
path = Path(text)
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"Video not found: {path}")
|
||||
source = str(path)
|
||||
cap = cv2.VideoCapture(source)
|
||||
if not cap.isOpened():
|
||||
raise RuntimeError(f"Failed to open video source: {video}")
|
||||
return cap, source
|
||||
Reference in New Issue
Block a user