OliviaCandClaude Opus 5.5
f58b81cb73
Already there: fx/fy/cx/cy and k1,k2,k3,p1,p2 of both streams, extrinsics depth->color (rotation row-major, translation mm). Added, as new keys only: width, height and distortion model name (an rs.distortion member) in "ir" and "rgb", and depth_scale_m at the top. Docstring states the extrinsics convention: p_rgb = R p_ir + t, R row-major (the SDK's is column-major). Same keys in /api/status and /api/open, which share the payload. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
778 lines
26 KiB
Python
778 lines
26 KiB
Python
"""REST API routes for the RealSense D435 camera server."""
|
|
|
|
import base64
|
|
import logging
|
|
import threading
|
|
from collections import deque
|
|
|
|
import cv2
|
|
import numpy as np
|
|
from fastapi import APIRouter, Depends, HTTPException, Query
|
|
from fastapi.responses import Response, JSONResponse
|
|
|
|
from src.backend.api.dependencies import verify_api_key
|
|
from src.backend.models.api import ok, fail
|
|
from src.backend.services.camera import RealSenseD435, CameraFrame, ColorFrame, RawFrame
|
|
from src.backend.services.image_saver import ImageSaver
|
|
from src.backend.services.phase_timing import timer
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
router = APIRouter(dependencies=[Depends(verify_api_key)])
|
|
|
|
_camera: RealSenseD435 | None = None
|
|
_saver: ImageSaver | None = None
|
|
_frame_count = 0
|
|
_frame_count_lock = threading.Lock() # frame_id is incremented from worker threads
|
|
|
|
|
|
def set_camera(camera: RealSenseD435) -> None:
|
|
global _camera
|
|
_camera = camera
|
|
|
|
|
|
def set_image_saver(saver: ImageSaver) -> None:
|
|
global _saver
|
|
_saver = saver
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Image encoding helpers
|
|
# ---------------------------------------------------------------------------
|
|
|
|
def _encode_rgb_jpeg(rgb: np.ndarray, quality: int = 85) -> bytes:
|
|
with timer.phase(f"jpeg_q{quality}"):
|
|
_, buf = cv2.imencode(".jpg", rgb, [cv2.IMWRITE_JPEG_QUALITY, quality])
|
|
return buf.tobytes()
|
|
|
|
|
|
def _encode_depth_png(depth: np.ndarray) -> bytes:
|
|
with timer.phase("png_depth"):
|
|
_, buf = cv2.imencode(".png", depth)
|
|
return buf.tobytes()
|
|
|
|
|
|
def _depth_colormap(depth: np.ndarray) -> np.ndarray:
|
|
with timer.phase("colormap"):
|
|
return _jet_colormap(depth)
|
|
|
|
|
|
def _jet_colormap(depth: np.ndarray) -> np.ndarray:
|
|
valid = depth[depth > 0]
|
|
if len(valid) == 0:
|
|
return np.zeros((*depth.shape, 3), dtype=np.uint8)
|
|
p5, p95 = np.percentile(valid, [5, 95])
|
|
vis = np.zeros_like(depth, dtype=np.uint8)
|
|
mask = depth > 0
|
|
vis[mask] = np.clip(
|
|
(depth[mask].astype(float) - p5) / max(p95 - p5, 1) * 255, 0, 255
|
|
).astype(np.uint8)
|
|
return cv2.applyColorMap(vis, cv2.COLORMAP_JET)
|
|
|
|
|
|
def _depth_stats(depth: np.ndarray) -> dict:
|
|
with timer.phase("depth_stats"):
|
|
valid = depth > 0
|
|
return {
|
|
"depth_min_mm": int(depth[valid].min()) if valid.any() else 0,
|
|
"depth_max_mm": int(depth.max()),
|
|
"depth_valid_pixels": int(valid.sum()),
|
|
}
|
|
|
|
|
|
def _b64(data: bytes) -> str:
|
|
with timer.phase("base64"):
|
|
return base64.b64encode(data).decode()
|
|
|
|
|
|
class EncodedFrame:
|
|
"""The encodings of ONE frame, each computed once, on first use, and shared.
|
|
|
|
Clients served the same frame (parallel requests, the WebSocket, NATS)
|
|
get the same bytes instead of encoding it again each.
|
|
"""
|
|
|
|
def __init__(self, frame: CameraFrame):
|
|
self.frame = frame
|
|
self._lock = threading.RLock() # reentrant: images_b64() computes the other fields inside
|
|
self._values: dict[str, object] = {}
|
|
|
|
def _once(self, key: str, compute):
|
|
with self._lock: # a second caller waits for the first encoding instead of repeating it
|
|
if key not in self._values:
|
|
self._values[key] = compute()
|
|
return self._values[key]
|
|
|
|
def rgb_jpeg(self) -> bytes:
|
|
return self._once("rgb_jpeg", lambda: _encode_rgb_jpeg(self.frame.rgb))
|
|
|
|
def depth_png(self) -> bytes:
|
|
return self._once("depth_png", lambda: _encode_depth_png(self.frame.depth))
|
|
|
|
def depth_color_jpeg(self) -> bytes:
|
|
return self._once("depth_color_jpeg", lambda: _encode_rgb_jpeg(_depth_colormap(self.frame.depth), 90))
|
|
|
|
def depth_stats(self) -> dict:
|
|
return self._once("depth_stats", lambda: _depth_stats(self.frame.depth))
|
|
|
|
def images_b64(self) -> dict:
|
|
return self._once("images_b64", lambda: {
|
|
"rgb_jpeg_b64": _b64(self.rgb_jpeg()),
|
|
"depth_png_b64": _b64(self.depth_png()),
|
|
"depth_color_b64": _b64(self.depth_color_jpeg()),
|
|
})
|
|
|
|
|
|
ENCODED_FRAMES_KEPT = 4 # recent frames whose encodings are kept (REST and WS may be one frame apart)
|
|
_encoded_frames: deque = deque(maxlen=ENCODED_FRAMES_KEPT)
|
|
_encoded_frames_lock = threading.Lock()
|
|
|
|
|
|
def encoded(frame: CameraFrame) -> EncodedFrame:
|
|
"""The shared EncodedFrame of this frame (same object for everyone asking)."""
|
|
with _encoded_frames_lock:
|
|
for enc in _encoded_frames:
|
|
if enc.frame is frame:
|
|
return enc
|
|
enc = EncodedFrame(frame)
|
|
_encoded_frames.append(enc)
|
|
return enc
|
|
|
|
|
|
COLOR_JPEG_QUALITY = 85 # same as /api/rgb
|
|
COLOR_WAIT_MAX_MS = 5000 # longest wait a client may ask for on the color channel
|
|
ENCODED_COLORS_KEPT = 4 # recent color frames whose JPEG is kept (own cache: 30 fps would flush the one above)
|
|
|
|
|
|
class EncodedColor:
|
|
"""JPEG of one frame of the color channel, encoded once for all its clients."""
|
|
|
|
def __init__(self, seq: int, color: ColorFrame):
|
|
self.seq = seq
|
|
self.color = color
|
|
self._lock = threading.Lock()
|
|
self._jpeg: bytes | None = None
|
|
|
|
def jpeg(self) -> bytes:
|
|
with self._lock:
|
|
if self._jpeg is None:
|
|
with timer.phase("jpeg_color"):
|
|
_, buf = cv2.imencode(".jpg", self.color.rgb, [cv2.IMWRITE_JPEG_QUALITY, COLOR_JPEG_QUALITY])
|
|
self._jpeg = buf.tobytes()
|
|
return self._jpeg
|
|
|
|
|
|
_encoded_colors: deque = deque(maxlen=ENCODED_COLORS_KEPT)
|
|
_encoded_colors_lock = threading.Lock()
|
|
|
|
|
|
def encoded_color(seq: int, color: ColorFrame) -> EncodedColor:
|
|
"""The shared EncodedColor of color frame number `seq`."""
|
|
with _encoded_colors_lock:
|
|
for enc in _encoded_colors:
|
|
if enc.seq == seq and enc.color is color:
|
|
return enc
|
|
enc = EncodedColor(seq, color)
|
|
_encoded_colors.append(enc)
|
|
return enc
|
|
|
|
|
|
def color_headers(seq: int, color: ColorFrame) -> dict:
|
|
"""Frame number and both clocks of a color frame, as response headers."""
|
|
return {
|
|
"X-Frame-Seq": str(seq),
|
|
"X-Camera-Timestamp-Ms": f"{color.camera_timestamp_ms:.3f}",
|
|
"X-Camera-Timestamp-Domain": color.timestamp_domain,
|
|
"X-Host-Timestamp": f"{color.host_timestamp:.6f}",
|
|
}
|
|
|
|
|
|
ENCODED_RAWS_KEPT = 4 # recent framesets whose raw depth PNG is kept
|
|
|
|
|
|
class EncodedRaw:
|
|
"""/api/raw/latest payload of one frameset, built once for all its clients.
|
|
|
|
The color JPEG is the one of the color channel (same frameset, same bytes).
|
|
"""
|
|
|
|
def __init__(self, seq: int, raw: RawFrame):
|
|
self.seq = seq
|
|
self.raw = raw
|
|
self._lock = threading.Lock()
|
|
self._payload: dict | None = None
|
|
|
|
def payload(self) -> dict:
|
|
with self._lock:
|
|
if self._payload is None:
|
|
with timer.phase("png_raw_depth"):
|
|
_, buf = cv2.imencode(".png", self.raw.depth)
|
|
color = self.raw.color
|
|
self._payload = {
|
|
"frameset_seq": self.seq,
|
|
"camera_timestamp_ms": color.camera_timestamp_ms,
|
|
"timestamp_domain": color.timestamp_domain,
|
|
"host_timestamp": color.host_timestamp,
|
|
"color_shape": list(color.rgb.shape),
|
|
"depth_shape": list(self.raw.depth.shape),
|
|
"depth_units": "mm",
|
|
"color_jpeg_b64": _b64(encoded_color(self.seq, color).jpeg()),
|
|
"depth_png_b64": _b64(buf.tobytes()),
|
|
}
|
|
return self._payload
|
|
|
|
|
|
_encoded_raws: deque = deque(maxlen=ENCODED_RAWS_KEPT)
|
|
_encoded_raws_lock = threading.Lock()
|
|
|
|
|
|
def encoded_raw(seq: int, raw: RawFrame) -> EncodedRaw:
|
|
"""The shared EncodedRaw of frameset number `seq` (built from the first RawFrame seen for it)."""
|
|
with _encoded_raws_lock:
|
|
for enc in _encoded_raws:
|
|
if enc.seq == seq and enc.raw.color is raw.color:
|
|
return enc
|
|
enc = EncodedRaw(seq, raw)
|
|
_encoded_raws.append(enc)
|
|
return enc
|
|
|
|
|
|
def _next_frame_id() -> int:
|
|
global _frame_count
|
|
with _frame_count_lock:
|
|
_frame_count += 1
|
|
return _frame_count
|
|
|
|
|
|
def _frame_to_dict(frame: CameraFrame, include_images: bool = True) -> dict:
|
|
"""/api/capture and /ws JSON of a frame. frame_id counts responses, as before;
|
|
frameset_seq (added field) numbers the camera frameset, as /api/raw/latest does."""
|
|
enc = encoded(frame)
|
|
result = {
|
|
"frame_id": _next_frame_id(),
|
|
"frameset_seq": frame.frameset_seq,
|
|
"timestamp": frame.timestamp,
|
|
"rgb_shape": list(frame.rgb.shape),
|
|
"depth_shape": list(frame.depth.shape),
|
|
**enc.depth_stats(),
|
|
}
|
|
if include_images:
|
|
result.update(enc.images_b64())
|
|
return result
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Endpoints
|
|
# ---------------------------------------------------------------------------
|
|
|
|
# /health is mounted as public (no API key) in main.py
|
|
|
|
|
|
def _calibration_payload() -> dict | None:
|
|
"""Full calibration (intrinsics + distortion + extrinsics) or None.
|
|
|
|
Enough to align depth to color outside the SDK: per stream width, height
|
|
and distortion model (rs.distortion member name), depth_scale_m (meters per
|
|
raw unit). extrinsics_ir_to_rgb: p_rgb = R p_ir + t, R row-major (the SDK's
|
|
is column-major), t in millimeters.
|
|
"""
|
|
if not _camera or not _camera.intrinsics:
|
|
return None
|
|
i = _camera.intrinsics
|
|
e = _camera.extrinsics
|
|
dir_ = _camera.distortion_ir
|
|
drgb = _camera.distortion_rgb
|
|
|
|
def _d(d):
|
|
return None if d is None else {"k1": d.k1, "k2": d.k2, "k3": d.k3, "p1": d.p1, "p2": d.p2}
|
|
|
|
def _stream(name):
|
|
width, height = _camera.stream_sizes.get(name, (None, None))
|
|
return {"width": width, "height": height, "model": _camera.distortion_models.get(name)}
|
|
|
|
return {
|
|
"ir": {
|
|
"fx": i.fx_ir, "fy": i.fy_ir, "cx": i.cx_ir, "cy": i.cy_ir,
|
|
"distortion": _d(dir_),
|
|
**_stream("ir"),
|
|
},
|
|
"rgb": {
|
|
"fx": i.fx_rgb, "fy": i.fy_rgb, "cx": i.cx_rgb, "cy": i.cy_rgb,
|
|
"distortion": _d(drgb),
|
|
**_stream("rgb"),
|
|
},
|
|
"extrinsics_ir_to_rgb": None if e is None else {
|
|
"rotation": list(e.rotation),
|
|
"translation": list(e.translation),
|
|
},
|
|
"depth_scale_m": _camera.depth_scale(),
|
|
}
|
|
|
|
|
|
def _backprojection_intrinsics() -> tuple[float, float, float, float]:
|
|
"""(fx, fy, cx, cy) for back-projecting a depth pixel to 3D.
|
|
|
|
Picks the intrinsics that match the grid the depth map lives on:
|
|
- depth aligned to color (USB 3 default) -> color (rgb) intrinsics
|
|
- no alignment (USB 2 fallback) -> depth (ir) intrinsics
|
|
Using the ir intrinsics on an aligned (color-grid) depth map inflates
|
|
X/Y by fx_rgb/fx_ir (~1.54x on the D435i).
|
|
"""
|
|
if _camera and _camera.intrinsics:
|
|
i = _camera.intrinsics
|
|
if _camera.is_depth_aligned_to_color():
|
|
return i.fx_rgb, i.fy_rgb, i.cx_rgb, i.cy_rgb
|
|
return i.fx_ir, i.fy_ir, i.cx_ir, i.cy_ir
|
|
# Default intrinsics for 640x480 when calibration is unavailable.
|
|
return 445.0, 445.0, 320.0, 240.0
|
|
|
|
|
|
@router.get("/status")
|
|
async def status():
|
|
"""Camera status and intrinsics."""
|
|
return ok({
|
|
"sdk_initialized": _camera.is_initialized() if _camera else False,
|
|
"camera_open": _camera.is_open() if _camera else False,
|
|
"color_available": _camera.is_color_available() if (_camera and _camera.is_open()) else False,
|
|
"usb_type": _camera.usb_type() if (_camera and _camera.is_open()) else None,
|
|
"frame_count": _frame_count,
|
|
"resolution": _camera.resolution() if (_camera and _camera.is_open()) else None,
|
|
"intrinsics": _calibration_payload(),
|
|
})
|
|
|
|
|
|
@router.get("/discover")
|
|
def discover(timeout_ms: int = Query(default=5000, ge=1000, le=30000)):
|
|
"""Discover available RealSense cameras on USB bus."""
|
|
if not _camera or not _camera.is_initialized():
|
|
return fail("SDK_NOT_INITIALIZED", "SDK not initialized")
|
|
|
|
devices = _camera.discover(timeout_ms=timeout_ms)
|
|
return ok({
|
|
"cameras": [
|
|
{
|
|
"index": d.index,
|
|
"vid": f"{d.vid:04x}",
|
|
"pid": f"{d.pid:04x}",
|
|
"serial": d.serial,
|
|
"bus": d.bus,
|
|
"device": d.device,
|
|
}
|
|
for d in devices
|
|
],
|
|
"count": len(devices),
|
|
})
|
|
|
|
|
|
@router.post("/open")
|
|
def open_camera(device_index: int = Query(default=0, ge=0)):
|
|
"""Open a discovered camera by index and start streaming.
|
|
|
|
Idempotent: if a camera is already open, returns the current open state
|
|
instead of failing — the server only supports one open camera at a time.
|
|
"""
|
|
if not _camera or not _camera.is_initialized():
|
|
return fail("SDK_NOT_INITIALIZED", "SDK not initialized")
|
|
|
|
if _camera.is_open():
|
|
return ok({
|
|
"status": "open",
|
|
"device_index": _camera.device_index,
|
|
"intrinsics": _calibration_payload(),
|
|
"already_open": True,
|
|
})
|
|
|
|
if _camera.open(device_index):
|
|
return ok({
|
|
"status": "open",
|
|
"device_index": device_index,
|
|
"intrinsics": _calibration_payload(),
|
|
"already_open": False,
|
|
})
|
|
else:
|
|
return fail("OPEN_FAILED", "Failed to open camera")
|
|
|
|
|
|
@router.post("/close")
|
|
def close_camera():
|
|
"""Close the active camera and stop streaming."""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("CAMERA_NOT_OPEN", "No camera is open")
|
|
|
|
_camera.close()
|
|
return ok({"status": "closed"})
|
|
|
|
|
|
@router.get("/capture")
|
|
def capture():
|
|
"""Capture a single RGB+depth frame. Returns JSON with base64-encoded images."""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("CAMERA_NOT_OPEN", "Camera not open")
|
|
|
|
frame = _camera.capture()
|
|
if frame is None:
|
|
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
|
|
|
|
data = _frame_to_dict(frame)
|
|
with timer.phase("json"):
|
|
return JSONResponse(ok(data))
|
|
|
|
|
|
@router.get("/rgb")
|
|
def get_rgb():
|
|
"""Get RGB frame as JPEG image."""
|
|
if not _camera or not _camera.is_open():
|
|
raise HTTPException(503, "Camera not open")
|
|
|
|
frame = _camera.cached_frame()
|
|
if frame is None:
|
|
raise HTTPException(504, "No RGB frame")
|
|
|
|
return Response(content=encoded(frame).rgb_jpeg(), media_type="image/jpeg")
|
|
|
|
|
|
@router.get("/depth")
|
|
def get_depth():
|
|
"""Get depth frame as 16-bit PNG (values in mm)."""
|
|
if not _camera or not _camera.is_open():
|
|
raise HTTPException(503, "Camera not open")
|
|
|
|
frame = _camera.cached_frame()
|
|
if frame is None:
|
|
raise HTTPException(504, "No depth frame")
|
|
|
|
return Response(content=encoded(frame).depth_png(), media_type="image/png")
|
|
|
|
|
|
@router.get("/depth/color")
|
|
def get_depth_colormap():
|
|
"""Get depth frame as JET colormap JPEG."""
|
|
if not _camera or not _camera.is_open():
|
|
raise HTTPException(503, "Camera not open")
|
|
|
|
frame = _camera.cached_frame()
|
|
if frame is None:
|
|
raise HTTPException(504, "No depth frame")
|
|
|
|
return Response(content=encoded(frame).depth_color_jpeg(), media_type="image/jpeg")
|
|
|
|
|
|
@router.get("/color/latest", summary="Fast color channel (no align)", tags=["capture"])
|
|
def get_color_latest(
|
|
after: int | None = Query(default=None, ge=0, description="Wait for a frame numbered above this"),
|
|
timeout_ms: int = Query(default=1000, ge=0, le=COLOR_WAIT_MAX_MS),
|
|
):
|
|
"""Latest color JPEG of the fast channel: every camera frame, NOT aligned to depth.
|
|
|
|
Not /api/rgb: that one is the color of the aligned color+depth frame, the
|
|
same frame /api/depth returns. With `after=N`, waits up to `timeout_ms` for
|
|
a frame numbered above N, so a client can read every frame without polling
|
|
on empty. 504 when no such frame arrives in time. Headers: X-Frame-Seq,
|
|
X-Camera-Timestamp-Ms (camera clock), X-Camera-Timestamp-Domain,
|
|
X-Host-Timestamp (arrival on the host, seconds).
|
|
"""
|
|
if not _camera or not _camera.is_open():
|
|
raise HTTPException(503, "Camera not open")
|
|
|
|
seq, color = _camera.color_frame(after, timeout_ms)
|
|
if color is None:
|
|
raise HTTPException(504, "No new color frame")
|
|
|
|
return Response(content=encoded_color(seq, color).jpeg(), media_type="image/jpeg",
|
|
headers=color_headers(seq, color))
|
|
|
|
|
|
@router.get("/raw/latest", summary="Raw color + depth of one frameset (no align)", tags=["capture"])
|
|
def get_raw_latest(
|
|
after: int | None = Query(default=None, ge=0, description="Wait for a frameset numbered above this"),
|
|
timeout_ms: int = Query(default=1000, ge=0, le=COLOR_WAIT_MAX_MS),
|
|
):
|
|
"""Color and depth of the SAME frameset, NOT aligned, for aligning on the host.
|
|
|
|
JSON in the usual envelope: frameset_seq (the numbering of /api/color/latest
|
|
and of frameset_seq in /api/capture), camera_timestamp_ms, timestamp_domain,
|
|
host_timestamp, color_jpeg_b64 (color sensor grid), depth_png_b64 (16-bit
|
|
PNG, millimeters, depth sensor grid), color_shape, depth_shape,
|
|
depth_units. Calibration to align them: /api/intrinsics. With `after=N`,
|
|
waits up to `timeout_ms` for a frameset numbered above N. 504 when none
|
|
arrives in time.
|
|
"""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("CAMERA_NOT_OPEN", "Camera not open")
|
|
|
|
seq, raw = _camera.raw_frame(after, timeout_ms)
|
|
if raw is None:
|
|
raise HTTPException(504, "No new frameset")
|
|
|
|
payload = encoded_raw(seq, raw).payload()
|
|
with timer.phase("json_raw"):
|
|
return JSONResponse(ok(payload))
|
|
|
|
|
|
@router.get("/pointcloud", summary="3D Point cloud", tags=["capture"])
|
|
def get_pointcloud(
|
|
step: int = Query(default=4, ge=1, le=16),
|
|
max_depth_mm: int = Query(default=4000, ge=100, le=10000),
|
|
):
|
|
"""Get 3D point cloud as JSON array of [x,y,z,r,g,b] points.
|
|
|
|
Points are computed server-side using depth + RGB + intrinsics.
|
|
step: subsampling factor (4 = every 4th pixel, faster)
|
|
max_depth_mm: discard points beyond this depth
|
|
"""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("NO_CAMERA", "Camera not open")
|
|
|
|
frame = _camera.capture()
|
|
if frame is None:
|
|
return fail("TIMEOUT", "Frame capture timeout")
|
|
|
|
depth = frame.depth
|
|
rgb = frame.rgb # BGR format
|
|
|
|
fx, fy, cx, cy = _backprojection_intrinsics()
|
|
|
|
h, w = depth.shape
|
|
points = []
|
|
|
|
for y in range(0, h, step):
|
|
for x in range(0, w, step):
|
|
z_val = float(depth[y, x])
|
|
if z_val <= 0 or z_val > max_depth_mm:
|
|
continue
|
|
z_m = z_val / 1000.0
|
|
x_m = (x - cx) * z_m / fx
|
|
y_m = (y - cy) * z_m / fy
|
|
b_val, g_val, r_val = rgb[y, x] # BGR to RGB
|
|
points.append([
|
|
round(x_m, 4), round(-y_m, 4), round(-z_m, 4),
|
|
int(r_val), int(g_val), int(b_val)
|
|
])
|
|
|
|
return ok({
|
|
"points": points,
|
|
"count": len(points),
|
|
"step": step,
|
|
"intrinsics": {
|
|
"fx": fx, "fy": fy, "cx": cx, "cy": cy
|
|
}
|
|
})
|
|
|
|
|
|
@router.get("/pointcloud/roi", summary="3D Point cloud within ROI", tags=["capture"])
|
|
def get_pointcloud_roi(
|
|
u_min: int = Query(..., ge=0, description="ROI left pixel (inclusive)"),
|
|
v_min: int = Query(..., ge=0, description="ROI top pixel (inclusive)"),
|
|
u_max: int = Query(..., ge=1, description="ROI right pixel (exclusive)"),
|
|
v_max: int = Query(..., ge=1, description="ROI bottom pixel (exclusive)"),
|
|
step: int = Query(default=1, ge=1, le=16),
|
|
max_depth_mm: int = Query(default=4000, ge=100, le=10000),
|
|
):
|
|
"""Get 3D point cloud restricted to a pixel-space ROI.
|
|
|
|
Coordinates are RAW image pixels (no flip applied) in the depth/RGB
|
|
frame (typically 640x480). Points are returned as [x, -y, -z, r, g, b]
|
|
— same convention as /pointcloud so the frontend renderer is compatible.
|
|
"""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("NO_CAMERA", "Camera not open")
|
|
|
|
frame = _camera.capture()
|
|
if frame is None:
|
|
return fail("TIMEOUT", "Frame capture timeout")
|
|
|
|
depth = frame.depth
|
|
rgb = frame.rgb # BGR format
|
|
h, w = depth.shape
|
|
|
|
u0 = max(0, min(w, u_min))
|
|
v0 = max(0, min(h, v_min))
|
|
u1 = max(0, min(w, u_max))
|
|
v1 = max(0, min(h, v_max))
|
|
if u1 <= u0 or v1 <= v0:
|
|
return fail("INVALID_ROI", f"Empty ROI ({u0},{v0})-({u1},{v1}) in {w}x{h}")
|
|
|
|
fx, fy, cx, cy = _backprojection_intrinsics()
|
|
|
|
# Vectorized: restrict both arrays to the ROI and filter by depth mask.
|
|
roi_depth = depth[v0:v1:step, u0:u1:step].astype(np.float32)
|
|
roi_rgb = rgb[v0:v1:step, u0:u1:step]
|
|
mask = (roi_depth > 0) & (roi_depth <= max_depth_mm)
|
|
|
|
if not mask.any():
|
|
return ok({
|
|
"points": [],
|
|
"count": 0,
|
|
"step": step,
|
|
"roi": {"u_min": u0, "v_min": v0, "u_max": u1, "v_max": v1},
|
|
"intrinsics": {"fx": fx, "fy": fy, "cx": cx, "cy": cy},
|
|
})
|
|
|
|
ys_local, xs_local = np.where(mask)
|
|
zs_mm = roi_depth[ys_local, xs_local]
|
|
# Absolute pixel coordinates within the full image (needed for fx/cx back-projection).
|
|
xs_abs = u0 + xs_local * step
|
|
ys_abs = v0 + ys_local * step
|
|
zs_m = zs_mm / 1000.0
|
|
xs_m = (xs_abs - cx) * zs_m / fx
|
|
ys_m = (ys_abs - cy) * zs_m / fy
|
|
bgr = roi_rgb[ys_local, xs_local] # (N, 3) BGR
|
|
|
|
# Match /pointcloud convention: [x, -y, -z, r, g, b]
|
|
points = np.stack([
|
|
np.round(xs_m, 4),
|
|
np.round(-ys_m, 4),
|
|
np.round(-zs_m, 4),
|
|
bgr[:, 2].astype(np.float32),
|
|
bgr[:, 1].astype(np.float32),
|
|
bgr[:, 0].astype(np.float32),
|
|
], axis=1).tolist()
|
|
|
|
return ok({
|
|
"points": points,
|
|
"count": len(points),
|
|
"step": step,
|
|
"roi": {"u_min": u0, "v_min": v0, "u_max": u1, "v_max": v1},
|
|
"intrinsics": {"fx": fx, "fy": fy, "cx": cx, "cy": cy},
|
|
})
|
|
|
|
|
|
@router.get("/measure", summary="3D measurement at pixel", tags=["capture"])
|
|
def measure_point(
|
|
u: int = Query(..., ge=0, description="Pixel X in raw image coords (0..W-1)"),
|
|
v: int = Query(..., ge=0, description="Pixel Y in raw image coords (0..H-1)"),
|
|
radius: int = Query(default=6, ge=0, le=50, description="Search radius (px) for nearest valid depth"),
|
|
):
|
|
"""Find nearest valid-depth pixel around (u, v) and back-project to 3D.
|
|
|
|
Useful for testing RGB/depth alignment: click on RGB at a pixel, get the
|
|
closest point-cloud point within `radius`. Coordinates are RAW image pixels
|
|
(no flip applied) in the depth/RGB frame (typically 640x480).
|
|
"""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("CAMERA_NOT_OPEN", "Camera not open")
|
|
|
|
frame = _camera.capture()
|
|
if frame is None:
|
|
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
|
|
|
|
depth = frame.depth
|
|
h, w = depth.shape
|
|
if u >= w or v >= h:
|
|
return fail("OUT_OF_RANGE", f"Pixel ({u},{v}) outside image ({w}x{h})")
|
|
|
|
u0 = max(0, u - radius)
|
|
u1 = min(w, u + radius + 1)
|
|
v0 = max(0, v - radius)
|
|
v1 = min(h, v + radius + 1)
|
|
window = depth[v0:v1, u0:u1]
|
|
ys, xs = np.where(window > 0)
|
|
if len(xs) == 0:
|
|
return fail("NO_DEPTH", f"No valid depth within radius {radius}")
|
|
|
|
abs_xs = xs + u0
|
|
abs_ys = ys + v0
|
|
d2 = (abs_xs - u) ** 2 + (abs_ys - v) ** 2
|
|
idx = int(np.argmin(d2))
|
|
u_found = int(abs_xs[idx])
|
|
v_found = int(abs_ys[idx])
|
|
z_mm = int(depth[v_found, u_found])
|
|
|
|
fx, fy, cx, cy = _backprojection_intrinsics()
|
|
|
|
z_m = z_mm / 1000.0
|
|
x_m = (u_found - cx) * z_m / fx
|
|
y_m = (v_found - cy) * z_m / fy
|
|
dist_mm = float(np.sqrt(x_m * x_m + y_m * y_m + z_m * z_m) * 1000.0)
|
|
|
|
return ok({
|
|
"u": u,
|
|
"v": v,
|
|
"u_found": u_found,
|
|
"v_found": v_found,
|
|
"pixel_offset": round(float(np.sqrt(float(d2[idx]))), 2),
|
|
"radius": radius,
|
|
"depth_mm": z_mm,
|
|
"distance_mm": round(dist_mm, 1),
|
|
"point_m": {
|
|
"x": round(x_m, 4),
|
|
"y": round(y_m, 4),
|
|
"z": round(z_m, 4),
|
|
},
|
|
})
|
|
|
|
|
|
@router.post("/snapshot")
|
|
def snapshot(
|
|
flip_h: bool = Query(default=False),
|
|
flip_v: bool = Query(default=False),
|
|
):
|
|
"""Capture a single RGB frame and save it as JPG in images_dir."""
|
|
if not _camera or not _camera.is_open():
|
|
return fail("CAMERA_NOT_OPEN", "Camera not open")
|
|
if _saver is None:
|
|
return fail("SAVER_NOT_READY", "Image saver not initialized")
|
|
|
|
frame = _camera.capture()
|
|
if frame is None:
|
|
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
|
|
|
|
filename = _saver.save_snapshot(frame.rgb, flip_h, flip_v)
|
|
return ok({"filename": filename})
|
|
|
|
|
|
@router.post("/recording/start")
|
|
async def recording_start(
|
|
flip_h: bool = Query(default=False),
|
|
flip_v: bool = Query(default=False),
|
|
interval: int = Query(default=1, ge=1, le=600),
|
|
):
|
|
"""Start continuous frame recording (applies to WebSocket stream loop).
|
|
|
|
interval: save 1 frame every N streamed frames (1 = every frame).
|
|
"""
|
|
if _saver is None:
|
|
return fail("SAVER_NOT_READY", "Image saver not initialized")
|
|
if _saver.is_recording:
|
|
return fail("ALREADY_RECORDING", "Recording already in progress")
|
|
|
|
status = _saver.start_recording(flip_h, flip_v, interval)
|
|
return ok(status)
|
|
|
|
|
|
@router.post("/recording/stop")
|
|
async def recording_stop():
|
|
"""Stop continuous frame recording."""
|
|
if _saver is None:
|
|
return fail("SAVER_NOT_READY", "Image saver not initialized")
|
|
if not _saver.is_recording:
|
|
return fail("NOT_RECORDING", "No recording in progress")
|
|
|
|
status = _saver.stop_recording()
|
|
return ok(status)
|
|
|
|
|
|
@router.get("/recording/status")
|
|
async def recording_status():
|
|
"""Current recording state."""
|
|
if _saver is None:
|
|
return fail("SAVER_NOT_READY", "Image saver not initialized")
|
|
return ok(_saver.status())
|
|
|
|
|
|
@router.get("/intrinsics")
|
|
async def get_intrinsics():
|
|
"""Get camera intrinsic parameters."""
|
|
if not _camera:
|
|
return fail("INTRINSICS_NOT_AVAILABLE", "Camera driver not ready")
|
|
if not _camera.is_open():
|
|
return fail("INTRINSICS_NOT_AVAILABLE", "Camera not open")
|
|
if not _camera.intrinsics:
|
|
return fail(
|
|
"INTRINSICS_NOT_AVAILABLE",
|
|
"Camera open but SDK did not return calibration (AS_SDK_GetCamParameter). "
|
|
"Check docker logs for 'GetCamParameter ... failed'.",
|
|
)
|
|
|
|
return ok(_calibration_payload())
|