bluepulsar
120 lines
3.9 KiB
Python
120 lines
3.9 KiB
Python
import os
|
||
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" # Windows 下禁用硬件加速，避免兼容性问题
|
||
|
||
import cv2
|
||
import orisys
|
||
import numpy as np
|
||
from scipy.ndimage import gaussian_filter
|
||
|
||
|
||
def get_depth(div, fps, H=400, W=400, Z0=30, win_size=10):
|
||
# Precompute
|
||
dt = 1.0 / fps # e.g., 1/30
|
||
Z_map = np.full((H, W), Z0, dtype=np.float32) # initial depth map
|
||
|
||
|
||
for win_y in range(0, H, win_size):
|
||
for win_x in range(0, W, win_size):
|
||
y1, y2 = win_y, min(win_y + win_size, H)
|
||
x1, x2 = win_x, min(win_x + win_size, W)
|
||
|
||
# Robust divergence estimate
|
||
D_win = np.median(div[y1:y2, x1:x2])
|
||
|
||
# Avoid numerical issues
|
||
if np.abs(D_win) < 1e-3:
|
||
continue
|
||
|
||
# Update depth in window
|
||
Z_old = Z_map[y1, x1] # or use window mean
|
||
Z_new = Z_old * np.exp(-0.5 * D_win * dt)
|
||
|
||
# Optionally fuse with dot-based depth if available
|
||
# Z_new = alpha * Z_new + (1 - alpha) * Z_from_dots
|
||
|
||
Z_map[y1:y2, x1:x2] = Z_new
|
||
return Z_map
|
||
|
||
def main():
|
||
|
||
|
||
# 方式1: 从视频文件读取
|
||
# sensor = orisys.Sensor("./s1.mp4", isstitch=True, cuda=True, verbose=True)
|
||
|
||
# 方式2: 从摄像头读取（取消注释使用）
|
||
sensor = orisys.Sensor(0, isstitch=True, cuda=True, verbose=True)
|
||
|
||
print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")
|
||
|
||
while True:
|
||
# 步骤 2.1: 获取并拼接图像
|
||
sensor.get_img()
|
||
|
||
# 步骤 2.2: 计算形变场
|
||
sensor.compute_deformation()
|
||
|
||
fps, flow,vnormal, depth = sensor.read_info(
|
||
sensor.info.FPS,
|
||
sensor.info.VRAW,
|
||
sensor.info.VNORMAL,
|
||
sensor.info.DEPTH
|
||
)
|
||
|
||
# depth = get_depth_improved(vnormal, fps)
|
||
|
||
print(f"FPS: {fps:5.1f}, 深度: {np.min(depth):8.2f}, {np.mean(depth):8.2f}, {np.max(depth):8.2f}")
|
||
|
||
|
||
|
||
|
||
arrows = orisys.util.draw_arrows(
|
||
sensor.img.copy(), # 使用 copy() 避免修改原图
|
||
vnormal,
|
||
threshold=5,
|
||
grid_spacing=10,
|
||
arrow_scale=1.0
|
||
)
|
||
cv2.imshow("形变场 Deformation", arrows)
|
||
|
||
# # 绘制深度图（热力图）
|
||
# # 深度值缩放：中心值 30，范围 ±10（即 20-40 映射到 0-255）
|
||
# depth_center = 30 # 中心深度值
|
||
# depth_range = 5 # 变化范围 ±10
|
||
# depth_normalized = (depth - (depth_center - depth_range)) / (2 * depth_range) # 归一化到 0-1
|
||
# depth_normalized = np.clip(depth_normalized, 0, 1) # 限制在 0-1 范围
|
||
depth_img = cv2.applyColorMap(
|
||
(depth * 255.0).astype('uint8'),
|
||
cv2.COLORMAP_HOT
|
||
)
|
||
cv2.imshow("深度图 Depth", depth_img)
|
||
|
||
mag_img = orisys.util.draw_magnitude_map(
|
||
sensor.img.copy(),
|
||
vnormal,
|
||
threshold=5,
|
||
colormap=cv2.COLORMAP_HOT
|
||
)
|
||
cv2.imshow("magnitude map", mag_img)
|
||
|
||
# =====================================================================
|
||
# 4. 键盘控制
|
||
# =====================================================================
|
||
key = cv2.waitKey(1) & 0xFF
|
||
if key == ord("q"):
|
||
print("\n退出程序...")
|
||
break
|
||
if key == ord("r"):
|
||
# 重置光流追踪器（当追踪漂移时使用）
|
||
sensor.reset()
|
||
print("光流追踪器已重置")
|
||
|
||
# =========================================================================
|
||
# 5. 清理资源
|
||
# =========================================================================
|
||
sensor.disconnect() # 释放视频源和相关资源
|
||
cv2.destroyAllWindows()
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main()
|