diff --git a/config/dd02-ov.json b/config/dd02-ov.json new file mode 100644 index 0000000..2a0c695 --- /dev/null +++ b/config/dd02-ov.json @@ -0,0 +1,29 @@ +{ + "name": "dd02-ov", + "type": "single_camera", + "version": "1.0", + "camera": { + "resolution_raw": [1280, 720], + "fps": 30, + "resolution_stitch": [1280, 720] + }, + "tracker": { + "adaptive": true, + "lower_threshold": 12.0, + "upper_threshold": 18.0, + "loop_closure_interval": 1, + "algorithm": "dis", + "dis_preset": "medium", + "dis_mode": 3, + "algorithm_list": ["raft", "dis"], + "hhd_downsample": 3 + }, + "contact": { + "min_contour_area": 3000, + "flow_threshold": 5, + "div_threshold": 0.05, + "expansion_threshold": 0.01, + "kernel_size": [5, 5], + "denoise_sigma": 12 + } +} diff --git a/scripts/qt_viewer_2.py b/scripts/qt_viewer_2.py new file mode 100644 index 0000000..8ad37e3 --- /dev/null +++ b/scripts/qt_viewer_2.py @@ -0,0 +1,830 @@ +""" +Orisys SDK 实时监控 — Qt 统一界面 + +将所有可视化和曲线整合到一个窗口中，适合销售演示使用。 +SDK version: 0.3.1 +""" + +import os +import sys +os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" +import cv2 +def _fix_qt_plugin_path(): + """OpenCV redirects Qt to cv2/qt/plugins; use PyQt5's plugins instead.""" + try: + from PyQt5.QtCore import QLibraryInfo + os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location( + QLibraryInfo.PluginsPath + ) + except Exception: + path = os.environ.get("QT_QPA_PLATFORM_PLUGIN_PATH", "") + if "cv2" in path.replace("\\", "/"): + os.environ.pop("QT_QPA_PLATFORM_PLUGIN_PATH", None) +_fix_qt_plugin_path() +import orisys +import numpy as np +import time +import argparse +from datetime import datetime +from scipy.signal import butter, filtfilt + +# Force curve sampling rate (Hz), matches typical sensor loop ~30 FPS. +FORCE_SAMPLE_RATE_HZ = 30.0 +PLOT_BUFFER_POINTS = 120 +PLOT_WINDOW_SEC = PLOT_BUFFER_POINTS / FORCE_SAMPLE_RATE_HZ # 4 s window +# Minimum y-axis span so autoscale does not over-zoom on small fluctuations. +Y_AXIS_MIN_SPAN = 20000 +CURVE_THEME_COLOR = '#00E5FF' + +from PyQt5.QtWidgets import ( + QMainWindow, QWidget, QHBoxLayout, QVBoxLayout, + QLabel, QApplication, QSplitter, QPlainTextEdit, + QPushButton, QFileDialog, QLineEdit, +) +from PyQt5.QtCore import Qt, QTimer +from PyQt5 import QtGui +from PyQt5.QtGui import QImage, QPixmap, QFont, QIcon + +try: + import pyqtgraph as pg +except Exception: + print("PyQtGraph is not installed. Please install it using 'pip install pyqt5 pyqtgraph'.") + sys.exit(1) + +# --------------------------------------------------------------------------- +# 工具 +# --------------------------------------------------------------------------- + +def cv2_to_pixmap(img_bgr: np.ndarray) -> QPixmap: + rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) + h, w, ch = rgb.shape + qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888) + return QPixmap.fromImage(qimg) + +def draw_magnitude_map(vfield, threshold=3, colormap=cv2.COLORMAP_JET) -> np.ndarray: + flow_field = vfield + magnitude = np.sqrt(flow_field[:, :, 0] ** 2 + flow_field[:, :, 1] ** 2) + + downsample_factor = 10 + h, w = magnitude.shape + magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor] + magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC) + + magnitude_thresholded = magnitude_smoothed.copy() + magnitude_thresholded[magnitude_smoothed < threshold] = 0 + + mag_min, mag_max = magnitude_thresholded.min(), magnitude_thresholded.max() + if mag_max > mag_min: + magnitude_norm = ((magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255).astype(np.uint8) + else: + magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8) + + magnitude_colored = cv2.applyColorMap(magnitude_norm, colormap) + out_img = cv2.resize(magnitude_colored, (400, 400), interpolation=cv2.INTER_CUBIC) + return out_img + + +def lowpass_filter(data, cutoff_hz, sample_rate_hz=FORCE_SAMPLE_RATE_HZ, order=2): + """ + Zero-phase Butterworth low-pass filter for uniformly sampled 1-D data. + + Parameters + ---------- + data : array-like + Input samples acquired at `sample_rate_hz` (default 30 Hz). + cutoff_hz : float + -3 dB cutoff frequency in Hz; use 0 to skip filtering (return raw data). + Otherwise must satisfy 0 < cutoff_hz < sample_rate_hz / 2. + sample_rate_hz : float + Sampling rate of `data` in Hz. + order : int + Butterworth filter order. + + Returns + ------- + np.ndarray + Filtered 1-D array with the same length as `data`. + """ + y = np.asarray(data, dtype=np.float64) + if cutoff_hz == 0: + return y.copy() + if y.size == 0: + return y + + nyquist = 0.5 * sample_rate_hz + if cutoff_hz < 0 or cutoff_hz >= nyquist: + raise ValueError( + f"cutoff_hz must be in (0, {nyquist}) for sample_rate_hz={sample_rate_hz}" + ) + + b, a = butter(order, cutoff_hz / nyquist, btype="low") + padlen = 3 * (max(len(b), len(a)) - 1) + if y.size <= padlen: + return y.copy() + + return filtfilt(b, a, y) + + +class TeeStream: + """同时写入多个流""" + def __init__(self, *streams): + self.streams = streams + + def write(self, text): + for s in self.streams: + s.write(text) + + def flush(self): + for s in self.streams: + s.flush() + + +class WidgetLogHandler: + """将 print 输出实时显示到 QPlainTextEdit""" + def __init__(self, widget: QPlainTextEdit): + self.widget = widget + self.buf = "" + + def write(self, text: str): + self.buf += text + while "\n" in self.buf: + line, self.buf = self.buf.split("\n", 1) + if line: + self.widget.appendPlainText(line) + + def flush(self): + if self.buf: + self.widget.appendPlainText(self.buf) + self.buf = "" + +# --------------------------------------------------------------------------- +# 样式表 +# --------------------------------------------------------------------------- + +STYLE = """ +QMainWindow { + background-color: #000000; +} + +/* ---- 图像面板 ---- */ +QLabel#arrowsLabel, QLabel#magLabel { + border: 1px solid #262626; + border-radius: 4px; + background-color: #0b0b0b; +} + +/* ---- 顶栏按钮 & 输入 - — */ +QPushButton { + background-color: #1a1a1a; + color: #999999; + border: 1px solid #333333; + border-radius: 4px; + padding: 6px 16px; + font-family: "SimHei"; + font-size: 13px; +} +QPushButton:hover { + background-color: #262626; + color: #cccccc; + border: 1px solid #555555; +} +QPushButton#btnStart { + background-color: #0d2818; + color: #4caf84; + border: 1px solid #1a4d30; +} +QPushButton#btnStart:hover { + background-color: #143d24; + color: #6fd4a0; +} +QPushButton#btnStart:disabled { + background-color: #0f0f0f; + color: #333333; + border: 1px solid #222222; +} +QPushButton#btnStop { + background-color: #2b1010; + color: #e05555; + border: 1px solid #4d1a1a; +} +QPushButton#btnStop:hover { + background-color: #3d1818; + color: #f07070; +} +QPushButton#btnStop:disabled { + background-color: #0f0f0f; + color: #333333; + border: 1px solid #222222; +} +QLineEdit { + background-color: #0d0d0d; + color: #999999; + border: 1px solid #333333; + border-radius: 4px; + padding: 5px 10px; + font-size: 13px; +} + +/* ---- 力曲线容器 ---- */ +GraphicsLayoutWidget { + border: 1px solid #262626; + border-radius: 4px; +} + +/* ---- 日志面板 ---- */ +QPlainTextEdit { + background-color: #0b0b0b; + color: #777777; + border: 1px solid #262626; + border-radius: 4px; + font-family: "Cascadia Code", "Consolas", "Courier New", monospace; + font-size: 13px; + padding: 8px; + selection-background-color: #1a3a4a; + selection-color: #cccccc; +} + +QPlainTextEdit QScrollBar:vertical { + background-color: #0b0b0b; + width: 6px; + margin: 1px; + border-radius: 3px; +} +QPlainTextEdit QScrollBar::handle:vertical { + background-color: #262626; + border-radius: 3px; + min-height: 30px; +} +QPlainTextEdit QScrollBar::handle:vertical:hover { + background-color: #3a3a3a; +} +QPlainTextEdit QScrollBar::add-line:vertical, +QPlainTextEdit QScrollBar::sub-line:vertical { + height: 0px; +} +QPlainTextEdit QScrollBar::add-page:vertical, +QPlainTextEdit QScrollBar::sub-page:vertical { + background: none; +} + +QSplitter::handle { + background-color: #1a1a1a; +} +QSplitter::handle:vertical { + height: 1px; +} +QSplitter::handle:horizontal { + width: 1px; +} +""" + +# --------------------------------------------------------------------------- +# 主窗口 +# --------------------------------------------------------------------------- + +class MainWindow(QMainWindow): + def __init__(self, args, log_file): + super().__init__() + self.setWindowTitle("Orisys SDK") + self.setStyleSheet(STYLE) + + logo_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "logo.png") + if os.path.exists(logo_path): + self.setWindowIcon(QIcon(logo_path)) + + self.sensor = None + self.start_time = None + self.args = args + + # 录像状态 + self.is_save = False + self.frame_count = 0 + self.writer = None + self.save_path = None + + # 曲线数据缓冲（先填满 npoints 再滚动；120 点 @ 30 Hz = 4 s） + self.npoints = PLOT_BUFFER_POINTS + self.lowpass_cutoff_hz = getattr(args, "cutoff", 5.0) + self._plot_sample_count = 0 + self._init_plot_buffer() + + self._setup_ui() + self._create_plots() + + # 同步参数到 UI + self.camera_edit.setText(args.video) + self.config_edit.setText(args.config) + self.cal_edit.setText(args.cal) + + # stdout → 日志文件 + 界面日志面板 + self.log_handler = WidgetLogHandler(self.log_widget) + sys.stdout = TeeStream(log_file, self.log_handler) + sys.stderr = sys.stdout + + self._y_range_alpha = 0.5 + + self.timer = QTimer() + self.timer.timeout.connect(self._on_timer) + self._running = False + + def _init_plot_buffer(self): + """Empty buffer; x-axis grows until npoints real samples, then scrolls.""" + self.data = np.full((self.npoints, 2, 3), np.nan, dtype=np.float64) + self._plot_sample_count = 0 + self._y_range_smooth = [None, None, None] + + # ---- UI 构建 ---- + + def _setup_ui(self): + central = QWidget() + self.setCentralWidget(central) + root_layout = QVBoxLayout(central) + root_layout.setContentsMargins(6, 6, 6, 6) + root_layout.setSpacing(6) + + # ---- 顶栏 ---- + bar = QHBoxLayout() + bar.setSpacing(8) + + self.btn_camera = QPushButton("摄像机") + self.btn_camera.setObjectName("btnCamera") + self.btn_camera.clicked.connect(self._select_camera) + bar.addWidget(self.btn_camera) + self.camera_edit = QLineEdit("1") + self.camera_edit.setPlaceholderText("摄像头编号 或 视频路径") + bar.addWidget(self.camera_edit, 1) + + self.btn_config = QPushButton("配置") + self.btn_config.setObjectName("btnConfig") + self.btn_config.clicked.connect(self._select_config) + bar.addWidget(self.btn_config) + self.config_edit = QLineEdit("./config/ddjx01.json") + bar.addWidget(self.config_edit, 1) + + self.btn_reset = QPushButton("重置") + self.btn_reset.setObjectName("btnReset") + self.btn_reset.setEnabled(False) + self.btn_reset.clicked.connect(self._on_reset) + bar.addWidget(self.btn_reset) + self.cal_edit = QLineEdit("./config/ddjx01.npy") + bar.addWidget(self.cal_edit, 1) + + self.btn_start = QPushButton("启动") + self.btn_start.setObjectName("btnStart") + self.btn_start.clicked.connect(self._on_start) + bar.addWidget(self.btn_start) + + self.btn_stop = QPushButton("暂停") + self.btn_stop.setObjectName("btnStop") + self.btn_stop.setEnabled(False) + self.btn_stop.clicked.connect(self._on_stop) + bar.addWidget(self.btn_stop) + + root_layout.addLayout(bar) + + # ---- 外层纵向 splitter：主区域 / 日志 ---- + v_splitter = QSplitter(Qt.Vertical) + + # ---- 主区域：图像 (左) | 曲线 (右) ---- + h_splitter = QSplitter(Qt.Horizontal) + + # 左侧图像面板 + left = QWidget() + left_layout = QVBoxLayout(left) + left_layout.setContentsMargins(0, 0, 0, 0) + left_layout.setSpacing(6) + + self.arrows_label = QLabel() + self.arrows_label.setObjectName("arrowsLabel") + self.arrows_label.setMinimumSize(160, 160) + self.arrows_label.setScaledContents(True) + self.arrows_label.setAlignment(Qt.AlignCenter) + left_layout.addWidget(self.arrows_label) + + self.mag_label = QLabel() + self.mag_label.setObjectName("magLabel") + self.mag_label.setMinimumSize(160, 160) + self.mag_label.setScaledContents(True) + self.mag_label.setAlignment(Qt.AlignCenter) + left_layout.addWidget(self.mag_label) + + h_splitter.addWidget(left) + + # 右侧力曲线 + pg.setConfigOption('background', '#000000') + pg.setConfigOption('foreground', '#777777') + pg.setConfigOptions(antialias=True) + self.graph_widget = pg.GraphicsLayoutWidget() + self.graph_widget.setMinimumWidth(300) + h_splitter.addWidget(self.graph_widget) + + self.h_splitter = h_splitter + + v_splitter.addWidget(h_splitter) + + # ---- 底部日志面板 ---- + self.log_widget = QPlainTextEdit() + self.log_widget.setReadOnly(True) + self.log_widget.setMaximumBlockCount(2000) + font = QFont("Consolas", 12) + font.setStyleHint(QFont.Monospace) + self.log_widget.setFont(font) + v_splitter.addWidget(self.log_widget) + + v_splitter.setStretchFactor(0, 4) + v_splitter.setStretchFactor(1, 1) + + root_layout.addWidget(v_splitter) + + def _create_plots(self): + titles = ["法向力", "切向力 X", "切向力 Y"] + self.plots = [] + self.curves = [] + self.dot_glows = [] + self.dot_cores = [] + + axis_pen = pg.mkPen(color=(38, 38, 38), width=1) # #262626 + label_font = QFont("SimHei", 9) + + for i, title in enumerate(titles): + p = self.graph_widget.addPlot(title=None) + + # 标题：黑体 + p.setTitle(title, color='#999999', size='14pt') + p.titleLabel.setFont(QFont("SimHei", 14)) + + # X 轴标签 + p.setLabel('bottom', '时间', units='s') + p.getAxis('bottom').label.setFont(label_font) + + # 仅保留左 + 下轴线，隐藏右 + 上边框 + p.showAxis('top', False) + p.showAxis('right', False) + p.getAxis('left').setPen(axis_pen) + p.getAxis('bottom').setPen(axis_pen) + + # 仅横向网格，极淡 + p.showGrid(x=False, y=True, alpha=0.12) + + # 等宽刻度字体 + tick_font = QFont("Consolas", 11) + tick_font.setStyleHint(QFont.Monospace) + p.getAxis('left').setTickFont(tick_font) + p.getAxis('bottom').setTickFont(tick_font) + + # 曲线: 青色描边 + 向下渐变填充（与 demo_wuxi 一致） + line_pen = pg.mkPen(color=CURVE_THEME_COLOR, width=2) + # grad_brush = make_gradient_brush(CURVE_THEME_COLOR, max_alpha=120) + curve = p.plot(pen=line_pen, fillLevel=0) + + # 终点光晕: #00e5ff 30% 透明，半径 8 + dot_glow = p.plot([], [], pen=None, symbol='o', symbolSize=8, + symbolPen=None, symbolBrush=(0, 229, 255, 76)) + # 终点核心: 纯白，半径 4 + dot_core = p.plot([], [], pen=None, symbol='o', symbolSize=4, + symbolPen=None, symbolBrush=(255, 255, 255, 255)) + + p.setXRange(0, PLOT_WINDOW_SEC, padding=0) + p.enableAutoRange(x=False, y=False) + + self.plots.append(p) + self.curves.append(curve) + self.dot_glows.append(dot_glow) + self.dot_cores.append(dot_core) + + if i < 2: + self.graph_widget.nextRow() + + def adjust_square_images(self): + """根据窗口高度调整左侧面板宽度，使两张图保持正方形""" + top_bar_h = 48 + log_h = self.log_widget.height() + spacing = 20 + available = self.height() - top_bar_h - log_h - spacing + each_h = max(available // 2, 160) + self.h_splitter.setSizes([each_h, self.width() - each_h]) + + # ---- 帧处理 ---- + + def _on_timer(self): + if self.sensor is None: + return + sensor = self.sensor + + isimg = sensor.get_img() + if isimg is None: + print("Failed to get image") + return + + # 旋转原始图像，使力方向与实际物理方向对齐 + sensor.img = cv2.rotate(sensor.img, cv2.ROTATE_90_COUNTERCLOCKWISE) + if sensor.frame is not None: + sensor.frame = cv2.rotate(sensor.frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + + # 录像写入 + if self.is_save and self.writer is not None and sensor.frame is not None: + self.frame_count += 1 + save_img = sensor.frame + if len(save_img.shape) == 2 or (len(save_img.shape) == 3 and save_img.shape[2] == 1): + if len(save_img.shape) == 3: + save_img = save_img.squeeze(2) + save_img = cv2.cvtColor(save_img, cv2.COLOR_GRAY2BGR) + self.writer.write(save_img) + + is_touched = sensor.compute_deformation(check_motion=True, threshold=1) + sensor.compute_contact() + + fps, fn, fx, fy, flow, vnormal, vshear, depth_map = sensor.read_info( + sensor.info.FPS, sensor.info.FNORMAL, + sensor.info.FSHEARX, sensor.info.FSHEARY, + sensor.info.VRAW, sensor.info.VNORMAL, + sensor.info.VSHEAR, + sensor.info.DEPTH + ) + + + print(f"FPS={fps:.2f}, 法向力={fn:.4f}, 切向力X={fx:.4f}, 切向力Y={fy:.4f}") + # print(f"CUPY Available: {sensor.CUPY_AVAILABLE}") + + # 图像 + _bg_bgr = (22, 16, 11) + _img = sensor.img + _h, _w = _img.shape[:2] + black_img = np.empty((_h, _w, 3), dtype=_img.dtype) + black_img[:, :, 0] = _bg_bgr[0] + black_img[:, :, 1] = _bg_bgr[1] + black_img[:, :, 2] = _bg_bgr[2] + + arrows = orisys.util.draw_arrows( + black_img, flow, + threshold=2, grid_spacing=20, + arrow_scale=0.5, + below_threshold_color=(160, 160, 160), + ) + arrows = cv2.resize(arrows, (400, 400), interpolation=cv2.INTER_CUBIC) + self.arrows_label.setPixmap(cv2_to_pixmap(arrows)) + + # 显示深度图 + div_abs = depth_map + if div_abs.max() > div_abs.min(): + div_normalized = ((div_abs - div_abs.min()) / (div_abs.max() - div_abs.min()) * 255).astype(np.uint8) + else: + div_normalized = np.zeros_like(div_abs, dtype=np.uint8) + + # 应用颜色映射（使用JET：蓝色=低散度，红色=高散度） + depth_colored = cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET) + + # mag = draw_magnitude_map(flow) + self.mag_label.setPixmap(cv2_to_pixmap(depth_colored)) + + # 曲线：先顺序写入 npoints 个真实样本，满后再左移滚动 + current_time = time.time() - self.start_time + if self._plot_sample_count < self.npoints: + idx = self._plot_sample_count + self.data[idx, 0, :] = current_time + self.data[idx, 1, 0] = fn + self.data[idx, 1, 1] = fx + self.data[idx, 1, 2] = fy + self._plot_sample_count += 1 + else: + for i in range(3): + self.data[:-1, 0, i] = self.data[1:, 0, i] + self.data[:-1, 1, i] = self.data[1:, 1, i] + self.data[-1, 0, :] = current_time + self.data[-1, 1, 0] = fn + self.data[-1, 1, 1] = fx + self.data[-1, 1, 2] = fy + + n = self._plot_sample_count + if n == 0: + return + + data_plot = self.data[:n].copy() + for i in range(3): + data_plot[:, 1, i] = lowpass_filter( + data_plot[:, 1, i], + self.lowpass_cutoff_hz, + sample_rate_hz=FORCE_SAMPLE_RATE_HZ, + ) + + for i, curve in enumerate(self.curves): + x = data_plot[:, 0, i] + y = data_plot[:, 1, i] + curve.setData(x, y) + ymin, ymax = y.min(), y.max() + if self._y_range_smooth[i] is None: + self._y_range_smooth[i] = (ymin, ymax) + else: + prev_min, prev_max = self._y_range_smooth[i] + alpha = self._y_range_alpha + ymin = alpha * ymin + (1 - alpha) * prev_min + ymax = alpha * ymax + (1 - alpha) * prev_max + span = ymax - ymin + if span < Y_AXIS_MIN_SPAN: + mid = (ymin + ymax) / 2 + half = Y_AXIS_MIN_SPAN / 2 + ymin = mid - half + ymax = mid + half + self._y_range_smooth[i] = (ymin, ymax) + pad = max((ymax - ymin) * 0.12, 0.01) + self.plots[i].setYRange(ymin - pad, ymax + pad) + last_x = x[-1:] + last_y = y[-1:] + self.dot_glows[i].setData(last_x, last_y) + self.dot_cores[i].setData(last_x, last_y) + + if n < self.npoints: + for p in self.plots: + p.setXRange(0, PLOT_WINDOW_SEC, padding=0) + else: + x0 = float(data_plot[0, 0, 0]) + x1 = float(data_plot[-1, 0, 0]) + if x1 <= x0: + x1 = x0 + PLOT_WINDOW_SEC + for p in self.plots: + p.setXRange(x0, x1, padding=0) + + # ---- 键盘 & 录像 ---- + + def _on_start(self): + if self._running: + return + self.btn_start.setText("启动中...") + self.btn_start.setEnabled(False) + QApplication.processEvents() + + try: + src = self.camera_edit.text() + cfg = self.config_edit.text() + cal = self.cal_edit.text() + verbose = getattr(self.args, 'verbose', True) + try: + src = int(src) + except ValueError: + pass + self.sensor = orisys.Sensor(src, config_name=cfg, cal_path=cal, verbose=verbose) + except Exception as e: + self.btn_start.setText("启动") + self.btn_start.setEnabled(True) + print(f"启动失败: {e}") + return + + self.start_time = time.time() + self._init_plot_buffer() + self._running = True + self.timer.start(10) + self.btn_start.setText("启动") + self.btn_stop.setEnabled(True) + self.btn_reset.setEnabled(True) + print(f"▶ 启动 — 摄像机={src}, 配置={cfg}, 标定={cal}") + + def _on_stop(self): + if not self._running: + return + self._running = False + self.timer.stop() + self.btn_stop.setText("停止中...") + self.btn_stop.setEnabled(False) + QApplication.processEvents() + + if self.sensor is not None: + self.sensor.disconnect() + self.sensor = None + + self.btn_start.setEnabled(True) + self.btn_stop.setText("暂停") + self.btn_reset.setEnabled(False) + print("⏸ 已暂停") + + def _select_camera(self): + text = self.camera_edit.text() + path, _ = QFileDialog.getOpenFileName( + self, "选择视频文件", "data", "视频 (*.mp4 *.avi *.mov);;所有文件 (*)", + ) + if path: + self.camera_edit.setText(path) + + def _select_config(self): + path, _ = QFileDialog.getOpenFileName( + self, "选择配置文件", "config", "JSON (*.json);;所有文件 (*)", + ) + if path: + self.config_edit.setText(path) + + def _on_reset(self): + """Reset optical flow tracker when drift occurs (same as basic_pipeline 'r' key).""" + if self.sensor is None or not self._running: + print("请先启动传感器后再重置追踪器。") + return + self.sensor.reset() + print("追踪器已重置。") + + def keyPressEvent(self, event): + key = event.key() + if key == Qt.Key_Escape: + if self.isFullScreen(): + self.showNormal() + else: + self.showFullScreen() + elif key == Qt.Key_Q: + print("\n正在退出示例程序...") + self.close() + elif key == Qt.Key_S: + self._start_recording() + elif key == Qt.Key_E: + self._stop_recording() + elif key == Qt.Key_R: + self._on_reset() + else: + super().keyPressEvent(event) + + def _start_recording(self): + if self.sensor is None: + print("请先启动采集") + return + self.save_path = "./data/pulse_test.mp4" + os.makedirs(os.path.dirname(self.save_path), exist_ok=True) + + save_img = self.sensor.frame + if save_img is None: + print("当前没有可用帧，无法确定录像尺寸。") + return + + temp_img = save_img.copy() + if len(temp_img.shape) == 2 or (len(temp_img.shape) == 3 and temp_img.shape[2] == 1): + if len(temp_img.shape) == 3: + temp_img = temp_img.squeeze(2) + temp_img = cv2.cvtColor(temp_img, cv2.COLOR_GRAY2BGR) + + fh, fw = temp_img.shape[:2] + self.writer = cv2.VideoWriter( + self.save_path, cv2.VideoWriter_fourcc(*'mp4v'), 30, (fw, fh), + ) + if self.writer.isOpened(): + self.is_save = True + print(f"开始录像：{self.save_path}") + else: + print(f"无法创建录像文件：{self.save_path}") + self.writer = None + + def _stop_recording(self): + self.is_save = False + if self.writer is not None: + self.writer.release() + self.writer = None + if self.save_path is not None: + print(f"录像结束：{self.save_path}，总帧数：{self.frame_count}") + else: + print(f"录像结束，总帧数：{self.frame_count}") + self.frame_count = 0 + + def closeEvent(self, event): + self.timer.stop() + if self.writer is not None: + self.writer.release() + if self.sensor is not None: + self.sensor.disconnect() + event.accept() + +# --------------------------------------------------------------------------- +# 入口 +# --------------------------------------------------------------------------- + +def main(): + script_dir = os.path.dirname(os.path.abspath(__file__)) + log_dir = os.path.join(script_dir, "logs") + os.makedirs(log_dir, exist_ok=True) + log_path = os.path.join(log_dir, f"qt_viewer_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log") + log_file = open(log_path, 'w', encoding='utf-8') + # stdout 到文件的临时重定向（创建 MainWindow 后会加上日志面板） + sys.stdout = log_file + sys.stderr = sys.stdout + + parser = argparse.ArgumentParser() + parser.add_argument("--video", "-v", type=str, default="0") + parser.add_argument("--config", "-c", type=str, default="./config/ddjx01.json") + parser.add_argument("--cal", "-cal", type=str, default="./config/ddjx01.npy") + parser.add_argument("--verbose", "-verbose", type=bool, default=True) + parser.add_argument( + "--cutoff", + type=float, + default=10.0, + help="Low-pass cutoff for force curves (Hz), sample rate 30 Hz", + ) + args = parser.parse_args() + + print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") + + # 高 DPI 适配（笔记本高分屏） + QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) + QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) + app = QApplication(sys.argv) + window = MainWindow(args, log_file) + window.showFullScreen() + QApplication.processEvents() + window.adjust_square_images() + + sys.exit(app.exec_()) + +if __name__ == '__main__': + main() diff --git a/src/orisys/__init__.py b/src/orisys/__init__.py index 829f606..d5e2e87 100644 --- a/src/orisys/__init__.py +++ b/src/orisys/__init__.py @@ -12,6 +12,9 @@ from .sensor import Sensor from . import util from . import configs +CompoundEyeSensor = Sensor +CameraSensor = Sensor + # Re-export list_configs as list_config for convenience list_config = configs.list_config export_config = configs.export_config @@ -19,6 +22,8 @@ export_config = configs.export_config __all__ = [ # class 'Sensor', + 'CompoundEyeSensor', + 'CameraSensor', # util functions 'util', diff --git a/src/orisys/frame_processors.py b/src/orisys/frame_processors.py new file mode 100644 index 0000000..f3e646f --- /dev/null +++ b/src/orisys/frame_processors.py @@ -0,0 +1,36 @@ +import cv2 +import numpy as np + + +class CompoundEyeFrameProcessor: + """Convert a raw compound-eye frame into one stitched BGR tactile image.""" + + def __init__(self, stitcher): + self.stitcher = stitcher + + def process(self, frame): + self.stitcher.get_stitch_unit(frame) + self.stitcher.extract_tiles() + return self.stitcher.stitch_rt_optimized(blend=False) + + +class SingleCameraFrameProcessor: + """Convert a normal camera frame into one BGR tactile image.""" + + def process(self, frame): + if frame.ndim == 2: + return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) + if frame.shape[2] == 1: + return cv2.cvtColor(frame[:, :, 0], cv2.COLOR_GRAY2BGR) + return frame + + +def create_frame_processor(sensor_type, stitcher=None): + sensor_type = (sensor_type or "compound_eye").lower() + if sensor_type in ("compound_eye", "compound-eye", "compound"): + if stitcher is None: + raise ValueError("Compound-eye sensor requires a Stitcher instance.") + return CompoundEyeFrameProcessor(stitcher) + if sensor_type in ("single_camera", "camera", "mono"): + return SingleCameraFrameProcessor() + raise ValueError(f"Unsupported sensor type: {sensor_type}") diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index dce081d..83eede0 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -7,7 +7,6 @@ and Helmholtz-Hodge decomposition. """ import os -import platform import numpy as np import cv2 import time @@ -23,10 +22,7 @@ from .hhd import nHHD from . import configs from .get_depth import Contact3D from .backend_availability import (check_cupy_available, check_opencl_available) -try: - import av # Optional: used for Linux camera capture path. -except ImportError: - av = None +from .frame_processors import create_frame_processor class Sensor: """ @@ -149,7 +145,12 @@ class Sensor: } # Initialize components - self.stitcher = Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose) + self.stitcher = ( + Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose) + if self._is_compound_eye() + else None + ) + self.frame_processor = create_frame_processor(self.sensor_type, stitcher=self.stitcher) self.__tracker = Tracker(adaptive=True, cuda=cuda, verbose=self.verbose, config_tracker=self.__config_tracker) self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose) self.__hhd = None @@ -167,7 +168,6 @@ class Sensor: self._av_stream = None self._av_decoder = None self.cap = None - # Treat both camera index (0) and V4L2 path ("/dev/video0") as live camera for threading/low-latency path. self._is_live_camera = ( isinstance(self.vid_src, int) or (isinstance(self.vid_src, str) and self.vid_src.startswith("/dev/video")) @@ -219,10 +219,9 @@ class Sensor: t_processing_start = time.perf_counter() self.frame = frame - self._extract_tiles(frame) + img_bgr = self.frame_processor.process(frame) t_after_extract = time.perf_counter() - img_bgr = self.stitcher.stitch_rt_optimized(blend=False) t_after_stitch = time.perf_counter() self.img_size_raw = img_bgr.shape @@ -538,22 +537,29 @@ class Sensor: def _load_config(self): """Load sensor configuration from JSON file.""" self._config = configs.load_config(self.config_name) + self.sensor_type = self._config.get( + "type", + "compound_eye" if "stitcher" in self._config else "single_camera", + ) self.__config_cam = self._config["camera"] self._config_resolution_raw = self.__config_cam["resolution_raw"] self._config_fps = self.__config_cam["fps"] - self._config_units = self.__config_cam["units"] + self._config_units = self.__config_cam.get("units", 1) self._config_resolution_stitch = self.__config_cam["resolution_stitch"] self._config_dis_preset = self._config["tracker"]["dis_preset"] self.__config_tracker = self._config["tracker"] - self.__config_stitcher = self._config["stitcher"] + self.__config_stitcher = self._config.get("stitcher") self.__config_contact = self._config["contact"] if self.verbose: print(f"Config: {self.config_name}") return True + def _is_compound_eye(self): + return str(self.sensor_type).lower() in ("compound_eye", "compound-eye", "compound") + def _open_video_source(self, override, box): """Initialize video capture and calibration.""" # _is_camera_index = isinstance(self.vid_src, int) @@ -589,16 +595,11 @@ class Sensor: self.use_pyav = False if not self._use_pyav: self.cap = cv2.VideoCapture(self.vid_src) - # Set buffer size to 1 to reduce latency and avoid frame accumulation - # Note: Not all backends support this, but it helps when available - # try: - # self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) - # except Exception: - # pass # Some backends don't support this property - self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G')) - self.cap.set(cv2.CAP_PROP_FPS, self._config_fps) - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0]) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1]) + if self._is_live_camera: + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G')) + self.cap.set(cv2.CAP_PROP_FPS, self._config_fps) + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0]) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1]) if self.verbose: @@ -633,7 +634,10 @@ class Sensor: if not ret: time.sleep(0.1) - if override: + if not self._is_compound_eye(): + self.stitch_valid = False + self.initial_img = self._make_initial_gray(frame) + elif override: if box is not None: self.stitcher.isstitch = self.isstitch = False self.stitcher.boxes = box @@ -641,24 +645,23 @@ class Sensor: print("Override=True but no box provided.") else: self._find_stitch_boxes(frame) - self._extract_tiles(frame) - # Feature-based stitching no longer needs eager overlap calibration here. - # Keep stitch_cal() as a fallback so older overlap-based stitching still works. - img = self.stitcher.stitch_rt_optimized() - # if img is None: - # img = self.stitch_cal() - img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - self.resolution_stitch = img.shape - self.initial_img = cv2.resize(img, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC) - if self.mirror: - self.initial_img = cv2.flip(self.initial_img, 1) - self.initial_img = self.initial_img.astype(np.uint8) + img = self.frame_processor.process(frame) + self.initial_img = self._make_initial_gray(img) # For live cameras, start a latest-frame capture thread to avoid latency buildup. if self._is_live_camera: self._start_capture_thread() + def _make_initial_gray(self, img_bgr): + gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) + self.resolution_stitch = gray.shape + initial = cv2.resize(gray, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC) + if self.mirror: + initial = cv2.flip(initial, 1) + return initial.astype(np.uint8) + + def _find_stitch_boxes(self, img_src, show=False): """Detect vision tile boxes with camera stabilization.""" n_boxes, v_mean0, timeout = 0, 0, 5 diff --git a/tools/3d_patch_generation_blender/Readme.md b/tools/3d_patch_generation_blender/Readme.md new file mode 100644 index 0000000..7698b9b --- /dev/null +++ b/tools/3d_patch_generation_blender/Readme.md @@ -0,0 +1,121 @@ +# 3D Patch Generation Blender 工具 + +本文件夹包含 Blender 场景文件以及辅助脚本，用于导出网格体/相机数据，并将一张采集到的 2D 图像重新映射回 3D 表面。 + +## 主要文件 + +- `dd02c_v2.blend` + DD02C 曲面与相机配置所使用的主要 Blender 工程文件。 + +- `blender_export.py` + 在 Blender 中运行，用于导出以下数据： + - 网格几何信息 + - 与 UV 相关的 loop 数据 + - 相机内参 + - 相机位姿 + - 预计算的像素到 3D 查找表 + +- `mapto3d.py` + 读取导出的数据和一张采集图像，并生成带颜色的 PLY 格式 3D 点云。 + +## 导出 / 生成的数据 + +- `exported_data/` + 由 `blender_export.py` 生成。通常应包含以下文件： + - `mesh.npz` + - `camera_intrinsics.npz` + - `world_to_cam.npy` + - `pixel_to_xyz.npz` + +- `captured_image.png` + 要映射回导出 3D 表面的输入图像。当前该图像由 Blender 渲染得到。 + +- `mapped_points.ply` + 由 `mapto3d.py` 生成的输出点云文件。 + +- `texture/` + 保存实验过程中生成的纹理图片。 + +## 工作流程 + +### 1. 准备 Blender 场景 + +在 Blender 中打开 `dd02c_v2.blend`。 + +请确认： +- 已选中目标网格 +- 相机存在且位置正确 +- 渲染分辨率与采集图像分辨率一致 + +### 2. 导出几何和相机数据 + +在 Blender 的里面上面转到“scripts” 模式，然后点击运行 `blender_export.py`。 + +该脚本会将所需数据导出到： + +```text +exported_data/ +``` + +脚本会写出以下内容： +- 网格顶点和面信息 +- UV loop 数据 +- OpenCV 风格的相机内参 +- world-to-camera 变换矩阵 +- 可选的逐像素 3D 查找表 + +### 3. 准备输入图像 + +将需要投影到 3D 表面的图像放到当前文件夹中，可以在Blender 渲染生成，例如： + +```text +captured_image.png +``` + +`mapto3d.py` 目前在其 `__main__` 代码块中直接指定输入图像路径。 + +### 4. 将图像像素映射到 3D + +运行： + +```bash +python mapto3d.py +``` + +该脚本会： +- 加载 Blender 导出的数据 +- 加载输入图像 +- 在需要时对图像做上下翻转，以匹配导出的像素映射顺序 +- 优先使用预计算的 `pixel_to_xyz.npz` +- 将结果保存为 `mapped_points.ply` + +如果缺少预计算查找表，脚本会退回到逐像素射线求交，运行速度会慢很多。 + +### 5. 查看输出结果 + +生成的点云可以使用支持 PLY 的工具打开，例如： +- Open3D +- MeshLab +- CloudCompare + +`mapto3d.py` 中也包含可选的 Open3D 可视化步骤。 + +## 说明 + +- `mapto3d.py` 目前假设导出的相机模型与输入图像分辨率一致。 +- 脚本现在默认会对加载的图像做上下翻转，以保证映射颜色与 Blender 导出的像素顺序一致。 +- 为了获得更好的性能，建议在 `exported_data/` 中保留 `pixel_to_xyz.npz`。 + +## 依赖 + +### Blender 侧 + +- Blender Python API：`bpy` +- `numpy` + +### Python 侧 + +- `numpy` +- `opencv-python` +- `open3d`（可选，用于可视化） + diff --git a/tools/3d_patch_generation_blender/dd02c.blend b/tools/3d_patch_generation_blender/dd02c.blend deleted file mode 100644 index 119edbd..0000000 Binary files a/tools/3d_patch_generation_blender/dd02c.blend and /dev/null differ diff --git a/tools/3d_patch_generation_blender/mapto3d_video.py b/tools/3d_patch_generation_blender/mapto3d_video.py new file mode 100644 index 0000000..e14d108 --- /dev/null +++ b/tools/3d_patch_generation_blender/mapto3d_video.py @@ -0,0 +1,411 @@ +import argparse +import os +import time + +import cv2 +import numpy as np + + +def load_exported_data(data_dir): + """Load Blender-exported mesh/camera files.""" + 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"Real-time video viewing requires precomputed pixel_to_xyz.npz: {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 prepare_static_points(data, display_stride=4): + """ + Prepare fixed 3D points from the precomputed pixel-to-3D map. + + The geometry does not change between video frames, so real-time viewing only + needs to update point colors. + """ + 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_stride = max(1, int(display_stride)) + display_indices = np.arange(len(valid_points))[::display_stride] + return valid_mask, display_indices, valid_points[display_indices] + + +def frame_to_rgb_colors(frame_bgr, data, valid_mask, display_indices, flip_vertical=True): + """Convert a video frame to RGB point colors aligned with the exported map.""" + target_w = int(data["width"]) + target_h = int(data["height"]) + h, w = frame_bgr.shape[:2] + if (w, h) != (target_w, target_h): + frame_bgr = cv2.resize(frame_bgr, (target_w, target_h), interpolation=cv2.INTER_LINEAR) + + frame_rgb = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2RGB) + if flip_vertical: + frame_rgb = np.flipud(frame_rgb).copy() + + colors = frame_rgb.astype(np.float64) / 255.0 + valid_colors = colors[valid_mask].reshape(-1, 3) + return valid_colors[display_indices] + + +def init_open3d_viewer(points, colors): + import open3d as o3d + + vis = o3d.visualization.Visualizer() + vis.create_window(window_name="Real-time 3D Point Cloud", width=1280, height=720) + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points.astype(np.float64)) + pcd.colors = o3d.utility.Vector3dVector(colors.astype(np.float64)) + vis.add_geometry(pcd) + + render_option = vis.get_render_option() + render_option.point_size = 2.0 + render_option.background_color = np.array([0.0, 0.0, 0.0]) + + vis.poll_events() + vis.update_renderer() + return vis, pcd, o3d + + +def save_colored_pointcloud(points, colors, output_path): + """Save displayed points as a PLY file.""" + valid_colors = (colors * 255).clip(0, 255).astype(np.uint8) + with open(output_path, "w", encoding="utf-8") as f: + f.write("ply\n") + f.write("format ascii 1.0\n") + f.write(f"element vertex {len(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(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}") + + +def view_video_pointcloud( + video_path, + data, + display_stride=4, + frame_step=1, + flip_vertical=True, + loop=False, + save_snapshot_path=None, +): + """Open a video and show it as a real-time colored 3D point cloud.""" + cap = cv2.VideoCapture(video_path) + if not cap.isOpened(): + raise FileNotFoundError(f"Could not open video: {video_path}") + + valid_mask, display_indices, display_points = prepare_static_points( + data, + display_stride=display_stride, + ) + + ok, frame = cap.read() + if not ok: + cap.release() + raise RuntimeError(f"Could not read first frame from video: {video_path}") + + display_colors = frame_to_rgb_colors( + frame, + data, + valid_mask, + display_indices, + flip_vertical=flip_vertical, + ) + vis, pcd, o3d = init_open3d_viewer(display_points, display_colors) + + print("Real-time point cloud viewer started.") + print("Close the Open3D window or press Ctrl+C in the terminal to stop.") + print(f"Displayed points: {len(display_points)} (stride={display_stride})") + + frame_idx = 0 + shown_frames = 0 + t0 = time.perf_counter() + + try: + while True: + alive = vis.poll_events() + if not alive: + break + + ok, frame = cap.read() + if not ok: + if loop: + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + continue + break + + frame_idx += 1 + if frame_idx % max(1, frame_step) != 0: + continue + + display_colors = frame_to_rgb_colors( + frame, + data, + valid_mask, + display_indices, + flip_vertical=flip_vertical, + ) + pcd.colors = o3d.utility.Vector3dVector(display_colors) + vis.update_geometry(pcd) + vis.update_renderer() + + shown_frames += 1 + if shown_frames % 30 == 0: + elapsed = max(time.perf_counter() - t0, 1e-6) + print(f"Displayed {shown_frames} frames, viewer FPS={shown_frames / elapsed:.1f}") + + finally: + if save_snapshot_path: + save_colored_pointcloud(display_points, display_colors, save_snapshot_path) + cap.release() + vis.destroy_window() + + +def parse_args(): + parser = argparse.ArgumentParser(description="View a video as a real-time 3D point cloud.") + parser.add_argument("--data-dir", default="./exported_data", help="Blender exported_data folder") + parser.add_argument("--video", default="20260602-094916.mp4", help="Input video path") + parser.add_argument("--display-stride", type=int, default=4, + help="Show every Nth valid point. Increase for faster viewing.") + parser.add_argument("--frame-step", type=int, default=1, + help="Process every Nth video frame.") + parser.add_argument("--no-flip-vertical", action="store_true", + help="Disable vertical flip before color mapping.") + parser.add_argument("--loop", action="store_true", help="Loop video playback.") + parser.add_argument("--save-snapshot", default="", + help="Optional PLY path for saving the last displayed frame.") + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + exported = load_exported_data(args.data_dir) + view_video_pointcloud( + args.video, + exported, + display_stride=args.display_stride, + frame_step=args.frame_step, + flip_vertical=not args.no_flip_vertical, + loop=args.loop, + save_snapshot_path=args.save_snapshot or None, + ) +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]) \ No newline at end of file diff --git a/tools/3d_patch_generation_blender/random_patch_dd02c.py b/tools/3d_patch_generation_blender/random_patch_dd02c.py index a010717..8a130b5 100644 --- a/tools/3d_patch_generation_blender/random_patch_dd02c.py +++ b/tools/3d_patch_generation_blender/random_patch_dd02c.py @@ -1,6 +1,7 @@ """ Texture Generator for Arbitrary 3D Surfaces (Blender Script) -- Mode: "patches" (colored patches) or "dots" (circular markers) +- Mode: "patches" (colored patches), "checker" (black/white patches), + or "dots" (circular markers) - Mesh UVs are filled white so that non‑visible parts become white. - Texture background (no‑mesh areas) stays black. """ @@ -91,6 +92,36 @@ def fill_polygon(image, pixels, width, height, polygon, color): idx = (y * width + x) * 4 pixels[idx:idx+4] = color +def fill_polygon_value(grid, width, height, polygon, value): + """Fill a polygon into a 2D scalar grid.""" + poly = [(int(round(x)), int(round(y))) for x, y in polygon] + if len(poly) < 3: + return + min_x = max(0, min(p[0] for p in poly)) + max_x = min(width - 1, max(p[0] for p in poly)) + min_y = max(0, min(p[1] for p in poly)) + max_y = min(height - 1, max(p[1] for p in poly)) + for y in range(min_y, max_y + 1): + intersections = [] + for i in range(len(poly)): + x1, y1 = poly[i] + x2, y2 = poly[(i + 1) % len(poly)] + if y1 == y2: + continue + if (y1 <= y < y2) or (y2 <= y < y1): + t = (y - y1) / (y2 - y1) + x = x1 + t * (x2 - x1) + intersections.append(x) + intersections.sort() + for k in range(0, len(intersections), 2): + if k + 1 >= len(intersections): + break + x_start = int(round(intersections[k])) + x_end = int(round(intersections[k + 1])) + x_start = max(0, min(width - 1, x_start)) + x_end = max(0, min(width - 1, x_end)) + grid[y, x_start:x_end + 1] = value + def draw_ellipse(pixels, width, height, cx, cy, a, b, angle, color): """Filled ellipse with semi‑axes a,b (pixels), rotation angle (rad).""" cos_a = np.cos(angle) @@ -152,6 +183,85 @@ def raycast_from_camera(scene, depsgraph, cam, obj, u_ndc, v_ndc, frame_width, f return location, face_index return None, None +def raycast_uv_from_camera(scene, depsgraph, cam, obj, u_ndc, v_ndc, frame_width, frame_height): + """Camera raycast through normalized sensor coordinates and return UV.""" + loc, face_idx = raycast_from_camera( + scene, depsgraph, cam, obj, u_ndc, v_ndc, frame_width, frame_height + ) + if loc is None: + return None + return get_uv_from_face(obj, face_idx, loc) + +def convex_hull_uv(points): + """Return UV points ordered around their 2D convex hull.""" + unique = sorted({(float(p.x), float(p.y)) for p in points}) + if len(unique) < 3: + return None + + def cross(o, a, b): + return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0]) + + lower = [] + for p in unique: + while len(lower) >= 2 and cross(lower[-2], lower[-1], p) <= 0: + lower.pop() + lower.append(p) + + upper = [] + for p in reversed(unique): + while len(upper) >= 2 and cross(upper[-2], upper[-1], p) <= 0: + upper.pop() + upper.append(p) + + hull = lower[:-1] + upper[:-1] + if len(hull) < 3: + return None + return [Vector((u, v)) for u, v in hull] + +def sample_patch_uv_polygon( + scene, depsgraph, cam, obj, + u_start, u_end, v_start, v_end, + frame_width, frame_height, + edge_sample_grid=5 +): + """ + Build a UV polygon for one camera-space patch. + + Full interior cells use the four corners. Edge cells that only partially hit + the mesh are sampled more densely and converted to a convex hull so they are + not dropped just because some corners miss the silhouette. + """ + corners_ndc = [ + (u_start, v_start), + (u_end, v_start), + (u_end, v_end), + (u_start, v_end), + ] + + corner_uvs = [] + for u_ndc, v_ndc in corners_ndc: + uv = raycast_uv_from_camera( + scene, depsgraph, cam, obj, u_ndc, v_ndc, frame_width, frame_height + ) + if uv is not None: + corner_uvs.append(uv) + + if len(corner_uvs) >= 3: + return corner_uvs + + sampled_uvs = list(corner_uvs) + for sy in range(edge_sample_grid): + v_ndc = v_start + (v_end - v_start) * sy / (edge_sample_grid - 1) + for sx in range(edge_sample_grid): + u_ndc = u_start + (u_end - u_start) * sx / (edge_sample_grid - 1) + uv = raycast_uv_from_camera( + scene, depsgraph, cam, obj, u_ndc, v_ndc, frame_width, frame_height + ) + if uv is not None: + sampled_uvs.append(uv) + + return convex_hull_uv(sampled_uvs) + def rasterize_mesh_uvs(obj, pixels, tex_width, tex_height, color=(1,1,1,1)): """ Fill all triangles of the active UV map with 'color' (default white). @@ -175,6 +285,74 @@ def rasterize_mesh_uvs(obj, pixels, tex_width, tex_height, color=(1,1,1,1)): pixel_poly = [(u * (tex_width - 1), v * (tex_height - 1)) for u, v in uv_poly] fill_polygon(None, pixels, tex_width, tex_height, pixel_poly, color) +def rasterize_mesh_mask(obj, tex_width, tex_height): + """Rasterize the active UV map to a boolean mesh mask.""" + mask = np.zeros((tex_height, tex_width), dtype=bool) + me = obj.data + uv_layer = me.uv_layers.active + if not uv_layer: + return mask + uv_data = uv_layer.data + for poly in me.polygons: + uv_poly = [] + for loop_idx in poly.loop_indices: + uv = uv_data[loop_idx].uv + uv_poly.append((uv.x, uv.y)) + pixel_poly = [(u * (tex_width - 1), v * (tex_height - 1)) for u, v in uv_poly] + fill_polygon_value(mask, tex_width, tex_height, pixel_poly, True) + return mask + +def paint_mask_color(pixels, width, height, mask, color): + """Paint all True pixels of a boolean mask with an RGBA color.""" + r, g, b, a = color + ys, xs = np.nonzero(mask) + for y, x in zip(ys, xs): + idx = (y * width + x) * 4 + pixels[idx:idx+4] = (r, g, b, a) + +def complete_checker_gaps(pixels, width, height, mesh_mask, patch_state, max_gap_px=6): + """ + Fill small white slivers inside the mesh by expanding nearby checker colors. + + `patch_state`: -1 = unassigned, 0 = black patch, 1 = white patch. + """ + for _ in range(max_gap_px): + candidate = mesh_mask & (patch_state < 0) + if not np.any(candidate): + break + + black_counts = np.zeros_like(patch_state, dtype=np.uint8) + white_counts = np.zeros_like(patch_state, dtype=np.uint8) + + for dy in (-1, 0, 1): + for dx in (-1, 0, 1): + if dx == 0 and dy == 0: + continue + + src_y0 = max(0, -dy) + src_y1 = height - max(0, dy) + src_x0 = max(0, -dx) + src_x1 = width - max(0, dx) + dst_y0 = max(0, dy) + dst_y1 = height - max(0, -dy) + dst_x0 = max(0, dx) + dst_x1 = width - max(0, -dx) + + neighbor_vals = patch_state[src_y0:src_y1, src_x0:src_x1] + black_counts[dst_y0:dst_y1, dst_x0:dst_x1] += (neighbor_vals == 0) + white_counts[dst_y0:dst_y1, dst_x0:dst_x1] += (neighbor_vals == 1) + + fillable = candidate & ((black_counts + white_counts) > 0) + if not np.any(fillable): + break + + patch_state[fillable] = np.where( + black_counts[fillable] > white_counts[fillable], 0, 1 + ).astype(np.int8) + + paint_mask_color(pixels, width, height, patch_state == 0, (0.0, 0.0, 0.0, 1.0)) + paint_mask_color(pixels, width, height, patch_state == 1, (1.0, 1.0, 1.0, 1.0)) + # ---------------------------------------------------------------------- # Color generation (for patch mode) # ---------------------------------------------------------------------- @@ -237,6 +415,12 @@ def generate_valid_color(neighbors, r): color[channel] = sampled return color +def checker_patch_color(i, j): + """Return a black/white checker color for patch index (i, j).""" + if (i + j) % 2 == 0: + return np.array([1.0, 1.0, 1.0], dtype=np.float32) + return np.array([0.0, 0.0, 0.0], dtype=np.float32) + # ---------------------------------------------------------------------- # Mode 1: Colored Patches # ---------------------------------------------------------------------- @@ -244,7 +428,8 @@ def generate_patch_texture( obj, cam, scene, depsgraph, nx=40, ny=30, r=0.05, texture_width=2048, texture_height=2048, - output_path="//patch_texture.png",uv_centre=None, cross_length=40, cross_thickness=3, cross_color=(1,0,0,1) + output_path="//patch_texture.png", uv_centre=None, cross_length=40, + cross_thickness=3, cross_color=(1,0,0,1), edge_sample_grid=5 ): cam_data = cam.data render = scene.render @@ -260,8 +445,10 @@ def generate_patch_texture( frame_width = sensor_width / focal frame_height = sensor_height / focal + edge_sample_grid = max(2, int(edge_sample_grid)) patch_uv_polygons = [] patch_colors = np.zeros((ny, nx, 3), dtype=np.float32) + skipped_patches = 0 print(f"Raycasting {nx*ny} patches...") for j in range(ny): @@ -270,18 +457,15 @@ def generate_patch_texture( for i in range(nx): u_start = i / nx u_end = (i+1) / nx - corners_ndc = [(u_start, v_start), (u_end, v_start), - (u_end, v_end), (u_start, v_end)] - uv_poly = [] - for (u_ndc, v_ndc) in corners_ndc: - loc, face_idx = raycast_from_camera(scene, depsgraph, cam, obj, - u_ndc, v_ndc, frame_width, frame_height) - if loc is not None: - uv = get_uv_from_face(obj, face_idx, loc) - if uv is not None: - uv_poly.append(uv) - if len(uv_poly) < 3: + uv_poly = sample_patch_uv_polygon( + scene, depsgraph, cam, obj, + u_start, u_end, v_start, v_end, + frame_width, frame_height, + edge_sample_grid=edge_sample_grid + ) + if uv_poly is None: patch_uv_polygons.append(None) + skipped_patches += 1 continue patch_uv_polygons.append(uv_poly) @@ -290,6 +474,7 @@ def generate_patch_texture( if i > 0: neighbors.append(patch_colors[j, i-1]) color = np.random.rand(3) if len(neighbors)==0 else generate_valid_color(neighbors, r) patch_colors[j, i] = color + print(f"Skipped {skipped_patches} patches with fewer than 3 mesh samples.") # Create image – start with black background img_name = "PatchTexture" @@ -331,7 +516,105 @@ def generate_patch_texture( return image # ---------------------------------------------------------------------- -# Mode 2: Circular Dot Markers +# Mode 2: Black/White Checker Patches +# ---------------------------------------------------------------------- +def generate_checker_texture( + obj, cam, scene, depsgraph, + nx=40, ny=40, + texture_width=2048, texture_height=2048, + output_path="//checker_texture.png", uv_centre=None, cross_length=40, + cross_thickness=3, cross_color=(1,0,0,1), edge_sample_grid=5, + gap_fill_px=6 +): + cam_data = cam.data + render = scene.render + res_x = render.resolution_x + res_y = render.resolution_y + if cam_data.sensor_fit == 'HORIZONTAL': + sensor_width = cam_data.sensor_width + sensor_height = sensor_width / (res_x / res_y) + else: + sensor_height = cam_data.sensor_height + sensor_width = sensor_height * (res_x / res_y) + focal = cam_data.lens + frame_width = sensor_width / focal + frame_height = sensor_height / focal + + edge_sample_grid = max(2, int(edge_sample_grid)) + patch_uv_polygons = [] + patch_colors = np.zeros((ny, nx, 3), dtype=np.float32) + skipped_patches = 0 + + print(f"Raycasting {nx*ny} checker patches...") + for j in range(ny): + v_start = j / ny + v_end = (j+1) / ny + for i in range(nx): + u_start = i / nx + u_end = (i+1) / nx + uv_poly = sample_patch_uv_polygon( + scene, depsgraph, cam, obj, + u_start, u_end, v_start, v_end, + frame_width, frame_height, + edge_sample_grid=edge_sample_grid + ) + if uv_poly is None: + patch_uv_polygons.append(None) + skipped_patches += 1 + continue + patch_uv_polygons.append(uv_poly) + patch_colors[j, i] = checker_patch_color(i, j) + print(f"Skipped {skipped_patches} checker patches with fewer than 3 mesh samples.") + + img_name = "CheckerTexture" + if img_name in bpy.data.images: + bpy.data.images.remove(bpy.data.images[img_name]) + image = bpy.data.images.new(img_name, width=texture_width, height=texture_height, alpha=True) + pixels = list(image.pixels) + mesh_mask = rasterize_mesh_mask(obj, texture_width, texture_height) + patch_state = np.full((texture_height, texture_width), -1, dtype=np.int8) + + fill_background(pixels, texture_width, texture_height, (0,0,0,1)) + paint_mask_color(pixels, texture_width, texture_height, mesh_mask, (1,1,1,1)) + + idx = 0 + for j in range(ny): + for i in range(nx): + uv_poly = patch_uv_polygons[idx]; idx += 1 + if uv_poly is None: + continue + pixel_poly = [(u*(texture_width-1), v*(texture_height-1)) for u,v in uv_poly] + color = patch_colors[j,i] + rgba = (color[0], color[1], color[2], 1.0) + fill_polygon(image, pixels, texture_width, texture_height, pixel_poly, rgba) + fill_polygon_value( + patch_state, + texture_width, + texture_height, + pixel_poly, + 1 if np.mean(color) >= 0.5 else 0, + ) + + complete_checker_gaps( + pixels, texture_width, texture_height, mesh_mask, patch_state, max_gap_px=gap_fill_px + ) + + if uv_centre is not None: + draw_cross(pixels, texture_width, texture_height, + uv_centre[0], uv_centre[1], + cross_length, cross_thickness, cross_color) + + image.pixels[:] = pixels + image.update() + abs_path = bpy.path.abspath(output_path) + image.filepath_raw = abs_path + image.file_format = 'PNG' + image.save() + print(f"Checker texture saved to {abs_path}") + return image + +# ---------------------------------------------------------------------- +# Mode 3: Circular Dot Markers # ---------------------------------------------------------------------- def generate_dot_texture( obj, cam, scene, depsgraph, @@ -381,34 +664,49 @@ def generate_dot_texture( ( u_ndc, v_ndc - dv_ndc) ] uv_offsets = [] - valid = True for (u_off, v_off) in offsets: - loc_off, f_idx_off = raycast_from_camera(scene, depsgraph, cam, obj, - u_off, v_off, frame_width, frame_height) - if loc_off is None: - valid = False; break - uv_off = get_uv_from_face(obj, f_idx_off, loc_off) - if uv_off is None: - valid = False; break + uv_off = raycast_uv_from_camera( + scene, depsgraph, cam, obj, + u_off, v_off, frame_width, frame_height + ) uv_offsets.append(uv_off) - if not valid: continue scale_u = 1.0 / res_x scale_v = 1.0 / res_y - du_dx = (uv_offsets[0] - uv_offsets[1]) / (2 * offset_pix * scale_u) - dv_dx = (uv_offsets[2] - uv_offsets[3]) / (2 * offset_pix * scale_v) r_pix = dot_diameter_pixels / 2.0 - a_uv = r_pix * du_dx.length * scale_u - b_uv = r_pix * dv_dx.length * scale_v + + right_uv, left_uv, up_uv, down_uv = uv_offsets + if right_uv is not None and left_uv is not None: + du_dx = (right_uv - left_uv) / (2 * offset_pix * scale_u) + a_uv = r_pix * du_dx.length * scale_u + elif right_uv is not None: + du_dx = (right_uv - center_uv) / (offset_pix * scale_u) + a_uv = r_pix * du_dx.length * scale_u + elif left_uv is not None: + du_dx = (center_uv - left_uv) / (offset_pix * scale_u) + a_uv = r_pix * du_dx.length * scale_u + else: + du_dx = Vector((1.0, 0.0)) + a_uv = r_pix / texture_width + + if up_uv is not None and down_uv is not None: + dv_dx = (up_uv - down_uv) / (2 * offset_pix * scale_v) + b_uv = r_pix * dv_dx.length * scale_v + elif up_uv is not None: + dv_dx = (up_uv - center_uv) / (offset_pix * scale_v) + b_uv = r_pix * dv_dx.length * scale_v + elif down_uv is not None: + dv_dx = (center_uv - down_uv) / (offset_pix * scale_v) + b_uv = r_pix * dv_dx.length * scale_v + else: + b_uv = r_pix / texture_height + + a_uv = max(a_uv, 1.0 / texture_width) + b_uv = max(b_uv, 1.0 / texture_height) angle = np.arctan2(du_dx.y, du_dx.x) if du_dx.length > 0 else 0.0 dots.append((center_uv.x, center_uv.y, a_uv, b_uv, angle)) print(f"Found {len(dots)} valid dots.") - - if uv_centre is not None: - draw_cross(pixels, texture_width, texture_height, - uv_centre[0], uv_centre[1], - cross_length, cross_thickness, cross_color) # Create image img_name = "DotTexture" @@ -430,6 +728,11 @@ def generate_dot_texture( draw_ellipse(pixels, texture_width, texture_height, cx_px, cy_px, a_px, b_px, angle_uv, dot_color) + if uv_centre is not None: + draw_cross(pixels, texture_width, texture_height, + uv_centre[0], uv_centre[1], + cross_length, cross_thickness, cross_color) + image.pixels[:] = pixels image.update() abs_path = bpy.path.abspath(output_path) @@ -444,15 +747,23 @@ def generate_dot_texture( # ---------------------------------------------------------------------- def main(): # ==================== CONFIGURATION ==================== - MODE = "patches" + MODE = "patches" # "patches", "checker", or "dots" OBJ_NAME = "FingerMesh" CAM_NAME = "Camera" TEXTURE_WIDTH = 4096 TEXTURE_HEIGHT = 4096 - OUTPUT_PATH = "//" + ("patch_texture.png" if MODE=="patches" else "dot_texture.png") + OUTPUT_NAMES = { + "patches": "patch_texture.png", + "checker": "checker_texture.png", + "dots": "dot_texture.png", + } + OUTPUT_PATH = "//" + OUTPUT_NAMES.get(MODE, "texture.png") # Patch mode PATCH_NX = 60; PATCH_NY = 50; COLOR_R = 0.05 + EDGE_SAMPLE_GRID = 5 + # Checker mode + CHECKER_NX = 40; CHECKER_NY = 40; CHECKER_GAP_FILL_PX = 6 # Dot mode DOT_DIAMETER_PIX = 12; DOT_SPACING_PIX = 40; DOT_COLOR = (0,0,0,1) # Cross @@ -506,7 +817,21 @@ def main(): uv_centre=uv_centre, cross_length=CROSS_LENGTH_PX, cross_thickness=CROSS_THICKNESS_PX, - cross_color=CROSS_COLOR + cross_color=CROSS_COLOR, + edge_sample_grid=EDGE_SAMPLE_GRID + ) + elif MODE == "checker": + generate_checker_texture( + obj, cam, scene, depsgraph, + nx=CHECKER_NX, ny=CHECKER_NY, + texture_width=TEXTURE_WIDTH, texture_height=TEXTURE_HEIGHT, + output_path=OUTPUT_PATH, + uv_centre=uv_centre, + cross_length=CROSS_LENGTH_PX, + cross_thickness=CROSS_THICKNESS_PX, + cross_color=CROSS_COLOR, + edge_sample_grid=EDGE_SAMPLE_GRID, + gap_fill_px=CHECKER_GAP_FILL_PX ) elif MODE == "dots": generate_dot_texture( @@ -521,6 +846,8 @@ def main(): cross_thickness=CROSS_THICKNESS_PX, cross_color=CROSS_COLOR ) + else: + raise ValueError(f"Unknown MODE: {MODE!r}. Use 'patches', 'checker', or 'dots'.") print(f"Completed in {time.time()-start_time:.2f} seconds.") diff --git a/tools/3d_patch_generation_blender/uv_to_printer.py b/tools/3d_patch_generation_blender/uv_to_printer.py deleted file mode 100644 index 7443ef4..0000000 --- a/tools/3d_patch_generation_blender/uv_to_printer.py +++ /dev/null @@ -1,180 +0,0 @@ -""" -Convert a UV texture map to a physically scaled image for UV printing. -Usage: python uv_to_printer.py --model model.obj --texture texture.png --resolution 100 --output print_ready.png -""" - -import argparse -import numpy as np -import cv2 -import trimesh -import os - -def compute_uv_physical_scale(mesh, uv_layer_name=None): - """ - Compute the physical dimensions (mm) of the UV space. - Assumes the UV map is a reasonably isometric unwrapping (e.g., cylindrical). - Returns: (width_mm, height_mm, scale_factor_u, scale_factor_v) - where scale_factor = physical length per UV unit. - """ - # Get UV coordinates and corresponding 3D positions - if not hasattr(mesh.visual, 'uv') or mesh.visual.uv is None: - raise ValueError("Mesh has no UV coordinates.") - - uvs = mesh.visual.uv - faces = mesh.faces - vertices = mesh.vertices - - # For each face, we have 3 UVs and 3 vertex positions - # We'll compute the physical edge lengths for all edges in UV space - # and compare to 3D edge lengths to estimate scaling. - - total_uv_length = 0.0 - total_3d_length = 0.0 - edge_count = 0 - - # Also track UV bounding box for overall dimensions - uv_min = uvs.min(axis=0) - uv_max = uvs.max(axis=0) - uv_range = uv_max - uv_min - - # Estimate scale factor by sampling edges - for face in faces: - for i in range(3): - j = (i + 1) % 3 - # UV edge - uv0 = uvs[face[i]] - uv1 = uvs[face[j]] - uv_len = np.linalg.norm(uv1 - uv0) - # 3D edge - v0 = vertices[face[i]] - v1 = vertices[face[j]] - len3d = np.linalg.norm(v1 - v0) - - if uv_len > 1e-6: - total_uv_length += uv_len - total_3d_length += len3d - edge_count += 1 - - if edge_count == 0: - raise ValueError("No valid edges found for scale estimation.") - - # Average scale factor (physical mm per UV unit) - scale_factor = total_3d_length / total_uv_length - - # Physical dimensions of the UV layout - width_mm = uv_range[0] * scale_factor - height_mm = uv_range[1] * scale_factor - - print(f"UV bounding box: {uv_range[0]:.3f} x {uv_range[1]:.3f} UV units") - print(f"Scale factor: {scale_factor:.3f} mm per UV unit") - print(f"Physical size: {width_mm:.2f} mm x {height_mm:.2f} mm") - - return width_mm, height_mm, scale_factor - -def generate_print_image(texture_path, width_mm, height_mm, pixels_per_mm, output_path): - """ - Load the texture image, scale it to match the physical dimensions at the - specified resolution, and save the result. - """ - # Read texture (with alpha if present) - texture = cv2.imread(texture_path, cv2.IMREAD_UNCHANGED) - if texture is None: - raise FileNotFoundError(f"Cannot load texture: {texture_path}") - - # Compute target pixel dimensions - target_width_px = int(round(width_mm * pixels_per_mm)) - target_height_px = int(round(height_mm * pixels_per_mm)) - - print(f"Target image size: {target_width_px} x {target_height_px} pixels") - - # Resize texture (using INTER_LINEAR for up/down scaling) - resized = cv2.resize(texture, (target_width_px, target_height_px), - interpolation=cv2.INTER_LINEAR) - - # Save output (preserve alpha if present) - cv2.imwrite(output_path, resized) - print(f"Print-ready image saved to: {output_path}") - -def process_exported_npz(npz_path, texture_path, pixels_per_mm, output_path): - """ - Alternative: load data exported from Blender script (vertices, faces, UVs). - """ - data = np.load(npz_path) - vertices = data['vertices'] - faces = data['faces'] - uv_data = data['uv_data'] - loop_to_vertex = data['loop_to_vertex'] - - # Reconstruct per-vertex UVs (average) - # For simplicity, we'll compute physical scale using face edges as in trimesh method. - # We need to build a temporary mesh-like structure. - - # Count number of faces - num_faces = len(faces) - # Build face UVs array (3 per face) - # The uv_data is per-loop, we need to group by face. - # Assuming faces are triangles. - face_uvs = [] - face_vertices = [] - uv_idx = 0 - for face in faces: - f_uvs = [] - f_verts = [] - for _ in range(len(face)): - f_uvs.append(uv_data[uv_idx]) - v_idx = loop_to_vertex[uv_idx] - f_verts.append(vertices[v_idx]) - uv_idx += 1 - face_uvs.append(np.array(f_uvs)) - face_vertices.append(np.array(f_verts)) - - total_uv_len = 0.0 - total_3d_len = 0.0 - count = 0 - uv_min = np.min(uv_data, axis=0) - uv_max = np.max(uv_data, axis=0) - uv_range = uv_max - uv_min - - for f_uv, f_v in zip(face_uvs, face_vertices): - for i in range(3): - j = (i+1)%3 - uv_len = np.linalg.norm(f_uv[i] - f_uv[j]) - v_len = np.linalg.norm(f_v[i] - f_v[j]) - if uv_len > 1e-6: - total_uv_len += uv_len - total_3d_len += v_len - count += 1 - - scale = total_3d_len / total_uv_len if count>0 else 1.0 - width_mm = uv_range[0] * scale - height_mm = uv_range[1] * scale - - print(f"Physical size from .npz: {width_mm:.2f} x {height_mm:.2f} mm") - generate_print_image(texture_path, width_mm, height_mm, pixels_per_mm, output_path) - -def main(): - parser = argparse.ArgumentParser(description="Convert UV texture to physically scaled print image.") - parser.add_argument("--model", help="3D model file (OBJ, PLY, STL) with UVs") - parser.add_argument("--npz", help="Alternatively, use exported .npz from Blender script") - parser.add_argument("--texture", required=True, help="Input texture image (PNG, JPG)") - parser.add_argument("--resolution", type=float, default=100.0, - help="Pixels per millimeter (default: 100)") - parser.add_argument("--output", default="print_ready.png", help="Output image path") - args = parser.parse_args() - - if not args.model and not args.npz: - parser.error("Either --model or --npz must be provided.") - - if args.npz: - process_exported_npz(args.npz, args.texture, args.resolution, args.output) - else: - # Load mesh with trimesh - mesh = trimesh.load(args.model, force='mesh') - if not isinstance(mesh, trimesh.Trimesh): - raise ValueError("Loaded object is not a single mesh.") - - width_mm, height_mm, scale = compute_uv_physical_scale(mesh) - generate_print_image(args.texture, width_mm, height_mm, args.resolution, args.output) - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/tools/dd02_3dviewer/exported_data/camera_intrinsics.npz b/tools/dd02_3dviewer/exported_data/camera_intrinsics.npz new file mode 100644 index 0000000..56b7885 Binary files /dev/null and b/tools/dd02_3dviewer/exported_data/camera_intrinsics.npz differ diff --git a/tools/dd02_3dviewer/exported_data/mesh.npz b/tools/dd02_3dviewer/exported_data/mesh.npz new file mode 100644 index 0000000..0b95784 Binary files /dev/null and b/tools/dd02_3dviewer/exported_data/mesh.npz differ diff --git a/tools/dd02_3dviewer/exported_data/pixel_to_xyz.npz b/tools/dd02_3dviewer/exported_data/pixel_to_xyz.npz new file mode 100644 index 0000000..7c83a7b Binary files /dev/null and b/tools/dd02_3dviewer/exported_data/pixel_to_xyz.npz differ diff --git a/tools/dd02_3dviewer/exported_data/world_to_cam.npy b/tools/dd02_3dviewer/exported_data/world_to_cam.npy new file mode 100644 index 0000000..e59cfe7 Binary files /dev/null and b/tools/dd02_3dviewer/exported_data/world_to_cam.npy differ diff --git a/tools/3d_patch_generation_blender/mapto3d.py b/tools/dd02_3dviewer/mapto3d.py similarity index 91% rename from tools/3d_patch_generation_blender/mapto3d.py rename to tools/dd02_3dviewer/mapto3d.py index d2d936f..49ffb05 100644 --- a/tools/3d_patch_generation_blender/mapto3d.py +++ b/tools/dd02_3dviewer/mapto3d.py @@ -85,17 +85,23 @@ def pixel_to_3d_ray(u, v, K, world_to_cam): return ray_world_origin, ray_world_dir -def map_image_to_3d(image_path, data, use_precomputed=True): +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: @@ -161,11 +167,16 @@ def save_colored_pointcloud(points, colors, output_path): if __name__ == "__main__": data_dir = "./exported_data" # Path to exported files - image_path = "captured_image.png" # Input camera image + 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) + 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 diff --git a/tools/dd02_3dviewer/sdk_mapto3d.py b/tools/dd02_3dviewer/sdk_mapto3d.py new file mode 100644 index 0000000..2c79caa --- /dev/null +++ b/tools/dd02_3dviewer/sdk_mapto3d.py @@ -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())