2. ToF传感器标定流程搭建

任务定义

本案例目标是为ToF传感器搭建一套可复现的标定流程，覆盖从原始深度数据采集到标定参数生成与精度验证的完整闭环。需要解决的核心问题是：ToF传感器因镜头畸变、多路径干扰和温度漂移导致的深度测量偏差，通过标定获得内参、畸变系数和深度校正参数，使校正后的深度误差满足应用要求。

标定流程架构

标定流程分为四个阶段：数据采集、参数求解、参数应用、精度验证。数据采集阶段使用棋盘格靶标在不同距离和角度下拍摄多帧图像；参数求解阶段分别计算相机内参与畸变系数、深度偏移量与温度补偿系数；参数应用阶段将标定结果写入传感器驱动；精度验证阶段通过独立测试集评估校正效果。

核心代码实现

以下代码实现标定流程的核心算法，包括棋盘格角点提取、内参求解、深度偏移计算和温度补偿拟合。代码使用OpenCV和NumPy，输入为多帧深度图与对应灰度图，输出为标定参数JSON文件。

import cv2
import numpy as np
import json
import glob
from scipy.optimize import curve_fit

class ToFCalibrator:
    def __init__(self, board_size=(9, 6), square_size_mm=30.0):
        self.board_size = board_size  # 内角点数
        self.square_size = square_size_mm / 1000.0  # 转为米
        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
        self.objp = np.zeros((board_size[0]*board_size[1], 3), np.float32)
        self.objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2)
        self.objp *= self.square_size

    def collect_corners(self, gray_images):
        """提取所有图像中的棋盘格角点"""
        obj_points, img_points = [], []
        valid_count = 0
        for img in gray_images:
            ret, corners = cv2.findChessboardCorners(img, self.board_size, None)
            if ret:
                corners_refined = cv2.cornerSubPix(
                    img, corners, (11, 11), (-1, -1), self.criteria)
                obj_points.append(self.objp)
                img_points.append(corners_refined)
                valid_count += 1
        if valid_count < 5:
            raise RuntimeError(f"有效标定图像不足: {valid_count}/5")
        return obj_points, img_points

    def calibrate_intrinsics(self, gray_images, depth_images):
        """标定相机内参、畸变系数和深度偏移"""
        obj_points, img_points = self.collect_corners(gray_images)
        h, w = gray_images[0].shape[:2]
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
            obj_points, img_points, (w, h), None, None)

        # 计算深度偏移: 对每帧棋盘格平面拟合深度值
        depth_offsets = []
        for i, (rvec, tvec) in enumerate(zip(rvecs, tvecs)):
            # 将棋盘格角点投影到图像坐标
            proj_points, _ = cv2.projectPoints(
                self.objp, rvec, tvec, mtx, dist)
            proj_points = proj_points.reshape(-1, 2).astype(int)

            # 提取对应位置的深度值并拟合平面
            valid_depths = []
            for (x, y) in proj_points:
                if 0 <= y < depth_images[i].shape[0] and 0 <= x < depth_images[i].shape[1]:
                    d = depth_images[i][y, x]
                    if d > 0:  # 有效深度
                        valid_depths.append(d)
            if len(valid_depths) > 10:
                # 真实距离 = 棋盘格到相机的Z距离
                real_dist = np.linalg.norm(tvec)
                measured_mean = np.mean(valid_depths) / 1000.0  # mm转m
                depth_offsets.append(measured_mean - real_dist)

        depth_offset = np.mean(depth_offsets) if depth_offsets else 0.0
        return mtx, dist, depth_offset

    def fit_temperature_coeff(self, temps, depth_errors):
        """拟合温度补偿系数: error = a * (T - T_ref)"""
        def linear_model(T, a):
            return a * (T - 25.0)  # 25°C为参考温度

        popt, _ = curve_fit(linear_model, temps, depth_errors)
        return popt[0]  # 温度系数 a (m/°C)

    def save_params(self, mtx, dist, depth_offset, temp_coeff, output_path):
        """保存标定参数为JSON"""
        params = {
            "camera_matrix": mtx.tolist(),
            "dist_coeffs": dist.tolist(),
            "depth_offset_m": float(depth_offset),
            "temp_coeff_m_per_c": float(temp_coeff),
            "ref_temp_c": 25.0,
            "calibration_date": "2024-01-15"
        }
        with open(output_path, 'w') as f:
            json.dump(params, f, indent=2)
        return params

    def apply_correction(self, raw_depth, temp, params):
        """应用标定参数校正深度"""
        # 深度偏移校正
        corrected = raw_depth / 1000.0 - params["depth_offset_m"]
        # 温度补偿
        corrected -= params["temp_coeff_m_per_c"] * (temp - params["ref_temp_c"])
        return corrected * 1000.0  # 转回mm

def main():
    """标定主流程"""
    # 1. 加载采集数据(示例路径, 实际使用时替换为真实数据)
    gray_files = sorted(glob.glob("calib_data/gray_*.png"))
    depth_files = sorted(glob.glob("calib_data/depth_*.npy"))
    temp_log = np.loadtxt("calib_data/temp_log.csv", delimiter=",")

    if len(gray_files) != len(depth_files):
        raise ValueError("灰度图与深度图数量不匹配")

    gray_images = [cv2.imread(f, cv2.IMREAD_GRAYSCALE) for f in gray_files]
    depth_images = [np.load(f) for f in depth_files]

    # 2. 初始化标定器
    calib = ToFCalibrator(board_size=(9, 6), square_size_mm=30.0)

    # 3. 标定内参与深度偏移
    mtx, dist, depth_offset = calib.calibrate_intrinsics(gray_images, depth_images)

    # 4. 温度补偿拟合(使用采集时记录的温度与对应深度误差)
    temps = temp_log[:, 0]
    depth_errors = temp_log[:, 1]
    temp_coeff = calib.fit_temperature_coeff(temps, depth_errors)

    # 5. 保存参数
    params = calib.save_params(mtx, dist, depth_offset, temp_coeff,
                               "tof_calib_params.json")
    print(f"标定完成: 深度偏移={depth_offset*1000:.2f}mm, "
          f"温度系数={temp_coeff*1000:.3f}mm/°C")

    # 6. 验证示例(使用第一帧数据)
    test_depth = depth_images[0]
    corrected = calib.apply_correction(test_depth, 30.0, params)
    print(f"校正前中心深度: {test_depth[240, 320]}mm, "
          f"校正后: {corrected[240, 320]:.1f}mm")

if __name__ == "__main__":
    main()

数据采集规范

标定数据采集需满足以下条件：棋盘格靶标尺寸为30mm方格，9×6内角点；拍摄距离覆盖0.5m至3m范围，至少5个距离档位；每个距离下旋转棋盘格至不同姿态（俯仰±30°、偏航±30°）；同时记录传感器温度与对应深度误差样本。采集时保持环境光照稳定，避免强反射物体进入视场。

验证方法与判定标准

完成标定后，使用独立测试集（不参与标定的数据）进行验证：

深度精度测试：将靶标置于已知距离（如1.0m、2.0m），采集100帧深度数据，计算校正后深度均值与真实距离的误差，判定标准为误差小于真实距离的0.5%；
平面拟合残差：对校正后的深度图拟合平面，计算点到平面的均方根残差，判定标准为小于3mm；
温度稳定性：在15°C至45°C温度范围内测试，校正后深度漂移不超过±2mm。

运行python tof_calibrator.py执行标定，程序输出标定参数文件tof_calib_params.json。验证时调用apply_correction()函数处理测试数据，对比校正前后的深度误差变化。若验证不通过，需补充更多姿态和距离的标定数据后重新标定。

📚 课程目录