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

update dd02 3d viewer v1

Browse Source
This commit is contained in:
ykcheungab
2026-06-02 12:15:36 +08:00
parent 7224fea411
commit 8a50492465
16 changed files with 2269 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/dd02_3dviewer/exported_data/camera_intrinsics.npz
BIN
View File
Binary file not shown.
tools/dd02_3dviewer/exported_data/mesh.npz
BIN
View File
Binary file not shown.
tools/dd02_3dviewer/exported_data/pixel_to_xyz.npz
BIN
View File
Binary file not shown.
tools/dd02_3dviewer/exported_data/world_to_cam.npy
BIN
View File
Binary file not shown.
tools/dd02_3dviewer/mapto3d.py
+188
View File
@@ -0,0 +1,188 @@
import numpy as np
import cv2
import os
def load_exported_data(data_dir):
"""Load all exported files."""
# Mesh
mesh = np.load(os.path.join(data_dir, "mesh.npz"))
vertices = mesh['vertices']
faces = mesh['faces']
uv_data = mesh['uv_data']
loop_to_vertex = mesh['loop_to_vertex']
# Camera intrinsics
cam_data = np.load(os.path.join(data_dir, "camera_intrinsics.npz"))
K = cam_data['K']
width = int(cam_data['width'])
height = int(cam_data['height'])
# Camera pose (world-to-camera)
world_to_cam = np.load(os.path.join(data_dir, "world_to_cam.npy"))
# Precomputed pixel-to-3D map (optional)
xyz_map_path = os.path.join(data_dir, "pixel_to_xyz.npz")
if os.path.exists(xyz_map_path):
xyz_map = np.load(xyz_map_path)['xyz_map']
else:
xyz_map = None
return {
'vertices': vertices,
'faces': faces,
'uv_data': uv_data,
'loop_to_vertex': loop_to_vertex,
'K': K,
'width': width,
'height': height,
'world_to_cam': world_to_cam,
'xyz_map': xyz_map
}
def ray_intersect_triangle(ray_origin, ray_dir, v0, v1, v2):
"""Möller–Trumbore intersection algorithm. Returns t (distance) or None."""
epsilon = 1e-8
edge1 = v1 - v0
edge2 = v2 - v0
h = np.cross(ray_dir, edge2)
a = np.dot(edge1, h)
if abs(a) < epsilon:
return None
f = 1.0 / a
s = ray_origin - v0
u = f * np.dot(s, h)
if u < 0.0 or u > 1.0:
return None
q = np.cross(s, edge1)
v = f * np.dot(ray_dir, q)
if v < 0.0 or u + v > 1.0:
return None
t = f * np.dot(edge2, q)
if t > epsilon:
return t
return None
def pixel_to_3d_ray(u, v, K, world_to_cam):
"""
Given pixel coordinates (u,v) and camera intrinsics K,
return a ray in world coordinates: origin and direction.
"""
# Invert K to get normalized image coordinates
fx = K[0,0]; fy = K[1,1]; cx = K[0,2]; cy = K[1,2]
x_norm = (u - cx) / fx
y_norm = (v - cy) / fy
# Ray in camera space: (x_norm, y_norm, 1.0)
ray_cam = np.array([x_norm, y_norm, 1.0], dtype=np.float64)
# Transform to world
cam_to_world = np.linalg.inv(world_to_cam)
R = cam_to_world[:3, :3]
t = cam_to_world[:3, 3]
ray_world_dir = R @ ray_cam
ray_world_dir /= np.linalg.norm(ray_world_dir)
ray_world_origin = t
return ray_world_origin, ray_world_dir
def map_image_to_3d(image_path, data, use_precomputed=True, flip_vertical=True):
"""
Map each pixel of the image to a 3D world point.
If use_precomputed and xyz_map exists, return that directly.
If `flip_vertical` is True, flip the loaded image top-to-bottom so its
row order matches the exported Blender camera/pixel mapping.
Otherwise, perform raycasting on the mesh.
Returns:
points: (H, W, 3) array of 3D points (NaN for misses).
colors: (H, W, 3) array of RGB colors from image.
"""
img = cv2.imread(image_path)
if img is None:
raise FileNotFoundError(f"Could not read image: {image_path}")
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) # to RGB
if flip_vertical:
img = np.flipud(img).copy()
h, w = img.shape[:2]
if use_precomputed and data['xyz_map'] is not None:
# Use precomputed map (must match resolution)
if h != data['height'] or w != data['width']:
raise ValueError("Image resolution does not match precomputed map.")
points = data['xyz_map'].copy()
colors = img.astype(np.float32) / 255.0
return points, colors
# Otherwise, perform raycasting on the mesh
vertices = data['vertices']
faces = data['faces']
K = data['K']
world_to_cam = data['world_to_cam']
points = np.full((h, w, 3), np.nan, dtype=np.float32)
colors = img.astype(np.float32) / 255.0
# For speed, we can use a bounding volume hierarchy (BVH).
# Here we implement a simple brute-force over all faces (slow for large meshes).
# In practice, precomputing the xyz_map is strongly recommended.
for y in range(h):
for x in range(w):
origin, direction = pixel_to_3d_ray(x, y, K, world_to_cam)
best_t = np.inf
best_hit = None
for face in faces:
v0 = vertices[face[0]]
v1 = vertices[face[1]]
v2 = vertices[face[2]]
t = ray_intersect_triangle(origin, direction, v0, v1, v2)
if t is not None and t < best_t:
best_t = t
best_hit = origin + direction * t
if best_hit is not None:
points[y, x] = best_hit
return points, colors
def save_colored_pointcloud(points, colors, output_path):
"""Save as PLY file (colored point cloud)."""
# Flatten valid points
mask = ~np.isnan(points[:,:,0])
valid_points = points[mask].reshape(-1, 3)
valid_colors = (colors[mask].reshape(-1, 3) * 255).astype(np.uint8)
with open(output_path, 'w') as f:
f.write("ply\n")
f.write("format ascii 1.0\n")
f.write(f"element vertex {len(valid_points)}\n")
f.write("property float x\n")
f.write("property float y\n")
f.write("property float z\n")
f.write("property uchar red\n")
f.write("property uchar green\n")
f.write("property uchar blue\n")
f.write("end_header\n")
for pt, col in zip(valid_points, valid_colors):
f.write(f"{pt[0]} {pt[1]} {pt[2]} {col[0]} {col[1]} {col[2]}\n")
print(f"Saved point cloud to {output_path}")
if __name__ == "__main__":
data_dir = "./exported_data" # Path to exported files
image_path = "captured_image_1.jpg" # Input camera image
output_ply = "mapped_points.ply"
data = load_exported_data(data_dir)
points, colors = map_image_to_3d(
image_path,
data,
use_precomputed=True,
flip_vertical=True,
)
save_colored_pointcloud(points, colors, output_ply)
# Optional: Visualize using Open3D
import open3d as o3d
pcd = o3d.geometry.PointCloud()
mask = ~np.isnan(points[:,:,0])
pcd.points = o3d.utility.Vector3dVector(points[mask].reshape(-1,3))
pcd.colors = o3d.utility.Vector3dVector(colors[mask].reshape(-1,3))
o3d.visualization.draw_geometries([pcd])
tools/dd02_3dviewer/sdk_mapto3d.py
+424
View File
@@ -0,0 +1,424 @@
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_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):
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):
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()
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_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)
valid_mask, display_indices, cloud_points = prepare_static_points(data, args.display_stride)
surface_normals = estimate_surface_normals(data["xyz_map"])
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
sensor.compute_deformation(decompose=(args.vector_mode == "shear-normal"), check_motion=False)
flow, img_bgr, vshear, vnormal = sensor.read_info(
sensor.info.VRAW,
sensor.info.IMG,
sensor.info.VSHEAR,
sensor.info.VNORMAL,
)
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.arrow_map_style:
cloud_colors = arrow_map_reference_colors(len(cloud_points))
else:
cloud_colors = align_image_to_map(
img_bgr,
data,
valid_mask,
display_indices,
flip_vertical=not args.no_flip_vertical,
)
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,
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 == "vraw-magnitude":
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(
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"))
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("--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("--vector-mode", choices=["vraw", "vraw-magnitude", "shear-normal"], 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")
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())