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

update 3d viewer to display deform instead of arrows

Browse Source
This commit is contained in:
ykcheungab
2026-06-02 21:12:17 +08:00
parent 8a50492465
commit 9fe93b23e4
26 changed files with 1314 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/dd02_3dviewer/exported_data/flatten_remap.npz
BIN
View File
Binary file not shown.
tools/dd02_3dviewer/sdk_mapto3d.py
+297 -97
View File
@@ -45,6 +45,12 @@ def load_exported_data(data_dir):
def parse_source(value):
value = str(value).strip()
for prefix in ("camera:", "cam:", "webcam:"):
if value.lower().startswith(prefix):
value = value[len(prefix):].strip()
break
try:
return int(value)
except ValueError:
@@ -53,13 +59,54 @@ def parse_source(value):
def prepare_static_points(data, display_stride):
xyz_map = data["xyz_map"]
height, width = xyz_map.shape[:2]
valid_mask = ~np.isnan(xyz_map[:, :, 0])
valid_points = xyz_map[valid_mask].reshape(-1, 3)
if len(valid_points) == 0:
raise ValueError("pixel_to_xyz.npz contains no valid mesh hit points.")
display_indices = np.arange(len(valid_points))[::max(1, int(display_stride))]
return valid_mask, display_indices, valid_points[display_indices]
flat_indices = np.flatnonzero(valid_mask.ravel())[display_indices]
display_ys = flat_indices // width
display_xs = flat_indices % width
return (
valid_mask,
display_indices,
valid_points[display_indices],
display_ys,
display_xs,
)
def mesh_pixel_to_world_scale(xyz_map, valid_mask):
valid_points = xyz_map[valid_mask]
if len(valid_points) == 0:
return 1e-3
extent = np.nanmax(valid_points, axis=0) - np.nanmin(valid_points, axis=0)
mesh_span = float(np.max(extent))
height, width = xyz_map.shape[:2]
return mesh_span / max(height, width, 1)
def sample_field_at_display_points(field, display_ys, display_xs):
if field.ndim == 2:
return field[display_ys, display_xs]
return field[display_ys, display_xs, :]
def deform_point_cloud(rest_points, displacement, pixel_to_world, deform_scale):
return rest_points + displacement * pixel_to_world * float(deform_scale)
def lift_flow_to_3d(flow, tangent_x, tangent_y, normals):
"""Lift raw 2D optical flow to tangent-plane 3D displacement."""
tx_norm = np.linalg.norm(tangent_x, axis=2, keepdims=True)
ty_norm = np.linalg.norm(tangent_y, axis=2, keepdims=True)
t1 = np.divide(tangent_x, tx_norm, out=np.zeros_like(tangent_x), where=tx_norm > 1e-8)
t2 = np.divide(tangent_y, ty_norm, out=np.zeros_like(tangent_y), where=ty_norm > 1e-8)
flow_3d = flow[:, :, 0:1] * t1 + flow[:, :, 1:2] * t2
normal_component = np.sum(flow_3d * normals, axis=2, keepdims=True)
return flow_3d - normal_component * normals
def align_image_to_map(img_bgr, data, valid_mask, display_indices, flip_vertical=True):
@@ -98,6 +145,47 @@ def magnitude_map_colors(flow, valid_mask, display_indices, max_magnitude=None):
).astype(np.float64)
def align_scalar_field_to_map(field, data, flip_vertical=True):
if field.ndim == 3:
field = np.linalg.norm(field, axis=2)
target_w = int(data["width"])
target_h = int(data["height"])
h, w = field.shape[:2]
if (w, h) != (target_w, target_h):
field = cv2.resize(field, (target_w, target_h), interpolation=cv2.INTER_LINEAR)
else:
field = field.copy()
if flip_vertical:
field = np.flipud(field).copy()
return field
def scalar_map_colors(field, valid_mask, display_indices, value_min=None, value_max=None):
valid_values = field[valid_mask].reshape(-1)
display_values = valid_values[display_indices]
finite_values = display_values[np.isfinite(display_values)]
if len(finite_values) == 0:
return arrow_map_reference_colors(len(display_indices))
if value_min is None:
value_min = float(np.percentile(finite_values, 5))
if value_max is None:
value_max = float(np.percentile(finite_values, 95))
if value_max <= value_min:
value_max = value_min + 1e-6
value = np.clip((display_values - value_min) / (value_max - value_min), 0.0, 1.0)
return np.column_stack(
(
value,
np.clip(1.0 - np.abs(value - 0.5) * 2.0, 0.0, 1.0),
1.0 - value,
)
).astype(np.float64)
def align_vector_field_to_map(field, data, flip_vertical=True, scale_as_flow=True):
target_w = int(data["width"])
target_h = int(data["height"])
@@ -131,6 +219,37 @@ def estimate_surface_normals(xyz_map):
return normals
def compute_surface_frames(xyz_map):
valid = ~np.isnan(xyz_map[:, :, 0])
filled = xyz_map.copy()
filled[~valid] = 0
d_y = np.gradient(filled, axis=0)
d_x = np.gradient(filled, axis=1)
normals = np.cross(d_x, d_y)
norm = np.linalg.norm(normals, axis=2, keepdims=True)
normals = np.divide(normals, norm, out=np.zeros_like(normals), where=norm > 1e-8)
d_x[~valid] = 0
d_y[~valid] = 0
normals[~valid] = 0
return d_x, d_y, normals, valid
def lift_deformation_to_3d(vshear, vnormal, tangent_x, tangent_y, normals, normal_gain=1.0):
# Unit tangents: map 2D pixel shear to 3D direction without world-scale Jacobian blow-up.
tx_norm = np.linalg.norm(tangent_x, axis=2, keepdims=True)
ty_norm = np.linalg.norm(tangent_y, axis=2, keepdims=True)
t1 = np.divide(tangent_x, tx_norm, out=np.zeros_like(tangent_x), where=tx_norm > 1e-8)
t2 = np.divide(tangent_y, ty_norm, out=np.zeros_like(tangent_y), where=ty_norm > 1e-8)
shear_3d = vshear[:, :, 0:1] * t1 + vshear[:, :, 1:2] * t2
normal_mag = np.linalg.norm(vnormal, axis=2, keepdims=True) * float(normal_gain)
normal_3d = normal_mag * normals
normal_component = np.sum(shear_3d * normals, axis=2, keepdims=True)
shear_tangent = shear_3d - normal_component * normals
return shear_tangent + normal_3d
def line_colors(mag, arrow_map_style):
if len(mag) == 0:
return np.empty((0, 3), dtype=np.float64)
@@ -141,13 +260,17 @@ def line_colors(mag, arrow_map_style):
return np.column_stack((norm_mag, 1.0 - norm_mag, np.zeros_like(norm_mag))).astype(np.float64)
def lines_from_vectors(starts, vectors, mag, flow_scale, arrow_map_style):
def lines_from_vectors(starts, vectors, mag, flow_scale, arrow_map_style, unit_direction=False):
vec_norm = np.linalg.norm(vectors, axis=1)
good = np.isfinite(vec_norm) & (vec_norm > 1e-8)
starts, vectors, mag = starts[good], vectors[good], mag[good]
if len(starts) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
if unit_direction:
vec_norm = vec_norm[:, None]
vectors = np.divide(vectors, vec_norm, out=np.zeros_like(vectors), where=vec_norm > 1e-8)
points = np.empty((len(starts) * 2, 3), dtype=np.float64)
points[0::2] = starts
points[1::2] = starts + vectors * flow_scale
@@ -189,81 +312,46 @@ def build_flow_lines(
return lines_from_vectors(starts, vectors, mag, flow_scale, arrow_map_style)
def build_shear_normal_lines(
def build_resultant_force_lines(
xyz_map,
valid_mask,
vshear,
vnormal,
normals,
resultant_force,
arrow_stride=32,
flow_scale=1.0,
min_magnitude=0.01,
normal_gain=1.0,
arrow_map_style=False,
):
h, w = vshear.shape[:2]
h, w = resultant_force.shape[:2]
ys, xs = np.mgrid[0:h:arrow_stride, 0:w:arrow_stride]
ys = ys.reshape(-1)
xs = xs.reshape(-1)
shear = vshear[ys, xs]
normal_field = vnormal[ys, xs]
shear_mag = np.linalg.norm(shear, axis=1)
normal_mag = np.linalg.norm(normal_field, axis=1) * normal_gain
combined_mag = shear_mag + normal_mag
keep = valid_mask[ys, xs] & (combined_mag >= min_magnitude)
xs, ys, shear, shear_mag, normal_mag, combined_mag = (
vectors = resultant_force[ys, xs]
mag = np.linalg.norm(vectors, axis=1)
keep = valid_mask[ys, xs] & (mag >= min_magnitude)
xs, ys, vectors, mag = (
xs[keep],
ys[keep],
shear[keep],
shear_mag[keep],
normal_mag[keep],
combined_mag[keep],
vectors[keep],
mag[keep],
)
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
tx = np.rint(xs + shear[:, 0]).astype(np.int32)
ty = np.rint(ys + shear[:, 1]).astype(np.int32)
inside = (0 <= tx) & (tx < w) & (0 <= ty) & (ty < h)
xs, ys, tx, ty, shear_mag, normal_mag, combined_mag = (
xs[inside],
ys[inside],
tx[inside],
ty[inside],
shear_mag[inside],
normal_mag[inside],
combined_mag[inside],
)
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
valid_target = valid_mask[ty, tx]
xs, ys, tx, ty, shear_mag, normal_mag, combined_mag = (
xs[valid_target],
ys[valid_target],
tx[valid_target],
ty[valid_target],
shear_mag[valid_target],
normal_mag[valid_target],
combined_mag[valid_target],
)
starts = xyz_map[ys, xs]
tangent = xyz_map[ty, tx] - starts
tangent_norm = np.linalg.norm(tangent, axis=1, keepdims=True)
tangent_unit = np.divide(tangent, tangent_norm, out=np.zeros_like(tangent), where=tangent_norm > 1e-8)
normal_unit = normals[ys, xs]
shear_ratio = shear_mag / np.maximum(combined_mag, 1e-8)
normal_ratio = normal_mag / np.maximum(combined_mag, 1e-8)
vectors = tangent_unit * shear_ratio[:, None] + normal_unit * normal_ratio[:, None]
vector_norm = np.linalg.norm(vectors, axis=1, keepdims=True)
vectors = np.divide(vectors, vector_norm, out=np.zeros_like(vectors), where=vector_norm > 1e-8)
return lines_from_vectors(starts, vectors, combined_mag, flow_scale, arrow_map_style)
return lines_from_vectors(
starts, vectors, mag, flow_scale, arrow_map_style, unit_direction=True
)
def init_viewer(cloud_points, cloud_colors, flow_points, flow_lines, flow_colors, arrow_map_style=False):
def init_viewer(
cloud_points,
cloud_colors,
flow_points,
flow_lines,
flow_colors,
arrow_map_style=False,
show_arrows=True,
):
import open3d as o3d
vis = o3d.visualization.Visualizer()
@@ -274,11 +362,13 @@ def init_viewer(cloud_points, cloud_colors, flow_points, flow_lines, flow_colors
pcd.colors = o3d.utility.Vector3dVector(cloud_colors.astype(np.float64))
vis.add_geometry(pcd)
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
vis.add_geometry(line_set)
line_set = None
if show_arrows:
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
vis.add_geometry(line_set)
render_option = vis.get_render_option()
render_option.point_size = 1.0 if arrow_map_style else 2.0
@@ -291,8 +381,13 @@ def init_viewer(cloud_points, cloud_colors, flow_points, flow_lines, flow_colors
def run_sdk_3d_flow(args):
data = load_exported_data(args.data_dir)
valid_mask, display_indices, cloud_points = prepare_static_points(data, args.display_stride)
surface_normals = estimate_surface_normals(data["xyz_map"])
valid_mask, display_indices, rest_cloud_points, display_ys, display_xs = prepare_static_points(
data, args.display_stride
)
tangent_x, tangent_y, surface_normals, valid_mask = compute_surface_frames(data["xyz_map"])
pixel_to_world = mesh_pixel_to_world_scale(data["xyz_map"], valid_mask)
show_arrows = args.display_mode == "arrows"
deform_cloud = args.display_mode == "deform"
sensor = orisys.Sensor(
parse_source(args.source),
@@ -314,19 +409,32 @@ def run_sdk_3d_flow(args):
if sensor.get_img() is None:
break
sensor.compute_deformation(decompose=(args.vector_mode == "shear-normal"), check_motion=False)
flow, img_bgr, vshear, vnormal = sensor.read_info(
needs_decomposition = args.vector_mode in ("shear-normal", "depth-map")
sensor.compute_deformation(decompose=needs_decomposition, check_motion=False)
if args.vector_mode == "depth-map":
sensor.compute_contact()
flow, img_bgr, vshear, vnormal, depth_map = sensor.read_info(
sensor.info.VRAW,
sensor.info.IMG,
sensor.info.VSHEAR,
sensor.info.VNORMAL,
sensor.info.DEPTH,
)
flow_map = align_vector_field_to_map(flow, data, flip_vertical=not args.no_flip_vertical)
if args.vector_mode == "vraw-magnitude":
cloud_colors = magnitude_map_colors(flow_map, valid_mask, display_indices, args.magnitude_max)
elif args.vector_mode == "depth-map":
depth_map = align_scalar_field_to_map(depth_map, data, flip_vertical=not args.no_flip_vertical)
cloud_colors = scalar_map_colors(
depth_map,
valid_mask,
display_indices,
value_min=args.depth_min,
value_max=args.depth_max,
)
elif args.arrow_map_style:
cloud_colors = arrow_map_reference_colors(len(cloud_points))
cloud_colors = arrow_map_reference_colors(len(rest_cloud_points))
else:
cloud_colors = align_image_to_map(
img_bgr,
@@ -336,35 +444,81 @@ def run_sdk_3d_flow(args):
flip_vertical=not args.no_flip_vertical,
)
flip_vertical = not args.no_flip_vertical
displacement_map = None
if args.vector_mode == "shear-normal":
vshear_map = align_vector_field_to_map(vshear, data, flip_vertical=not args.no_flip_vertical)
vnormal_map = align_vector_field_to_map(vnormal, data, flip_vertical=not args.no_flip_vertical)
flow_points, flow_lines, flow_colors = build_shear_normal_lines(
data["xyz_map"],
valid_mask,
vshear_map = align_vector_field_to_map(vshear, data, flip_vertical=flip_vertical)
vnormal_map = align_vector_field_to_map(vnormal, data, flip_vertical=flip_vertical)
displacement_map = lift_deformation_to_3d(
vshear_map,
vnormal_map,
tangent_x,
tangent_y,
surface_normals,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
normal_gain=args.normal_gain,
arrow_map_style=args.arrow_map_style,
)
elif args.vector_mode == "vraw-magnitude":
elif args.vector_mode == "depth-map" and deform_cloud:
vnormal_map = align_vector_field_to_map(vnormal, data, flip_vertical=flip_vertical)
zero_shear = np.zeros_like(vnormal_map)
displacement_map = lift_deformation_to_3d(
zero_shear,
vnormal_map,
tangent_x,
tangent_y,
surface_normals,
normal_gain=args.normal_gain,
)
elif args.vector_mode in ("vraw", "vraw-magnitude"):
displacement_map = lift_flow_to_3d(flow_map, tangent_x, tangent_y, surface_normals)
if deform_cloud and args.deform_color == "magnitude" and displacement_map is not None:
cloud_colors = magnitude_map_colors(
displacement_map, valid_mask, display_indices, args.magnitude_max
)
if deform_cloud:
if displacement_map is None:
raise ValueError(
f"display-mode deform is not supported with vector-mode {args.vector_mode!r}"
)
point_displacement = sample_field_at_display_points(
displacement_map, display_ys, display_xs
)
cloud_points = deform_point_cloud(
rest_cloud_points,
point_displacement,
pixel_to_world,
args.deform_scale,
)
flow_points = np.empty((0, 3), dtype=np.float64)
flow_lines = np.empty((0, 2), dtype=np.int32)
flow_colors = np.empty((0, 3), dtype=np.float64)
else:
flow_points, flow_lines, flow_colors = build_flow_lines(
data["xyz_map"],
valid_mask,
flow_map,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
arrow_map_style=args.arrow_map_style,
)
cloud_points = rest_cloud_points
if args.vector_mode == "shear-normal":
flow_points, flow_lines, flow_colors = build_resultant_force_lines(
data["xyz_map"],
valid_mask,
displacement_map,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
arrow_map_style=args.arrow_map_style,
)
elif args.vector_mode in ("vraw-magnitude", "depth-map"):
flow_points = np.empty((0, 3), dtype=np.float64)
flow_lines = np.empty((0, 2), dtype=np.int32)
flow_colors = np.empty((0, 3), dtype=np.float64)
else:
flow_points, flow_lines, flow_colors = build_flow_lines(
data["xyz_map"],
valid_mask,
flow_map,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
arrow_map_style=args.arrow_map_style,
)
if vis is None:
vis, pcd, line_set, o3d = init_viewer(
@@ -374,21 +528,26 @@ def run_sdk_3d_flow(args):
flow_lines,
flow_colors,
arrow_map_style=args.arrow_map_style,
show_arrows=show_arrows,
)
print("SDK 3D flow viewer started. Close the Open3D window to stop.")
mode_label = "deformed cloud" if deform_cloud else "arrows"
print(f"SDK 3D flow viewer started ({mode_label}). Close the Open3D window to stop.")
else:
pcd.points = o3d.utility.Vector3dVector(cloud_points.astype(np.float64))
pcd.colors = o3d.utility.Vector3dVector(cloud_colors)
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
if line_set is not None:
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
vis.update_geometry(line_set)
vis.update_geometry(pcd)
vis.update_geometry(line_set)
vis.update_renderer()
frame_count += 1
if frame_count % 30 == 0:
elapsed = max(time.perf_counter() - t0, 1e-6)
print(f"frames={frame_count}, viewer FPS={frame_count / elapsed:.1f}, arrows={len(flow_lines)}")
arrow_count = len(flow_lines) if show_arrows else 0
print(f"frames={frame_count}, viewer FPS={frame_count / elapsed:.1f}, arrows={arrow_count}")
finally:
sensor.disconnect()
if vis is not None:
@@ -398,16 +557,57 @@ def run_sdk_3d_flow(args):
def parse_args():
parser = argparse.ArgumentParser(description="Map SDK flow fields to a 3D point cloud.")
parser.add_argument("--data-dir", default=os.path.join(SCRIPT_DIR, "exported_data"))
parser.add_argument("--source", default=os.path.join(SCRIPT_DIR, "20260602-094916.mp4"))
parser.add_argument(
"--source",
default=os.path.join(SCRIPT_DIR, "20260602-094916.mp4"),
help="Video path or live camera ID, for example 0, 1, camera:0, or cam:1.",
)
parser.add_argument("--config", default=os.path.join(REPO_ROOT, "config", "dd02-ov.json"))
parser.add_argument("--display-stride", type=int, default=4)
parser.add_argument("--arrow-stride", type=int, default=8)
parser.add_argument("--flow-scale", type=float, default=3.0)
parser.add_argument("--min-flow", type=float, default=0.5)
parser.add_argument(
"--flow-scale",
type=float,
default=3.0,
help="Arrow length in 3D viewer units (shear-normal uses unit direction; magnitude shown by color).",
)
parser.add_argument(
"--min-flow",
type=float,
default=0.5,
help="Minimum 2D-equivalent resultant magnitude before drawing an arrow.",
)
parser.add_argument("--normal-gain", type=float, default=1.0)
parser.add_argument(
"--display-mode",
choices=["arrows", "deform"],
default="arrows",
help="arrows: line vectors; deform: displace the point cloud by the 3D deformation field.",
)
parser.add_argument(
"--deform-scale",
type=float,
default=1.0,
help="Gain on cloud displacement (multiplied by mesh pixel-to-world scale).",
)
parser.add_argument(
"--deform-color",
choices=["texture", "magnitude"],
default="texture",
help="Point colors in deform mode: camera texture or deformation magnitude heat-map.",
)
parser.add_argument("--magnitude-max", type=float, default=None,
help="Upper VRAW magnitude for heat-map color scaling; default uses frame percentile.")
parser.add_argument("--vector-mode", choices=["vraw", "vraw-magnitude", "shear-normal"], default="vraw")
parser.add_argument("--depth-min", type=float, default=None,
help="Minimum depth value for heat-map color scaling; default uses frame percentile.")
parser.add_argument("--depth-max", type=float, default=None,
help="Maximum depth value for heat-map color scaling; default uses frame percentile.")
parser.add_argument(
"--vector-mode",
choices=["vraw", "vraw-magnitude", "shear-normal", "depth-map"],
default="vraw",
help="Vector display mode. shear-normal shows 3D resultant vectors (surface-tangent shear + surface-normal component).",
)
parser.add_argument(
"--arrow-map-style",
action="store_true",
tools/dd02_3dviewer/sdk_mapto3d_flatten.py
+562
View File
@@ -0,0 +1,562 @@
import argparse
import os
import sys
import time
import cv2
import numpy as np
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
REPO_ROOT = os.path.abspath(os.path.join(SCRIPT_DIR, "..", ".."))
SRC_DIR = os.path.join(REPO_ROOT, "src")
if SRC_DIR not in sys.path:
sys.path.insert(0, SRC_DIR)
import orisys
def load_flatten_remap(path):
remap = np.load(path)
result = {
"map_x": remap["map_x"].astype(np.float32),
"map_y": remap["map_y"].astype(np.float32),
"valid_mask": remap["valid_mask"].astype(bool),
}
if "flat_xyz" in remap:
result["flat_xyz"] = remap["flat_xyz"].astype(np.float32)
return result
def flip_sensor_frames_vertical(sensor):
"""Flip captured BGR/gray frames to match Blender-exported map orientation."""
if sensor._img_bgr_raw is not None:
sensor._img_bgr_raw = cv2.flip(sensor._img_bgr_raw, 0)
if sensor.img is not None:
cv2.flip(sensor.img, 0, dst=sensor.img)
def flatten_frame(frame, remap, border_value=0):
flat = cv2.remap(
frame,
remap["map_x"],
remap["map_y"],
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
borderValue=border_value,
)
flat = flat.copy()
flat[~remap["valid_mask"]] = border_value
return flat
def load_exported_data(data_dir):
mesh = np.load(os.path.join(data_dir, "mesh.npz"))
cam_data = np.load(os.path.join(data_dir, "camera_intrinsics.npz"))
xyz_map_path = os.path.join(data_dir, "pixel_to_xyz.npz")
if not os.path.exists(xyz_map_path):
raise FileNotFoundError(f"Missing precomputed 3D map: {xyz_map_path}")
xyz_map = np.load(xyz_map_path)["xyz_map"]
width = int(cam_data["width"])
height = int(cam_data["height"])
if xyz_map.shape[:2] != (height, width):
raise ValueError(
f"pixel_to_xyz shape {xyz_map.shape[:2]} does not match camera size {(height, width)}"
)
return {
"vertices": mesh["vertices"],
"faces": mesh["faces"],
"uv_data": mesh["uv_data"],
"loop_to_vertex": mesh["loop_to_vertex"],
"K": cam_data["K"],
"width": width,
"height": height,
"world_to_cam": np.load(os.path.join(data_dir, "world_to_cam.npy")),
"xyz_map": xyz_map,
}
def parse_source(value):
value = str(value).strip()
for prefix in ("camera:", "cam:", "webcam:"):
if value.lower().startswith(prefix):
value = value[len(prefix):].strip()
break
try:
return int(value)
except ValueError:
return value
def prepare_static_points(data, display_stride):
xyz_map = data["xyz_map"]
valid_mask = ~np.isnan(xyz_map[:, :, 0])
valid_points = xyz_map[valid_mask].reshape(-1, 3)
if len(valid_points) == 0:
raise ValueError("pixel_to_xyz.npz contains no valid mesh hit points.")
display_indices = np.arange(len(valid_points))[::max(1, int(display_stride))]
return valid_mask, display_indices, valid_points[display_indices]
def align_image_to_map(img_bgr, data, valid_mask, display_indices, flip_vertical=True, flip_horizontal=True):
target_w = int(data["width"])
target_h = int(data["height"])
h, w = img_bgr.shape[:2]
if (w, h) != (target_w, target_h):
img_bgr = cv2.resize(img_bgr, (target_w, target_h), interpolation=cv2.INTER_LINEAR)
img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)
if flip_vertical:
img_rgb = np.flipud(img_rgb).copy()
if flip_horizontal:
img_rgb = np.fliplr(img_rgb).copy()
colors = img_rgb.astype(np.float64) / 255.0
return colors[valid_mask].reshape(-1, 3)[display_indices]
def arrow_map_reference_colors(count, color=(0.04, 0.04, 0.04)):
return np.tile(np.asarray(color, dtype=np.float64), (count, 1))
def magnitude_map_colors(flow, valid_mask, display_indices, max_magnitude=None):
mag = np.linalg.norm(flow, axis=2)
valid_mag = mag[valid_mask].reshape(-1)
display_mag = valid_mag[display_indices]
if max_magnitude is None or max_magnitude <= 0:
max_magnitude = max(float(np.percentile(display_mag, 95)), 1e-6)
value = np.clip(display_mag / max_magnitude, 0.0, 1.0)
return np.column_stack(
(
value,
np.clip(1.0 - np.abs(value - 0.5) * 2.0, 0.0, 1.0),
1.0 - value,
)
).astype(np.float64)
def align_scalar_field_to_map(field, data, flip_vertical=True):
if field.ndim == 3:
field = np.linalg.norm(field, axis=2)
target_w = int(data["width"])
target_h = int(data["height"])
h, w = field.shape[:2]
if (w, h) != (target_w, target_h):
field = cv2.resize(field, (target_w, target_h), interpolation=cv2.INTER_LINEAR)
else:
field = field.copy()
if flip_vertical:
field = np.flipud(field).copy()
return field
def scalar_map_colors(field, valid_mask, display_indices, value_min=None, value_max=None):
valid_values = field[valid_mask].reshape(-1)
display_values = valid_values[display_indices]
finite_values = display_values[np.isfinite(display_values)]
if len(finite_values) == 0:
return arrow_map_reference_colors(len(display_indices))
if value_min is None:
value_min = float(np.percentile(finite_values, 5))
if value_max is None:
value_max = float(np.percentile(finite_values, 95))
if value_max <= value_min:
value_max = value_min + 1e-6
value = np.clip((display_values - value_min) / (value_max - value_min), 0.0, 1.0)
return np.column_stack(
(
value,
np.clip(1.0 - np.abs(value - 0.5) * 2.0, 0.0, 1.0),
1.0 - value,
)
).astype(np.float64)
def align_vector_field_to_map(field, data, flip_vertical=True, scale_as_flow=True):
target_w = int(data["width"])
target_h = int(data["height"])
h, w = field.shape[:2]
if (w, h) != (target_w, target_h):
sx = target_w / max(w, 1)
sy = target_h / max(h, 1)
field = cv2.resize(field, (target_w, target_h), interpolation=cv2.INTER_LINEAR)
if scale_as_flow:
field[:, :, 0] *= sx
field[:, :, 1] *= sy
else:
field = field.copy()
if flip_vertical:
field = np.flipud(field).copy()
field[:, :, 1] *= -1.0
return field
def estimate_surface_normals(xyz_map):
valid = ~np.isnan(xyz_map[:, :, 0])
filled = xyz_map.copy()
filled[~valid] = 0
d_y = np.gradient(filled, axis=0)
d_x = np.gradient(filled, axis=1)
normals = np.cross(d_x, d_y)
norm = np.linalg.norm(normals, axis=2, keepdims=True)
normals = np.divide(normals, norm, out=np.zeros_like(normals), where=norm > 1e-8)
normals[~valid] = 0
return normals
def line_colors(mag, arrow_map_style):
if len(mag) == 0:
return np.empty((0, 3), dtype=np.float64)
if arrow_map_style:
return np.tile(np.array([0.0, 0.9, 1.0], dtype=np.float64), (len(mag), 1))
norm_mag = mag / max(float(np.percentile(mag, 95)), 1e-6)
norm_mag = np.clip(norm_mag, 0.0, 1.0)
return np.column_stack((norm_mag, 1.0 - norm_mag, np.zeros_like(norm_mag))).astype(np.float64)
def lines_from_vectors(starts, vectors, mag, flow_scale, arrow_map_style):
vec_norm = np.linalg.norm(vectors, axis=1)
good = np.isfinite(vec_norm) & (vec_norm > 1e-8)
starts, vectors, mag = starts[good], vectors[good], mag[good]
if len(starts) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
points = np.empty((len(starts) * 2, 3), dtype=np.float64)
points[0::2] = starts
points[1::2] = starts + vectors * flow_scale
lines = np.column_stack((np.arange(0, len(points), 2), np.arange(1, len(points), 2))).astype(np.int32)
return points, lines, line_colors(mag, arrow_map_style)
def build_flow_lines(
xyz_map,
valid_mask,
flow,
arrow_stride=32,
flow_scale=1.0,
min_magnitude=0.5,
arrow_map_style=False,
):
h, w = flow.shape[:2]
ys, xs = np.mgrid[0:h:arrow_stride, 0:w:arrow_stride]
ys = ys.reshape(-1)
xs = xs.reshape(-1)
sample_flow = flow[ys, xs]
mag = np.linalg.norm(sample_flow, axis=1)
keep = valid_mask[ys, xs] & (mag >= min_magnitude)
xs, ys, sample_flow, mag = xs[keep], ys[keep], sample_flow[keep], mag[keep]
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
tx = np.rint(xs + sample_flow[:, 0]).astype(np.int32)
ty = np.rint(ys + sample_flow[:, 1]).astype(np.int32)
inside = (0 <= tx) & (tx < w) & (0 <= ty) & (ty < h)
xs, ys, tx, ty, mag = xs[inside], ys[inside], tx[inside], ty[inside], mag[inside]
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
valid_target = valid_mask[ty, tx]
xs, ys, tx, ty, mag = xs[valid_target], ys[valid_target], tx[valid_target], ty[valid_target], mag[valid_target]
starts = xyz_map[ys, xs]
vectors = xyz_map[ty, tx] - starts
return lines_from_vectors(starts, vectors, mag, flow_scale, arrow_map_style)
def build_shear_normal_lines(
xyz_map,
valid_mask,
vshear,
vnormal,
normals,
arrow_stride=32,
flow_scale=1.0,
min_magnitude=0.01,
normal_gain=1.0,
arrow_map_style=False,
):
h, w = vshear.shape[:2]
ys, xs = np.mgrid[0:h:arrow_stride, 0:w:arrow_stride]
ys = ys.reshape(-1)
xs = xs.reshape(-1)
shear = vshear[ys, xs]
normal_field = vnormal[ys, xs]
shear_mag = np.linalg.norm(shear, axis=1)
normal_mag = np.linalg.norm(normal_field, axis=1) * normal_gain
combined_mag = shear_mag + normal_mag
keep = valid_mask[ys, xs] & (combined_mag >= min_magnitude)
xs, ys, shear, shear_mag, normal_mag, combined_mag = (
xs[keep],
ys[keep],
shear[keep],
shear_mag[keep],
normal_mag[keep],
combined_mag[keep],
)
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
tx = np.rint(xs + shear[:, 0]).astype(np.int32)
ty = np.rint(ys + shear[:, 1]).astype(np.int32)
inside = (0 <= tx) & (tx < w) & (0 <= ty) & (ty < h)
xs, ys, tx, ty, shear_mag, normal_mag, combined_mag = (
xs[inside],
ys[inside],
tx[inside],
ty[inside],
shear_mag[inside],
normal_mag[inside],
combined_mag[inside],
)
if len(xs) == 0:
return np.empty((0, 3)), np.empty((0, 2), dtype=np.int32), np.empty((0, 3))
valid_target = valid_mask[ty, tx]
xs, ys, tx, ty, shear_mag, normal_mag, combined_mag = (
xs[valid_target],
ys[valid_target],
tx[valid_target],
ty[valid_target],
shear_mag[valid_target],
normal_mag[valid_target],
combined_mag[valid_target],
)
starts = xyz_map[ys, xs]
tangent = xyz_map[ty, tx] - starts
tangent_norm = np.linalg.norm(tangent, axis=1, keepdims=True)
tangent_unit = np.divide(tangent, tangent_norm, out=np.zeros_like(tangent), where=tangent_norm > 1e-8)
normal_unit = normals[ys, xs]
shear_ratio = shear_mag / np.maximum(combined_mag, 1e-8)
normal_ratio = normal_mag / np.maximum(combined_mag, 1e-8)
vectors = tangent_unit * shear_ratio[:, None] + normal_unit * normal_ratio[:, None]
vector_norm = np.linalg.norm(vectors, axis=1, keepdims=True)
vectors = np.divide(vectors, vector_norm, out=np.zeros_like(vectors), where=vector_norm > 1e-8)
return lines_from_vectors(starts, vectors, combined_mag, flow_scale, arrow_map_style)
def init_viewer(cloud_points, cloud_colors, flow_points, flow_lines, flow_colors, arrow_map_style=False):
import open3d as o3d
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="SDK 3D Flow", width=1280, height=720)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(cloud_points.astype(np.float64))
pcd.colors = o3d.utility.Vector3dVector(cloud_colors.astype(np.float64))
vis.add_geometry(pcd)
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
vis.add_geometry(line_set)
render_option = vis.get_render_option()
render_option.point_size = 1.0 if arrow_map_style else 2.0
render_option.line_width = 2.0
render_option.background_color = np.array([0.0, 0.0, 0.0])
vis.poll_events()
vis.update_renderer()
return vis, pcd, line_set, o3d
def run_sdk_3d_flow(args):
data = load_exported_data(args.data_dir)
flatten_remap = load_flatten_remap(args.flatten_remap) if args.flatten_remap else None
view_data = data
if flatten_remap is not None:
if "flat_xyz" not in flatten_remap:
raise ValueError("Flatten remap must contain flat_xyz for 3D visualization.")
view_data = dict(data)
view_data["xyz_map"] = flatten_remap["flat_xyz"]
view_data["height"], view_data["width"] = view_data["xyz_map"].shape[:2]
valid_mask, display_indices, cloud_points = prepare_static_points(view_data, args.display_stride)
surface_normals = estimate_surface_normals(view_data["xyz_map"])
flip_vertical = not args.no_flip_vertical
sensor = orisys.Sensor(
parse_source(args.source),
config_name=args.config,
isstitch=False,
cuda=args.cuda,
verbose=args.verbose,
mirror=not args.no_mirror,
)
vis = pcd = line_set = o3d = None
frame_count = 0
t0 = time.perf_counter()
try:
while True:
if vis is not None and not vis.poll_events():
break
if sensor.get_img() is None:
break
if flip_vertical:
flip_sensor_frames_vertical(sensor)
flat_bgr = None
if flatten_remap is not None:
flat_bgr = flatten_frame(sensor.read_info(sensor.info.IMG)[0], flatten_remap)
flat_gray = cv2.cvtColor(flat_bgr, cv2.COLOR_BGR2GRAY)
sensor.img = flat_gray
sensor._img_bgr_raw = flat_bgr
sensor.img_size_raw = flat_bgr.shape
needs_decomposition = args.vector_mode in ("shear-normal", "depth-map")
sensor.compute_deformation(decompose=needs_decomposition, check_motion=False)
if args.vector_mode == "depth-map":
sensor.compute_contact()
flow, img_bgr, vshear, vnormal, depth_map = sensor.read_info(
sensor.info.VRAW,
sensor.info.IMG,
sensor.info.VSHEAR,
sensor.info.VNORMAL,
sensor.info.DEPTH,
)
if flat_bgr is not None:
img_bgr = flat_bgr
active_data = view_data if flatten_remap is not None else data
# Input is already flipped above; flattened domain uses flat_xyz directly.
flip_for_map = False if (flatten_remap is not None or flip_vertical) else True
flow_map = align_vector_field_to_map(flow, active_data, flip_vertical=flip_for_map)
if args.vector_mode in ("vraw-magnitude", "vraw-magnitude-arrows"):
cloud_colors = magnitude_map_colors(flow_map, valid_mask, display_indices, args.magnitude_max)
elif args.vector_mode == "depth-map":
depth_map = align_scalar_field_to_map(depth_map, active_data, flip_vertical=flip_for_map)
cloud_colors = scalar_map_colors(
depth_map,
valid_mask,
display_indices,
value_min=args.depth_min,
value_max=args.depth_max,
)
elif args.arrow_map_style:
cloud_colors = arrow_map_reference_colors(len(cloud_points))
else:
cloud_colors = align_image_to_map(
img_bgr,
active_data,
valid_mask,
display_indices,
flip_vertical=flip_for_map,
)
if args.vector_mode == "shear-normal":
vshear_map = align_vector_field_to_map(vshear, active_data, flip_vertical=flip_for_map)
vnormal_map = align_vector_field_to_map(vnormal, active_data, flip_vertical=flip_for_map)
flow_points, flow_lines, flow_colors = build_shear_normal_lines(
active_data["xyz_map"],
valid_mask,
vshear_map,
vnormal_map,
surface_normals,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
normal_gain=args.normal_gain,
arrow_map_style=args.arrow_map_style,
)
elif args.vector_mode in ("vraw-magnitude", "depth-map"):
flow_points = np.empty((0, 3), dtype=np.float64)
flow_lines = np.empty((0, 2), dtype=np.int32)
flow_colors = np.empty((0, 3), dtype=np.float64)
else:
flow_points, flow_lines, flow_colors = build_flow_lines(
active_data["xyz_map"],
valid_mask,
flow_map,
arrow_stride=args.arrow_stride,
flow_scale=args.flow_scale,
min_magnitude=args.min_flow,
arrow_map_style=args.arrow_map_style,
)
if vis is None:
vis, pcd, line_set, o3d = init_viewer(
cloud_points,
cloud_colors,
flow_points,
flow_lines,
flow_colors,
arrow_map_style=args.arrow_map_style,
)
print("SDK 3D flow viewer started. Close the Open3D window to stop.")
else:
pcd.colors = o3d.utility.Vector3dVector(cloud_colors)
line_set.points = o3d.utility.Vector3dVector(flow_points.astype(np.float64))
line_set.lines = o3d.utility.Vector2iVector(flow_lines)
line_set.colors = o3d.utility.Vector3dVector(flow_colors.astype(np.float64))
vis.update_geometry(pcd)
vis.update_geometry(line_set)
vis.update_renderer()
frame_count += 1
if frame_count % 30 == 0:
elapsed = max(time.perf_counter() - t0, 1e-6)
print(f"frames={frame_count}, viewer FPS={frame_count / elapsed:.1f}, arrows={len(flow_lines)}")
finally:
sensor.disconnect()
if vis is not None:
vis.destroy_window()
def parse_args():
parser = argparse.ArgumentParser(description="Map SDK flow fields to a 3D point cloud.")
parser.add_argument("--data-dir", default=os.path.join(SCRIPT_DIR, "exported_data"))
parser.add_argument(
"--source",
default=os.path.join(SCRIPT_DIR, "20260602-094916.mp4"),
help="Video path or live camera ID, for example 0, 1, camera:0, or cam:1.",
)
parser.add_argument("--config", default=os.path.join(REPO_ROOT, "config", "dd02-ov.json"))
parser.add_argument("--display-stride", type=int, default=4)
parser.add_argument("--arrow-stride", type=int, default=32)
parser.add_argument("--flow-scale", type=float, default=3.0)
parser.add_argument("--min-flow", type=float, default=0.5)
parser.add_argument("--normal-gain", type=float, default=1.0)
parser.add_argument("--magnitude-max", type=float, default=None,
help="Upper VRAW magnitude for heat-map color scaling; default uses frame percentile.")
parser.add_argument("--depth-min", type=float, default=None,
help="Minimum depth value for heat-map color scaling; default uses frame percentile.")
parser.add_argument("--depth-max", type=float, default=None,
help="Maximum depth value for heat-map color scaling; default uses frame percentile.")
parser.add_argument("--flatten-remap", default=None,
help="Optional flatten_remap.npz to run optical flow in UV-flattened image space.")
parser.add_argument(
"--vector-mode",
choices=["vraw", "vraw-magnitude", "vraw-magnitude-arrows", "shear-normal", "depth-map"],
default="vraw",
)
parser.add_argument(
"--arrow-map-style",
action="store_true",
help="Hide real texture and use a dark Qt-viewer-like arrow map style.",
)
parser.add_argument(
"--no-flip-vertical",
action="store_true",
help="Do not flip captured video vertically before processing.",
)
parser.add_argument("--no-mirror", action="store_true")
parser.add_argument("--cuda", action="store_true")
parser.add_argument("--verbose", action="store_true")
return parser.parse_args()
if __name__ == "__main__":
run_sdk_3d_flow(parse_args())
tools/dd02_3dviewer/texture/patch_texture_0525_N120.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.2 MiB

tools/dd02_3dviewer/texture/patch_texture_0525_N60.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 795 KiB

tools/dd02_3dviewer/texture/patch_texture_0525_N75.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 898 KiB

tools/dd02_3dviewer/texture/patch_texture_0525_N90.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1002 KiB