RGBD相机测尺寸Python以及原理详解

1. RGBD相机简介

RGBD相机（如Intel RealSense D435if）是一种能够同时捕获彩色图像（RGB）和深度图像（Depth）的传感器。它通过红外结构光、双目视觉或飞行时间（ToF）等技术，为场景中的每个像素提供距离信息，从而构建三维点云。

2. 测长度的基本原理

利用RGBD相机测量物体长度的核心原理是三维空间几何计算。其流程可概括为：

数据获取：相机拍摄目标物体，得到一张彩色图（RGB）和一张深度图（D）。深度图的每个像素值代表该点到相机的距离（通常以毫米为单位）。
坐标转换 ：利用相机的内参（焦距、主点）将深度图中每个像素的（u, v, d）坐标转换为相机坐标系下的三维点（X, Y, Z）。转换公式通常为：
- X = (u - cx) * d / fx
- Y = (v - cy) * d / fy
- Z = d
  其中 (fx, fy) 为焦距，(cx, cy) 为主点坐标。
点云生成与选取：所有转换后的点构成三维点云。用户在彩色图上框选或点击物体的两个端点（例如A4纸的长边两端），系统找到这两个像素点对应的三维空间点P1(X1, Y1, Z1)和P2(X2, Y2, Z2)。
距离计算 ：根据三维空间两点间的欧氏距离公式计算长度：
python 复制代码
```
import math
length = math.sqrt((X2 - X1)**2 + (Y2 - Y1)**2 + (Z2 - Z1)**2)
```

python 复制代码

import numpy as np
# ==================== 相机参数配置 ====================
# 相机内参，假设无畸变
FX, FY, CX, CY = 1365.0, 1365.0, 959.0, 572.0
_K = np.array([[FX, 0, CX],
               [0, FY, CY],
               [0, 0, 1]], dtype=np.float64)

# 深度值单位 -> 毫米 的缩放系数
# RealSense / Kinect: 1.0 (深度图单位即 mm)
# 部分相机深度单位为 0.1mm: 填 0.1
# 若深度图以 float 米为单位: 填 1000.0
DEPTH_SCALE = 1


def get_depth_at(depth, u, v, radius=3):  # 获取像素点 (u,v) 附近的有效深度均值 (mm)
    """
    获取像素点 (u,v) 附近的有效深度均值 (mm)
    对深度空洞/噪声做局部中值滤波处理
    返回 None 表示该区域无有效深度
    """
    h, w = depth.shape[:2]
    if not (0 <= u < w and 0 <= v < h):
        return None
    y0, y1 = max(0, v - radius), min(h, v + radius + 1)
    x0, x1 = max(0, u - radius), min(w, u + radius + 1)
    region = depth[y0:y1, x0:x1]
    valid = region[(region > 0) & np.isfinite(region)]
    if valid.size == 0:
        return None
    return float(np.median(valid)) * DEPTH_SCALE   # 应用深度缩放系数


def pixel_to_camera(u, v, depth_val, K=None):  # 像素坐标 + 深度 -> 相机系3D坐标 (单位: mm)
    """
    像素坐标 + 深度 -> 相机系3D坐标 (单位: mm)
    X = (u - cx) * z / fx
    Y = (v - cy) * z / fy
    Z = z
    """
    K = K if K is not None else _K
    X = (u - K[0, 2]) * depth_val / K[0, 0]
    Y = (v - K[1, 2]) * depth_val / K[1, 1]
    return np.array([X, Y, depth_val])



def get_distance(p3d_2,p3d_1):#2点间距离计算
    dist_mm = np.linalg.norm(p3d_2 - p3d_1)
    return dist_mm

python 复制代码

import cv2
from Distance_compute import get_depth_at, pixel_to_camera, get_distance
class DepthPointSelector:
    """深度图交互式选点测量"""

    def __init__(self, color_img, depth):
        self.color_img = color_img.copy()
        self.depth = depth
        self.points = []
        self.measurements = []  # [(p1, p2, dist_mm, z1, z2)]

    def draw_overlay(self):
        img_draw = self.color_img.copy()
        for (p1, p2, dist_mm, z1, z2) in self.measurements:
            cv2.line(img_draw, p1, p2, (0, 0, 255), 2)
            cv2.circle(img_draw, p1, 5, (0, 255, 0), -1)
            cv2.circle(img_draw, p2, 5, (0, 255, 0), -1)
            mid = ((p1[0] + p2[0]) // 2, (p1[1] + p2[1]) // 2)
            label = f"{dist_mm:.1f} mm"
            if abs(z2 - z1) > 5:  # 两点深度差异大时标注深度
                label += f" (z:{z1:.0f}->{z2:.0f})"
            cv2.putText(img_draw, label, mid, cv2.FONT_HERSHEY_SIMPLEX,
                        0.8, (0, 0, 255), 2)
        for i, p in enumerate(self.points):
            cv2.circle(img_draw, p, 5, (255, 0, 0), -1)
            cv2.putText(img_draw, str(i + 1), (p[0] + 10, p[1] + 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
        return img_draw

def main():
    COLOR_PATH = "RGB_20260817_202539.png"
    DEPTH_PATH = "DepthRaw_20260817_202539.png"
    color_img = cv2.imread(COLOR_PATH)
    # 以原始位深读取深度图，避免 16 位数据被截断为 8 位
    depth = cv2.imread(DEPTH_PATH, cv2.IMREAD_UNCHANGED)
    if color_img is None or depth is None:
        print("图像读取失败，请检查文件路径")
        return
    # 若读取结果意外为 3 通道，取第一个通道
    if depth.ndim == 3:
        depth = depth[:, :, 0]
    print(f"彩色图: {COLOR_PATH} ({color_img.shape[1]}x{color_img.shape[0]})")
    print(f"深度图: {DEPTH_PATH} ({depth.shape[1]}x{depth.shape[0]})  dtype={depth.dtype}")

    # ========== 交互测量 ==========
    selector = DepthPointSelector(color_img, depth)
    window_name = "深度尺寸测量"

    def on_mouse(event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            z = get_depth_at(depth, x, y)
            if z is None:
                print(f"  ({x}, {y}) 处无有效深度值，请重新点击")
                return
            selector.points.append((x, y))
            print(f"  点击点 {len(selector.points)}: ({x}, {y}), 深度 = {z:.1f} mm")

            if len(selector.points) == 2:
                (x1, y1), (x2, y2) = selector.points
                z1 = get_depth_at(depth, x1, y1)
                z2 = get_depth_at(depth, x2, y2)

                # 反投影到相机系3D坐标 (mm)
                p3d_1 = pixel_to_camera(x1, y1, z1)
                p3d_2 = pixel_to_camera(x2, y2, z2)

                # 空间欧氏距离 (真实3D距离，不受远近影响)
                dist_mm = get_distance(p3d_2, p3d_1)   # 修复：传入两个 3D 点

                selector.measurements.append(((x1, y1), (x2, y2), dist_mm, z1, z2))
                print(f"  3D点1: ({p3d_1[0]:.1f}, {p3d_1[1]:.1f}, {p3d_1[2]:.1f}) mm")
                print(f"  3D点2: ({p3d_2[0]:.1f}, {p3d_2[1]:.1f}, {p3d_2[2]:.1f}) mm")
                print(f"  >>> 两点空间距离: {dist_mm:.2f} mm ({dist_mm/10:.2f} cm)")
                selector.points = []

    cv2.namedWindow(window_name)
    cv2.setMouseCallback(window_name, on_mouse)

    print("\n左键点击选点测量，'r' 清空，'q' 或 Esc 退出")

    while True:
        display = selector.draw_overlay()
        cv2.imshow(window_name, display)
        key = cv2.waitKey(30) & 0xFF

        if key == ord('q') or key == 27:
            break
        elif key == ord('r'):
            selector.measurements = []
            selector.points = []
            print("已清空所有测量")

    cv2.destroyAllWindows()

    # 输出汇总
    print("\n" + "=" * 50)
    print("测量结果汇总:")
    for i, (p1, p2, dist_mm, z1, z2) in enumerate(selector.measurements):
        print(f"  测量 {i + 1}: ({p1[0]},{p1[1]}) -> ({p2[0]},{p2[1]})"
              f" = {dist_mm:.2f} mm (深度 {z1:.0f} -> {z2:.0f} mm)")
    print("=" * 50)


if __name__ == "__main__":
    main()

测量结果，A4纸的长度是21mm，测量出来是215mm，还是比较准的。