integrated finger process pipeline on remapped 2d image
This commit is contained in:
ykcheungab
@@ -8,14 +8,70 @@ applied to each live frame with cv2.remap().
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import orisys
|
||||
from orisys.frame_processors import UVUnrollFrameProcessor
|
||||
from orisys.unroll import (
|
||||
barycentric_grid,
|
||||
build_or_load_domain,
|
||||
compute_uv_distortion_metrics,
|
||||
face_uvs_from_loop_data,
|
||||
load_calib_json,
|
||||
load_exported_data,
|
||||
recommend_edge_expand,
|
||||
uv_triangle_to_image,
|
||||
)
|
||||
|
||||
|
||||
class StepTimer:
|
||||
"""Accumulate per-step wall times for rolling runtime averages."""
|
||||
|
||||
def __init__(self):
|
||||
self._starts = {}
|
||||
self.totals = {}
|
||||
self.counts = {}
|
||||
|
||||
def start(self, name):
|
||||
self._starts[name] = time.perf_counter()
|
||||
|
||||
def stop(self, name):
|
||||
dt = time.perf_counter() - self._starts.pop(name)
|
||||
self.totals[name] = self.totals.get(name, 0.0) + dt
|
||||
self.counts[name] = self.counts.get(name, 0) + 1
|
||||
return dt
|
||||
|
||||
def avg_ms(self, name):
|
||||
if self.counts.get(name, 0) == 0:
|
||||
return 0.0
|
||||
return 1000.0 * self.totals[name] / self.counts[name]
|
||||
|
||||
|
||||
def benchmark_step(label, func, *args, **kwargs):
|
||||
"""Run func once and print its wall time."""
|
||||
t0 = time.perf_counter()
|
||||
result = func(*args, **kwargs)
|
||||
ms = (time.perf_counter() - t0) * 1000.0
|
||||
print(f"[benchmark] {label}: {ms:.1f} ms")
|
||||
return result
|
||||
|
||||
|
||||
def print_runtime_benchmark(timer):
|
||||
"""Format rolling average timings for the live loop."""
|
||||
parts = [
|
||||
f"capture={timer.avg_ms('capture'):.1f}ms",
|
||||
f"uv_prep={timer.avg_ms('uv_prepare'):.1f}ms",
|
||||
f"uv_remap={timer.avg_ms('uv_remap'):.1f}ms",
|
||||
f"get_img={timer.avg_ms('get_img'):.1f}ms",
|
||||
f"flow={timer.avg_ms('flow'):.1f}ms",
|
||||
f"hhd={timer.avg_ms('hhd'):.1f}ms",
|
||||
f"contact={timer.avg_ms('contact'):.1f}ms",
|
||||
f"viz={timer.avg_ms('viz'):.1f}ms",
|
||||
]
|
||||
return "bench[" + ", ".join(parts) + "]"
|
||||
|
||||
|
||||
def resize_to_height(image, target_height):
|
||||
@@ -58,6 +114,52 @@ def add_panel_title(image, title):
|
||||
return out
|
||||
|
||||
|
||||
def apply_display_mask(image, valid_mask, border_value=0):
|
||||
"""Zero invalid UV pixels in visualization panels."""
|
||||
if image is None or valid_mask is None:
|
||||
return image
|
||||
out = image.copy()
|
||||
mask = valid_mask.astype(np.uint8)
|
||||
if mask.shape != out.shape[:2]:
|
||||
mask = cv2.resize(mask, (out.shape[1], out.shape[0]), interpolation=cv2.INTER_NEAREST)
|
||||
invalid = ~mask.astype(bool)
|
||||
out[invalid] = border_value
|
||||
return out
|
||||
|
||||
|
||||
def contact_inside_mask(centroid, valid_mask):
|
||||
if centroid is None or valid_mask is None:
|
||||
return None
|
||||
if len(centroid) < 2:
|
||||
return None
|
||||
cx, cy = int(centroid[0]), int(centroid[1])
|
||||
if cy < 0 or cy >= valid_mask.shape[0] or cx < 0 or cx >= valid_mask.shape[1]:
|
||||
return False
|
||||
return bool(valid_mask[cy, cx])
|
||||
|
||||
|
||||
def overlay_runtime_status(image, sensor, centroid=None):
|
||||
"""Annotate a panel with mask-aware runtime diagnostics."""
|
||||
out = image.copy()
|
||||
stats = getattr(sensor, "mask_stats", {}) or {}
|
||||
coverage = 100.0 * float(stats.get("coverage", 0.0))
|
||||
flow_valid = float(stats.get("flow_mean_valid", 0.0))
|
||||
flow_invalid = float(stats.get("flow_mean_invalid", 0.0))
|
||||
centroid_ok = contact_inside_mask(centroid, getattr(sensor, "valid_mask", None))
|
||||
centroid_label = "n/a" if centroid_ok is None else ("inside" if centroid_ok else "outside")
|
||||
lines = [
|
||||
f"mask {coverage:.1f}%",
|
||||
f"flow valid {flow_valid:.3f}",
|
||||
f"flow invalid {flow_invalid:.3f}",
|
||||
f"contact {centroid_label}",
|
||||
]
|
||||
y = 54
|
||||
for line in lines:
|
||||
cv2.putText(out, line, (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 2, cv2.LINE_AA)
|
||||
y += 24
|
||||
return out
|
||||
|
||||
|
||||
def make_tiled_dashboard(panels, tile_width=640, tile_height=360):
|
||||
"""Arrange four panels into a 2x2 dashboard."""
|
||||
tiles = [
|
||||
@@ -69,382 +171,6 @@ def make_tiled_dashboard(panels, tile_width=640, tile_height=360):
|
||||
return np.vstack([top_row, bottom_row])
|
||||
|
||||
|
||||
def load_calib_json(json_path):
|
||||
with open(json_path, "r", encoding="utf-8") as f:
|
||||
calib = json.load(f)
|
||||
K = np.array(calib["camera_matrix"], dtype=np.float64)
|
||||
dist = np.array(calib["distortion"], dtype=np.float64).flatten()
|
||||
img_size = tuple(int(v) for v in calib["image_size"])
|
||||
return K, dist, img_size
|
||||
|
||||
|
||||
def load_exported_data(mesh_path, intrinsics_path, pose_path):
|
||||
mesh = np.load(mesh_path)
|
||||
cam = np.load(intrinsics_path)
|
||||
return {
|
||||
"vertices": mesh["vertices"].astype(np.float64),
|
||||
"faces": mesh["faces"].astype(np.int32),
|
||||
"uv_data": mesh["uv_data"].astype(np.float64),
|
||||
"loop_to_vertex": mesh["loop_to_vertex"].astype(np.int32),
|
||||
"K": cam["K"].astype(np.float64),
|
||||
"width": int(cam["width"]),
|
||||
"height": int(cam["height"]),
|
||||
"world_to_cam": np.load(pose_path).astype(np.float64),
|
||||
}
|
||||
|
||||
|
||||
def face_uvs_from_loop_data(faces, uv_data, loop_to_vertex):
|
||||
if len(uv_data) == 0:
|
||||
raise ValueError("mesh.npz does not contain UV data; UV flattening is unavailable.")
|
||||
if len(uv_data) != len(faces) * 3:
|
||||
raise ValueError("Expected a triangulated mesh with exactly 3 UV loops per face.")
|
||||
|
||||
face_loop_vertices = loop_to_vertex.reshape(-1, 3)
|
||||
if face_loop_vertices.shape != faces.shape or not np.array_equal(face_loop_vertices, faces):
|
||||
raise ValueError("UV loop order does not match exported face vertex order.")
|
||||
return uv_data.reshape(-1, 3, 2)
|
||||
|
||||
|
||||
def project_world_to_image(points_world, K, world_to_cam):
|
||||
"""Project world points into the image using Blender/OpenCV-compatible intrinsics."""
|
||||
points_h = np.column_stack((points_world, np.ones(len(points_world), dtype=np.float64)))
|
||||
points_cam = (world_to_cam @ points_h.T).T[:, :3]
|
||||
z = -points_cam[:, 2]
|
||||
x = K[0, 0] * (points_cam[:, 0] / z) + K[0, 2]
|
||||
y = K[1, 1] * (points_cam[:, 1] / z) + K[1, 2]
|
||||
return np.column_stack((x, y)), z
|
||||
|
||||
|
||||
def barycentric_grid(px, py, tri):
|
||||
a, b, c = tri
|
||||
denom = (b[1] - c[1]) * (a[0] - c[0]) + (c[0] - b[0]) * (a[1] - c[1])
|
||||
if abs(denom) < 1e-12:
|
||||
return None
|
||||
w0 = ((b[1] - c[1]) * (px - c[0]) + (c[0] - b[0]) * (py - c[1])) / denom
|
||||
w1 = ((c[1] - a[1]) * (px - c[0]) + (a[0] - c[0]) * (py - c[1])) / denom
|
||||
w2 = 1.0 - w0 - w1
|
||||
return w0, w1, w2
|
||||
|
||||
|
||||
def triangle_edge_lengths_3d(tri):
|
||||
return np.array(
|
||||
[
|
||||
np.linalg.norm(tri[1] - tri[0]),
|
||||
np.linalg.norm(tri[2] - tri[1]),
|
||||
np.linalg.norm(tri[0] - tri[2]),
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def triangle_edge_lengths_2d(tri):
|
||||
return np.array(
|
||||
[
|
||||
np.linalg.norm(tri[1] - tri[0]),
|
||||
np.linalg.norm(tri[2] - tri[1]),
|
||||
np.linalg.norm(tri[0] - tri[2]),
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def triangle_area_3d(tri):
|
||||
return 0.5 * np.linalg.norm(np.cross(tri[1] - tri[0], tri[2] - tri[0]))
|
||||
|
||||
|
||||
def triangle_area_2d(tri):
|
||||
return 0.5 * abs(
|
||||
(tri[1, 0] - tri[0, 0]) * (tri[2, 1] - tri[0, 1])
|
||||
- (tri[1, 1] - tri[0, 1]) * (tri[2, 0] - tri[0, 0])
|
||||
)
|
||||
|
||||
|
||||
def summarize_uv_region(relative_scale, anisotropy, mask):
|
||||
mask_scale = mask & np.isfinite(relative_scale) & (relative_scale > 0)
|
||||
mask_aniso = mask & np.isfinite(anisotropy) & (anisotropy > 0)
|
||||
if not np.any(mask_scale):
|
||||
return {
|
||||
"face_count": 0,
|
||||
"relative_scale_p05": float("nan"),
|
||||
"relative_scale_p50": float("nan"),
|
||||
"relative_scale_p95": float("nan"),
|
||||
"relative_scale_min": float("nan"),
|
||||
"relative_scale_max": float("nan"),
|
||||
"anisotropy_p50": float("nan"),
|
||||
"anisotropy_p95": float("nan"),
|
||||
"anisotropy_max": float("nan"),
|
||||
}
|
||||
return {
|
||||
"face_count": int(np.count_nonzero(mask_scale)),
|
||||
"relative_scale_p05": float(np.percentile(relative_scale[mask_scale], 5)),
|
||||
"relative_scale_p50": float(np.percentile(relative_scale[mask_scale], 50)),
|
||||
"relative_scale_p95": float(np.percentile(relative_scale[mask_scale], 95)),
|
||||
"relative_scale_min": float(np.min(relative_scale[mask_scale])),
|
||||
"relative_scale_max": float(np.max(relative_scale[mask_scale])),
|
||||
"anisotropy_p50": float(np.percentile(anisotropy[mask_aniso], 50)) if np.any(mask_aniso) else float("nan"),
|
||||
"anisotropy_p95": float(np.percentile(anisotropy[mask_aniso], 95)) if np.any(mask_aniso) else float("nan"),
|
||||
"anisotropy_max": float(np.max(anisotropy[mask_aniso])) if np.any(mask_aniso) else float("nan"),
|
||||
}
|
||||
|
||||
|
||||
def compute_uv_distortion_metrics(data, edge_band=0.12):
|
||||
"""Estimate local UV stretch/compression and anisotropy per face."""
|
||||
faces = data["faces"]
|
||||
vertices = data["vertices"]
|
||||
face_uvs = face_uvs_from_loop_data(faces, data["uv_data"], data["loop_to_vertex"])
|
||||
|
||||
local_scale = np.full(len(faces), np.nan, dtype=np.float64)
|
||||
relative_scale = np.full(len(faces), np.nan, dtype=np.float64)
|
||||
anisotropy = np.full(len(faces), np.nan, dtype=np.float64)
|
||||
uv_centers = np.full((len(faces), 2), np.nan, dtype=np.float64)
|
||||
edge_mask = np.zeros(len(faces), dtype=bool)
|
||||
|
||||
for idx, (face, uv_tri) in enumerate(zip(faces, face_uvs)):
|
||||
tri_3d = vertices[face]
|
||||
tri_uv = uv_tri
|
||||
uv_centers[idx] = np.mean(tri_uv, axis=0)
|
||||
|
||||
edges_3d = triangle_edge_lengths_3d(tri_3d)
|
||||
edges_uv = triangle_edge_lengths_2d(tri_uv)
|
||||
valid_edges = edges_uv > 1e-9
|
||||
if np.count_nonzero(valid_edges) < 2:
|
||||
continue
|
||||
|
||||
edge_scales = edges_3d[valid_edges] / edges_uv[valid_edges]
|
||||
local_scale[idx] = float(np.exp(np.mean(np.log(np.clip(edge_scales, 1e-12, None)))))
|
||||
anisotropy[idx] = float(np.max(edge_scales) / max(np.min(edge_scales), 1e-12))
|
||||
|
||||
area_uv = triangle_area_2d(tri_uv)
|
||||
area_3d = triangle_area_3d(tri_3d)
|
||||
if area_uv > 1e-12 and area_3d > 1e-12:
|
||||
local_scale[idx] = float(np.sqrt(area_3d / area_uv))
|
||||
|
||||
valid_scale = np.isfinite(local_scale) & (local_scale > 0)
|
||||
if not np.any(valid_scale):
|
||||
raise RuntimeError("Could not compute UV distortion metrics; UV triangles may be degenerate.")
|
||||
|
||||
median_scale = float(np.median(local_scale[valid_scale]))
|
||||
relative_scale[valid_scale] = local_scale[valid_scale] / median_scale
|
||||
min_edge_distance = np.min(
|
||||
np.column_stack(
|
||||
[
|
||||
uv_centers[:, 0],
|
||||
1.0 - uv_centers[:, 0],
|
||||
uv_centers[:, 1],
|
||||
1.0 - uv_centers[:, 1],
|
||||
]
|
||||
),
|
||||
axis=1,
|
||||
)
|
||||
edge_mask = min_edge_distance < edge_band
|
||||
middle_mask = ~edge_mask
|
||||
|
||||
valid_aniso = np.isfinite(anisotropy) & (anisotropy > 0)
|
||||
stats = {
|
||||
"median_mm_per_uv": median_scale,
|
||||
"relative_scale_p05": float(np.percentile(relative_scale[valid_scale], 5)),
|
||||
"relative_scale_p50": float(np.percentile(relative_scale[valid_scale], 50)),
|
||||
"relative_scale_p95": float(np.percentile(relative_scale[valid_scale], 95)),
|
||||
"relative_scale_min": float(np.min(relative_scale[valid_scale])),
|
||||
"relative_scale_max": float(np.max(relative_scale[valid_scale])),
|
||||
"anisotropy_p50": float(np.percentile(anisotropy[valid_aniso], 50)) if np.any(valid_aniso) else float("nan"),
|
||||
"anisotropy_p95": float(np.percentile(anisotropy[valid_aniso], 95)) if np.any(valid_aniso) else float("nan"),
|
||||
"anisotropy_max": float(np.max(anisotropy[valid_aniso])) if np.any(valid_aniso) else float("nan"),
|
||||
"edge_band": float(edge_band),
|
||||
}
|
||||
return {
|
||||
"relative_scale": relative_scale,
|
||||
"anisotropy": anisotropy,
|
||||
"uv_centers": uv_centers,
|
||||
"edge_mask": edge_mask,
|
||||
"middle_mask": middle_mask,
|
||||
"stats": stats,
|
||||
"middle_stats": summarize_uv_region(relative_scale, anisotropy, middle_mask),
|
||||
"edge_stats": summarize_uv_region(relative_scale, anisotropy, edge_mask),
|
||||
}
|
||||
|
||||
|
||||
def recommend_edge_expand(distortion, max_expand=3.0):
|
||||
"""Suggest how much more UV space to allocate to the edge band."""
|
||||
middle_p95 = distortion["middle_stats"]["relative_scale_p95"]
|
||||
edge_p95 = distortion["edge_stats"]["relative_scale_p95"]
|
||||
if not np.isfinite(middle_p95) or not np.isfinite(edge_p95) or middle_p95 <= 0:
|
||||
return 1.0
|
||||
# Use a square-root response so large edge distortion does not over-expand.
|
||||
return float(np.clip(np.sqrt(edge_p95 / middle_p95), 1.0, max_expand))
|
||||
|
||||
|
||||
def warp_uv_axis(values, edge_band, edge_expand):
|
||||
"""Allocate more normalized output span to UV edge regions."""
|
||||
u = np.asarray(values, dtype=np.float64)
|
||||
e = float(np.clip(edge_band, 0.0, 0.49))
|
||||
s = float(max(edge_expand, 1.0))
|
||||
if e <= 0.0 or s <= 1.0:
|
||||
return u.copy()
|
||||
|
||||
middle = 1.0 - 2.0 * e
|
||||
total = middle + 2.0 * e * s
|
||||
out = np.empty_like(u)
|
||||
|
||||
left = u < e
|
||||
right = u > (1.0 - e)
|
||||
center = ~(left | right)
|
||||
|
||||
out[left] = (u[left] * s) / total
|
||||
out[center] = (e * s + (u[center] - e)) / total
|
||||
out[right] = (e * s + middle + (u[right] - (1.0 - e)) * s) / total
|
||||
return out
|
||||
|
||||
|
||||
def uv_triangle_to_image(uv_tri, out_width, out_height, flip_v=True, edge_band=0.12, edge_expand=1.0):
|
||||
"""Map UV triangle into output image coordinates with optional edge expansion."""
|
||||
tri = np.empty_like(uv_tri)
|
||||
u_warp = warp_uv_axis(uv_tri[:, 0], edge_band=edge_band, edge_expand=edge_expand)
|
||||
v_warp = warp_uv_axis(uv_tri[:, 1], edge_band=edge_band, edge_expand=edge_expand)
|
||||
tri[:, 0] = u_warp * (out_width - 1)
|
||||
tri[:, 1] = ((1.0 - v_warp) if flip_v else v_warp) * (out_height - 1)
|
||||
return tri
|
||||
|
||||
|
||||
def build_uv_remap(
|
||||
data,
|
||||
source_K,
|
||||
source_width,
|
||||
source_height,
|
||||
out_width,
|
||||
out_height,
|
||||
flip_v=True,
|
||||
edge_band=0.12,
|
||||
edge_expand=1.0,
|
||||
):
|
||||
faces = data["faces"]
|
||||
vertices = data["vertices"]
|
||||
face_uvs = face_uvs_from_loop_data(faces, data["uv_data"], data["loop_to_vertex"])
|
||||
|
||||
map_x = np.full((out_height, out_width), -1.0, dtype=np.float32)
|
||||
map_y = np.full((out_height, out_width), -1.0, dtype=np.float32)
|
||||
valid_mask = np.zeros((out_height, out_width), dtype=bool)
|
||||
depth_buffer = np.full((out_height, out_width), np.inf, dtype=np.float64)
|
||||
|
||||
print("Building UV remap ...")
|
||||
t_start = time.time()
|
||||
for face_idx, (face, uv_tri) in enumerate(zip(faces, face_uvs)):
|
||||
tri = uv_triangle_to_image(
|
||||
uv_tri,
|
||||
out_width,
|
||||
out_height,
|
||||
flip_v=flip_v,
|
||||
edge_band=edge_band,
|
||||
edge_expand=edge_expand,
|
||||
)
|
||||
|
||||
x0 = max(0, int(np.floor(np.min(tri[:, 0]))))
|
||||
x1 = min(out_width - 1, int(np.ceil(np.max(tri[:, 0]))))
|
||||
y0 = max(0, int(np.floor(np.min(tri[:, 1]))))
|
||||
y1 = min(out_height - 1, int(np.ceil(np.max(tri[:, 1]))))
|
||||
if x1 < x0 or y1 < y0:
|
||||
continue
|
||||
|
||||
yy, xx = np.mgrid[y0:y1 + 1, x0:x1 + 1]
|
||||
bary = barycentric_grid(xx + 0.5, yy + 0.5, tri)
|
||||
if bary is None:
|
||||
continue
|
||||
|
||||
w0, w1, w2 = bary
|
||||
inside = (w0 >= -1e-6) & (w1 >= -1e-6) & (w2 >= -1e-6)
|
||||
if not np.any(inside):
|
||||
continue
|
||||
|
||||
weights = np.stack((w0[inside], w1[inside], w2[inside]), axis=1)
|
||||
points_world = weights @ vertices[face]
|
||||
pixels, depth = project_world_to_image(points_world, source_K, data["world_to_cam"])
|
||||
|
||||
src_x = pixels[:, 0]
|
||||
src_y = pixels[:, 1]
|
||||
target_y = yy[inside]
|
||||
target_x = xx[inside]
|
||||
valid = (
|
||||
(depth > 0)
|
||||
& (src_x >= 0)
|
||||
& (src_x <= source_width - 1)
|
||||
& (src_y >= 0)
|
||||
& (src_y <= source_height - 1)
|
||||
& (depth < depth_buffer[target_y, target_x])
|
||||
)
|
||||
if not np.any(valid):
|
||||
continue
|
||||
|
||||
tx = target_x[valid]
|
||||
ty = target_y[valid]
|
||||
map_x[ty, tx] = src_x[valid].astype(np.float32)
|
||||
map_y[ty, tx] = src_y[valid].astype(np.float32)
|
||||
valid_mask[ty, tx] = True
|
||||
depth_buffer[ty, tx] = depth[valid]
|
||||
|
||||
if (face_idx % 200) == 0:
|
||||
print(f" face {face_idx}/{len(faces)}")
|
||||
|
||||
print(f"Remap build took {time.time() - t_start:.1f} s")
|
||||
return {
|
||||
"map_x": map_x,
|
||||
"map_y": map_y,
|
||||
"valid_mask": valid_mask,
|
||||
"source_width": np.array(source_width, dtype=np.int32),
|
||||
"source_height": np.array(source_height, dtype=np.int32),
|
||||
"out_width": np.array(out_width, dtype=np.int32),
|
||||
"out_height": np.array(out_height, dtype=np.int32),
|
||||
"flip_v": np.array(int(flip_v), dtype=np.int32),
|
||||
"edge_band": np.array(edge_band, dtype=np.float64),
|
||||
"edge_expand": np.array(edge_expand, dtype=np.float64),
|
||||
"projection_K": source_K.astype(np.float64),
|
||||
"method": np.array("uv"),
|
||||
}
|
||||
|
||||
|
||||
def remap_valid_ratio(remap):
|
||||
return float(np.mean(remap["valid_mask"]))
|
||||
|
||||
|
||||
def load_cached_remap(path):
|
||||
data = np.load(path)
|
||||
return {key: data[key] for key in data.files}
|
||||
|
||||
|
||||
def cache_matches(remap, source_K, source_width, source_height, out_width, out_height, flip_v, edge_band, edge_expand):
|
||||
required = {"map_x", "map_y", "valid_mask", "source_width", "source_height", "out_width", "out_height"}
|
||||
if not required.issubset(remap.keys()):
|
||||
return False
|
||||
if remap["map_x"].shape != (out_height, out_width):
|
||||
return False
|
||||
if int(remap["source_width"]) != source_width or int(remap["source_height"]) != source_height:
|
||||
return False
|
||||
if int(remap["out_width"]) != out_width or int(remap["out_height"]) != out_height:
|
||||
return False
|
||||
if "flip_v" in remap and int(remap["flip_v"]) != int(flip_v):
|
||||
return False
|
||||
if "edge_band" in remap and not np.isclose(float(remap["edge_band"]), float(edge_band), rtol=1e-6, atol=1e-6):
|
||||
return False
|
||||
if "edge_expand" in remap and not np.isclose(float(remap["edge_expand"]), float(edge_expand), rtol=1e-6, atol=1e-6):
|
||||
return False
|
||||
if "projection_K" in remap and not np.allclose(remap["projection_K"], source_K, rtol=1e-6, atol=1e-6):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def flatten_frame(frame, remap, border_value=0):
|
||||
flat = cv2.remap(
|
||||
frame,
|
||||
remap["map_x"].astype(np.float32),
|
||||
remap["map_y"].astype(np.float32),
|
||||
interpolation=cv2.INTER_LINEAR,
|
||||
borderMode=cv2.BORDER_CONSTANT,
|
||||
borderValue=border_value,
|
||||
)
|
||||
flat = flat.copy()
|
||||
flat[~remap["valid_mask"].astype(bool)] = border_value
|
||||
return flat
|
||||
|
||||
|
||||
def make_uv_debug_image(data, out_width, out_height, flip_v=True, edge_band=0.12, edge_expand=1.0):
|
||||
"""Create a UV-domain reference image to verify orientation of the remap."""
|
||||
face_uvs = face_uvs_from_loop_data(data["faces"], data["uv_data"], data["loop_to_vertex"])
|
||||
@@ -576,22 +302,59 @@ def open_source(video_path, camera_id):
|
||||
return cv2.VideoCapture(video_path) if video_path else cv2.VideoCapture(camera_id)
|
||||
|
||||
|
||||
def parse_video_source(value):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return value
|
||||
PERF_TUNING_EPILOG = """
|
||||
Performance tuning (config JSON + CLI):
|
||||
tracker.dis_preset medium -> fast/ultrafast faster DIS optical flow
|
||||
tracker.loop_closure_interval 1 -> 3-5 fewer loop-closure remaps
|
||||
tracker.hhd_downsample 3 -> 4 smaller HHD Poisson grid
|
||||
contact.denoise_sigma 12 -> 6 or 0 faster FoE contact step
|
||||
--masked_poisson_solver sparse vs capfft benchmark HHD solve time
|
||||
--no-viz skip dashboard/imshow ~20 ms/frame savings
|
||||
"""
|
||||
|
||||
|
||||
def align_frame_to_uv_map(frame_bgr, flip_vertical=True):
|
||||
"""Match live-frame orientation to the Blender-exported UV remap convention."""
|
||||
if not flip_vertical:
|
||||
return frame_bgr
|
||||
return np.flipud(frame_bgr).copy()
|
||||
def run_deformation_timed(sensor, timer, check_motion=True, threshold=0, tail_frames=10):
|
||||
"""Run flow + gated HHD with separate benchmark buckets."""
|
||||
timer.start("flow")
|
||||
sensor.compute_flow()
|
||||
timer.stop("flow")
|
||||
|
||||
timer.start("hhd")
|
||||
run_hhd = True
|
||||
if check_motion:
|
||||
is_motion = sensor.detect_motion(threshold=threshold)
|
||||
try:
|
||||
tail_frames = max(0, int(tail_frames))
|
||||
except (TypeError, ValueError):
|
||||
tail_frames = 0
|
||||
if is_motion:
|
||||
sensor.tail_frames_count = 0
|
||||
elif sensor.tail_frames_count < tail_frames:
|
||||
sensor.tail_frames_count += 1
|
||||
else:
|
||||
run_hhd = False
|
||||
sensor.tail_frames_count = tail_frames
|
||||
if run_hhd:
|
||||
sensor.compute_hhd()
|
||||
timer.stop("hhd")
|
||||
|
||||
|
||||
def configure_masked_poisson(sensor, args):
|
||||
"""Apply masked-HHD solver settings to the externally fed SDK sensor."""
|
||||
sensor.masked_poisson_solver = args.masked_poisson_solver
|
||||
sensor.masked_poisson_max_boundary = args.masked_poisson_max_boundary
|
||||
sensor.masked_poisson_boundary_stride = args.masked_poisson_boundary_stride
|
||||
sensor.masked_poisson_max_iters = args.masked_poisson_max_iters
|
||||
sensor.masked_poisson_tol = args.masked_poisson_tol
|
||||
sensor.masked_poisson_reg = args.masked_poisson_reg
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Real-time UV-based finger unrolling")
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Real-time UV-based finger unrolling",
|
||||
epilog=PERF_TUNING_EPILOG,
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
)
|
||||
parser.add_argument("--mesh", required=True, help="Exported mesh.npz path")
|
||||
parser.add_argument("--K", dest="intrinsics_path", required=True, help="Exported camera_intrinsics.npz path")
|
||||
parser.add_argument("--pose", required=True, help="Exported world_to_cam.npy path")
|
||||
@@ -599,8 +362,8 @@ def main():
|
||||
# With a 12% UV edge band on each side, only about 76% of each axis is the
|
||||
# lower-distortion middle region. Use a slightly larger full remap so the
|
||||
# useful center keeps roughly the same sampling density as the prior 506x550.
|
||||
parser.add_argument("--out_width", type=int, default=672)
|
||||
parser.add_argument("--out_height", type=int, default=728)
|
||||
parser.add_argument("--out_width", type=int, default=504)
|
||||
parser.add_argument("--out_height", type=int, default=506)
|
||||
parser.add_argument("--camera_id", type=int, default=0)
|
||||
parser.add_argument("--video", help="Video file path (overrides camera)")
|
||||
parser.add_argument("--map_cache", default="unroll_map_uv.npz")
|
||||
@@ -610,13 +373,30 @@ def main():
|
||||
parser.add_argument("--edge_expand", type=float, default=0.0, help="Extra UV allocation for edge band; 0 chooses automatically from distortion stats")
|
||||
parser.add_argument("--sdk_config", default="./config/dd02-ov.json", help="Orisys SDK config path/name")
|
||||
parser.add_argument("--sdk_source", default=None, help="Dummy source used to initialize Sensor; defaults to --video or --camera_id")
|
||||
parser.add_argument("--masked_poisson_solver", choices=["sparse", "capfft"], default="sparse", help="Masked HHD Poisson solver used inside the SDK")
|
||||
parser.add_argument("--masked_poisson_max_boundary", type=int, default=96, help="Maximum number of UV-boundary samples used by the capfft solver")
|
||||
parser.add_argument("--masked_poisson_boundary_stride", type=int, default=1, help="Subsample stride applied before capfft boundary sampling")
|
||||
parser.add_argument("--masked_poisson_max_iters", type=int, default=3, help="Maximum capfft correction iterations per Poisson solve")
|
||||
parser.add_argument("--masked_poisson_tol", type=float, default=1e-4, help="Residual tolerance used by the capfft correction loop")
|
||||
parser.add_argument("--masked_poisson_reg", type=float, default=1e-8, help="Diagonal regularization applied to the capacitance matrix")
|
||||
parser.add_argument("--verbose", action="store_true", help="Enable verbose SDK logs")
|
||||
parser.add_argument(
|
||||
"--no-viz",
|
||||
action="store_true",
|
||||
help="Skip OpenCV dashboard; print forces and benchmark timings only",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
data = load_exported_data(args.mesh, args.intrinsics_path, args.pose)
|
||||
print("[benchmark] Startup timing:")
|
||||
data = benchmark_step("load_exported_data", load_exported_data, args.mesh, args.intrinsics_path, args.pose)
|
||||
flip_v = not args.no_flip_v
|
||||
flip_source_vertical = not args.no_flip_source_vertical
|
||||
distortion = compute_uv_distortion_metrics(data, edge_band=args.edge_band)
|
||||
distortion = benchmark_step(
|
||||
"compute_uv_distortion_metrics",
|
||||
compute_uv_distortion_metrics,
|
||||
data,
|
||||
edge_band=args.edge_band,
|
||||
)
|
||||
distortion_stats = distortion["stats"]
|
||||
middle_stats = distortion["middle_stats"]
|
||||
edge_stats = distortion["edge_stats"]
|
||||
@@ -648,7 +428,7 @@ def main():
|
||||
)
|
||||
|
||||
if args.calib:
|
||||
source_K, dist_coeffs, img_size = load_calib_json(args.calib)
|
||||
source_K, dist_coeffs, img_size = benchmark_step("load_calib_json", load_calib_json, args.calib)
|
||||
source_width, source_height = img_size
|
||||
print(f"Calibration loaded. Live image size: {(source_width, source_height)}")
|
||||
else:
|
||||
@@ -658,44 +438,30 @@ def main():
|
||||
source_height = data["height"]
|
||||
print("No calibration file provided; using exported intrinsics for projection.")
|
||||
|
||||
remap = None
|
||||
if os.path.exists(args.map_cache):
|
||||
cached = load_cached_remap(args.map_cache)
|
||||
if cache_matches(
|
||||
cached,
|
||||
source_K=source_K,
|
||||
source_width=source_width,
|
||||
source_height=source_height,
|
||||
out_width=args.out_width,
|
||||
out_height=args.out_height,
|
||||
flip_v=flip_v,
|
||||
edge_band=args.edge_band,
|
||||
edge_expand=edge_expand,
|
||||
):
|
||||
remap = cached
|
||||
print(f"Loaded cached UV remap. Valid ratio: {remap_valid_ratio(remap):.3f}")
|
||||
else:
|
||||
print("Cached UV remap is stale. Regenerating.")
|
||||
domain = benchmark_step(
|
||||
"build_or_load_domain",
|
||||
build_or_load_domain,
|
||||
args.mesh,
|
||||
args.intrinsics_path,
|
||||
args.pose,
|
||||
args.map_cache,
|
||||
source_K,
|
||||
source_width,
|
||||
source_height,
|
||||
args.out_width,
|
||||
args.out_height,
|
||||
flip_v=flip_v,
|
||||
edge_band=args.edge_band,
|
||||
edge_expand=edge_expand,
|
||||
include_surface=True,
|
||||
)
|
||||
remap = domain.remap
|
||||
remap.ensure_fast_maps()
|
||||
edge_expand = float(remap.metadata.get("edge_expand", edge_expand))
|
||||
|
||||
if remap is None:
|
||||
remap = build_uv_remap(
|
||||
data,
|
||||
source_K=source_K,
|
||||
source_width=source_width,
|
||||
source_height=source_height,
|
||||
out_width=args.out_width,
|
||||
out_height=args.out_height,
|
||||
flip_v=flip_v,
|
||||
edge_band=args.edge_band,
|
||||
edge_expand=edge_expand,
|
||||
)
|
||||
valid_ratio = remap_valid_ratio(remap)
|
||||
print(f"Generated UV remap valid ratio: {valid_ratio:.3f}")
|
||||
if valid_ratio == 0.0:
|
||||
raise RuntimeError("Generated UV remap contains no valid samples.")
|
||||
np.savez_compressed(args.map_cache, **remap)
|
||||
|
||||
uv_debug = make_uv_debug_image(
|
||||
uv_debug = benchmark_step(
|
||||
"make_uv_debug_image",
|
||||
make_uv_debug_image,
|
||||
data,
|
||||
out_width=args.out_width,
|
||||
out_height=args.out_height,
|
||||
@@ -703,7 +469,9 @@ def main():
|
||||
edge_band=args.edge_band,
|
||||
edge_expand=edge_expand,
|
||||
)
|
||||
uv_distortion = make_uv_distortion_image(
|
||||
uv_distortion = benchmark_step(
|
||||
"make_uv_distortion_image",
|
||||
make_uv_distortion_image,
|
||||
data,
|
||||
distortion,
|
||||
out_width=args.out_width,
|
||||
@@ -712,11 +480,14 @@ def main():
|
||||
edge_band=args.edge_band,
|
||||
edge_expand=edge_expand,
|
||||
)
|
||||
uv_reference = np.hstack(
|
||||
[
|
||||
resize_to_height(uv_debug, args.out_height),
|
||||
resize_to_height(uv_distortion, args.out_height),
|
||||
]
|
||||
uv_reference = benchmark_step(
|
||||
"uv_reference_compose",
|
||||
lambda: np.hstack(
|
||||
[
|
||||
resize_to_height(uv_debug, args.out_height),
|
||||
resize_to_height(uv_distortion, args.out_height),
|
||||
]
|
||||
),
|
||||
)
|
||||
|
||||
cap = open_source(args.video, args.camera_id)
|
||||
@@ -727,80 +498,150 @@ def main():
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, source_width)
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, source_height)
|
||||
|
||||
sensor = orisys.Sensor(
|
||||
"external",
|
||||
config_name=args.sdk_config,
|
||||
verbose=args.verbose,
|
||||
mirror=False,
|
||||
sensor = benchmark_step(
|
||||
"sensor_init",
|
||||
lambda: orisys.Sensor(
|
||||
"external",
|
||||
config_name=args.sdk_config,
|
||||
verbose=args.verbose,
|
||||
mirror=False,
|
||||
),
|
||||
)
|
||||
configure_masked_poisson(sensor, args)
|
||||
print(
|
||||
"Masked HHD solver: "
|
||||
f"{sensor.masked_poisson_solver} "
|
||||
f"(max_boundary={sensor.masked_poisson_max_boundary}, "
|
||||
f"stride={sensor.masked_poisson_boundary_stride}, "
|
||||
f"iters={sensor.masked_poisson_max_iters}, "
|
||||
f"tol={sensor.masked_poisson_tol:.1e})"
|
||||
)
|
||||
|
||||
print("Press 'q' to quit.")
|
||||
frame_processor = UVUnrollFrameProcessor(
|
||||
domain.remap,
|
||||
camera_matrix=source_K if dist_coeffs is not None else None,
|
||||
dist_coeffs=dist_coeffs,
|
||||
source_width=source_width,
|
||||
source_height=source_height,
|
||||
flip_source_vertical=flip_source_vertical,
|
||||
border_value=(0, 0, 0),
|
||||
)
|
||||
|
||||
ok, warmup_frame = cap.read()
|
||||
if ok:
|
||||
warmup_prepared = frame_processor.prepare_source(warmup_frame)
|
||||
warmup_unrolled = frame_processor.flatten_prepared(warmup_prepared)
|
||||
sensor.get_img(
|
||||
frame=warmup_unrolled,
|
||||
already_processed=True,
|
||||
valid_mask=frame_processor.valid_mask,
|
||||
)
|
||||
benchmark_step("hhd_warmup", sensor.warmup_deformation)
|
||||
else:
|
||||
print("[benchmark] hhd_warmup: skipped (no frame from source)")
|
||||
|
||||
if args.no_viz:
|
||||
print("Running with --no-viz (dashboard disabled).")
|
||||
else:
|
||||
print("Press 'q' to quit.")
|
||||
runtime_timer = StepTimer()
|
||||
try:
|
||||
while True:
|
||||
runtime_timer.start("capture")
|
||||
ok, frame = cap.read()
|
||||
runtime_timer.stop("capture")
|
||||
if not ok:
|
||||
break
|
||||
|
||||
if frame.shape[1] != source_width or frame.shape[0] != source_height:
|
||||
frame = cv2.resize(frame, (source_width, source_height), interpolation=cv2.INTER_LINEAR)
|
||||
runtime_timer.start("uv_prepare")
|
||||
frame_source = frame_processor.prepare_source(frame)
|
||||
runtime_timer.stop("uv_prepare")
|
||||
|
||||
if dist_coeffs is not None:
|
||||
frame_source = cv2.undistort(frame, source_K, dist_coeffs)
|
||||
else:
|
||||
frame_source = frame
|
||||
frame_source = align_frame_to_uv_map(frame_source, flip_vertical=flip_source_vertical)
|
||||
runtime_timer.start("uv_remap")
|
||||
unrolled = frame_processor.flatten_prepared(frame_source)
|
||||
runtime_timer.stop("uv_remap")
|
||||
|
||||
unrolled = flatten_frame(frame_source, remap, border_value=(0, 0, 0))
|
||||
runtime_timer.start("get_img")
|
||||
sensor.get_img(
|
||||
frame=unrolled,
|
||||
already_processed=True,
|
||||
valid_mask=frame_processor.valid_mask,
|
||||
)
|
||||
runtime_timer.stop("get_img")
|
||||
|
||||
sensor.get_img(frame=unrolled, already_processed=True)
|
||||
sensor.compute_deformation(check_motion=True, threshold=0)
|
||||
run_deformation_timed(sensor, runtime_timer, check_motion=True, threshold=0)
|
||||
|
||||
runtime_timer.start("contact")
|
||||
sensor.compute_contact()
|
||||
runtime_timer.stop("contact")
|
||||
|
||||
fps, flow, vnormal, vshear, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.VRAW,
|
||||
sensor.info.VNORMAL,
|
||||
sensor.info.VSHEAR,
|
||||
sensor.info.IMG,
|
||||
sensor.info.CONTOUR,
|
||||
sensor.info.CENTROID,
|
||||
sensor.info.DEPTH,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
fps = sensor._fps
|
||||
fnormal = sensor.fnormal
|
||||
fshearx = sensor.fshearx
|
||||
fsheary = sensor.fsheary
|
||||
contact_ok = contact_inside_mask(sensor.centroid, sensor.valid_mask)
|
||||
|
||||
arrows = orisys.util.draw_arrows(
|
||||
img,
|
||||
flow,
|
||||
threshold=2,
|
||||
grid_spacing=20,
|
||||
arrow_scale=1.0,
|
||||
)
|
||||
contact_img = orisys.util.draw_contact(img, contour, centroid)
|
||||
if not args.no_viz:
|
||||
runtime_timer.start("viz")
|
||||
fps, flow, vnormal, vshear, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.VRAW,
|
||||
sensor.info.VNORMAL,
|
||||
sensor.info.VSHEAR,
|
||||
sensor.info.IMG,
|
||||
sensor.info.CONTOUR,
|
||||
sensor.info.CENTROID,
|
||||
sensor.info.DEPTH,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
|
||||
depth_norm = depth_map
|
||||
if depth_norm.size > 0 and depth_norm.max() > depth_norm.min():
|
||||
depth_u8 = ((depth_norm - depth_norm.min()) / (depth_norm.max() - depth_norm.min()) * 255).astype(np.uint8)
|
||||
else:
|
||||
depth_u8 = np.zeros_like(depth_norm, dtype=np.uint8)
|
||||
depth_colored = cv2.applyColorMap(depth_u8, cv2.COLORMAP_JET)
|
||||
arrows = orisys.util.draw_arrows(
|
||||
img,
|
||||
flow,
|
||||
threshold=1,
|
||||
grid_spacing=15,
|
||||
arrow_scale=1.0,
|
||||
)
|
||||
contact_img = orisys.util.draw_contact(img, contour, centroid)
|
||||
|
||||
dashboard = make_tiled_dashboard(
|
||||
[
|
||||
("Original", frame_source),
|
||||
("UV Map | Distortion", uv_reference),
|
||||
("UV Unrolled", unrolled),
|
||||
("Flow", arrows),
|
||||
],
|
||||
tile_width=640,
|
||||
tile_height=360,
|
||||
)
|
||||
cv2.imshow("UV Unroll Dashboard", dashboard)
|
||||
depth_norm = depth_map
|
||||
if depth_norm.size > 0 and depth_norm.max() > depth_norm.min():
|
||||
depth_u8 = ((depth_norm - depth_norm.min()) / (depth_norm.max() - depth_norm.min()) * 255).astype(np.uint8)
|
||||
else:
|
||||
depth_u8 = np.zeros_like(depth_norm, dtype=np.uint8)
|
||||
depth_colored = cv2.applyColorMap(depth_u8, cv2.COLORMAP_JET)
|
||||
|
||||
display_mask = sensor.valid_mask
|
||||
arrows = apply_display_mask(arrows, display_mask, border_value=(0, 0, 0))
|
||||
contact_img = apply_display_mask(contact_img, display_mask, border_value=(0, 0, 0))
|
||||
depth_colored = apply_display_mask(depth_colored, display_mask, border_value=(0, 0, 0))
|
||||
arrows = overlay_runtime_status(arrows, sensor, centroid=centroid)
|
||||
contact_ok = contact_inside_mask(centroid, display_mask)
|
||||
|
||||
dashboard = make_tiled_dashboard(
|
||||
[
|
||||
("Original", frame_source),
|
||||
("UV Map | Distortion", uv_reference),
|
||||
("UV Unrolled", unrolled),
|
||||
("Flow", arrows),
|
||||
],
|
||||
tile_width=640,
|
||||
tile_height=360,
|
||||
)
|
||||
cv2.imshow("UV Unroll Dashboard", dashboard)
|
||||
runtime_timer.stop("viz")
|
||||
|
||||
print(
|
||||
f"FPS={fps:.2f}, 法向力={fnormal:.4f}, 切向力X={fshearx:.4f}, "
|
||||
f"切向力Y={fsheary:.4f}, 展开图尺寸={unrolled.shape[:2]}",
|
||||
f"切向力Y={fsheary:.4f}, 展开图尺寸={unrolled.shape[:2]}, "
|
||||
f"solver={sensor.masked_poisson_solver}, "
|
||||
f"mask={sensor.mask_stats['coverage']:.3f}, "
|
||||
f"flow(valid/invalid)=({sensor.mask_stats['flow_mean_valid']:.3f}/"
|
||||
f"{sensor.mask_stats['flow_mean_invalid']:.3f}), "
|
||||
f"contact_in_mask={contact_ok}, "
|
||||
f"{print_runtime_benchmark(runtime_timer)}",
|
||||
end="\r",
|
||||
)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user