baoshi-measure-cargo/cargo_height_measure.py

"""本地可视化测量脚本。

用于直接打开深度相机窗口，实时查看 ROI、最近点与距离标注，
便于调试参数与现场观测效果。
"""

import time

import cv2

from depth_common import (
    Settings,
    TemporalFilter,
    compute_roi_bounds,
    extract_depth_data,
    find_nearest_point,
    init_depth_pipeline,
    nearest_distance_in_roi,
)

# 键盘退出键。
ESC_KEY = 27

# 控制终端打印频率（秒）。
PRINT_INTERVAL = 1

# 从环境变量加载深度处理参数。
SETTINGS = Settings.from_env()


def main():
    """启动实时深度查看窗口。"""
    # 启用时间滤波以降低深度抖动。
    temporal_filter = TemporalFilter(alpha=0.5)
    try:
        # 初始化深度相机并获取深度内参。
        pipeline, depth_intrinsics, depth_profile = init_depth_pipeline()
        print("depth profile: ", depth_profile)
    except Exception as e:
        print(e)
        return

    last_print_time = time.time()
    while True:
        try:
            # 等待一帧数据，超时时间 100ms。
            frames = pipeline.wait_for_frames(100)
            if frames is None:
                continue

            depth_frame = frames.get_depth_frame()
            depth_data = extract_depth_data(depth_frame, SETTINGS, temporal_filter)
            if depth_data is None:
                continue

            # 计算中心 ROI 范围。
            bounds = compute_roi_bounds(depth_data, depth_intrinsics, SETTINGS)
            if bounds is None:
                continue

            x_start, x_end, y_start, y_end, center_distance = bounds
            roi = depth_data[y_start:y_end, x_start:x_end]

            # 计算 ROI 最近距离。
            nearest_distance = nearest_distance_in_roi(roi, SETTINGS) or 0

            # 找到最近点坐标用于绘制圆点标记。
            nearest_point = find_nearest_point(
                roi,
                x_start,
                y_start,
                SETTINGS,
                nearest_distance,
            )

            # 限频打印测量值，避免刷屏。
            current_time = time.time()
            if current_time - last_print_time >= PRINT_INTERVAL:
                print(
                    "nearest distance in "
                    f"{SETTINGS.roi_width_cm}cm x {SETTINGS.roi_height_cm}cm area: ",
                    nearest_distance,
                )
                last_print_time = current_time

            # 生成深度伪彩图并叠加标注信息。
            depth_image = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
            depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET)

            cv2.rectangle(
                depth_image,
                (x_start, y_start),
                (x_end - 1, y_end - 1),
                (0, 255, 0),
                2,
            )
            if nearest_point is not None:
                cv2.circle(depth_image, nearest_point, 4, (0, 0, 0), -1)
                cv2.circle(depth_image, nearest_point, 6, (0, 255, 255), 2)

            cv2.putText(
                depth_image,
                f"nearest: {nearest_distance} mm",
                (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.8,
                (255, 255, 255),
                2,
                cv2.LINE_AA,
            )
            cv2.putText(
                depth_image,
                f"center: {int(center_distance)} mm",
                (10, 60),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.8,
                (255, 255, 255),
                2,
                cv2.LINE_AA,
            )

            cv2.imshow("Depth Viewer", depth_image)
            key = cv2.waitKey(1)
            if key == ord("q") or key == ESC_KEY:
                break
        except KeyboardInterrupt:
            break

    # 退出前释放窗口和相机资源。
    cv2.destroyAllWindows()
    pipeline.stop()


if __name__ == "__main__":
    main()