【双光相机配准】红外-可见光双光相机的坐标转换原理与实现

目录

• • 前置步骤
  • 深度估计原理与公式
  • 单应性矩阵（Homography）原理
  • • [Homography 的数学背景](#Homography 的数学背景)
    • 单应性与相机投影关系
    • 实现步骤
    • 实现代码
  • 深度估计+Homography坐标转换代码

前置步骤

1. 需要计算出可见光相机的内参，参考：
   【双光相机配准】可见光相机内参标定流程

   获取到内参文件
   vis_camera_calibration_{timestamp}.json

2. 需要计算出红外-可见光双光相机在不同距离下的Homography单应性矩阵，参考：
   【双光相机配准】可见光与红外相机计算Homography

获取到双光相机在多个距离下的Homography单应性矩阵文件：homography.yaml

深度估计原理与公式

背景原理：针孔相机模型

计算深度/距离的基础就是 针孔相机模型（Pinhole Camera Model）。

它的基本投影关系是：

其中：

X,Y,Z：物体在真实世界坐标系中的三维坐标（Z 是深度/距离）。

𝑥,𝑦：物体在相机成像平面上的二维坐标（像素坐标）。

𝑓：相机焦距（以像素为单位，pixel）。

应用到物体宽度/高度：

物体真实宽度（单位 mm）：Wreal​

物体在图像中的像素宽度：Wpixel​

焦距（像素单位，px）：f

方法1：使用宽度/高度计算

    def estimate_distance_from_bbox(self, bbox_vis, real_width_mm, real_height_mm, use_width=True):
        """
        使用像素尺寸和真实尺寸估计距离
        单位统一为毫米
        返回距离 cm
        """
        x1, y1, x2, y2 = bbox_vis
        pixel_size = (x2 - x1) if use_width else (y2 - y1)
        real_size_mm = real_width_mm if use_width else real_height_mm

        # 深度公式：distance_mm = (real_size_mm * focal_length_px_per_mm) / pixel_size_px
        distance_mm = (real_size_mm * self.focal_length_px_per_mm) / pixel_size
        distance_cm = distance_mm / 10.0  # 转 cm
        return distance_cm

方法2：独立计算，再取平均

 def estimate_distance_avg(self, bbox_vis, real_width_mm, real_height_mm):
        """
        方法2: 宽度和高度分别计算距离, 然后取平均
        单位统一为毫米
        返回距离 cm
        """
        x1, y1, x2, y2 = bbox_vis
        pixel_w = x2 - x1
        pixel_h = y2 - y1

        # 宽度方向估计
        dist_w = (real_width_mm * self.focal_length_px_per_mm) / pixel_w
        # 高度方向估计
        dist_h = (real_height_mm * self.focal_length_px_per_mm) / pixel_h

        # 平均
        distance_mm = 0.5 * (dist_w + dist_h)
        return distance_mm / 10.0  # 转 cm

方法3：面积法（更常见）

基于物体的 投影面积：

def estimate_distance_area(self, bbox_vis, real_width_mm, real_height_mm):
        """
        方法3: 基于面积比计算距离
        单位统一为毫米
        返回距离 cm
        """
        x1, y1, x2, y2 = bbox_vis
        pixel_w = x2 - x1
        pixel_h = y2 - y1

        # 投影面积
        pixel_area = pixel_w * pixel_h
        real_area_mm2 = real_width_mm * real_height_mm

        # 深度公式：Z = f * sqrt(real_area / pixel_area)
        distance_mm = self.focal_length_px_per_mm * (real_area_mm2 / pixel_area) ** 0.5
        return distance_mm / 10.0  # 转 cm

单应性矩阵（Homography）原理

Homography 的数学背景

在平面几何或透视投影下，两个图像之间的点对应关系可以用 单应性矩阵 H 表示：

(𝑥,𝑦)：源图像的点坐标（齐次坐标表示为 [𝑥,𝑦,1]𝑇

(𝑥′,𝑦′)：目标图像的点坐标

𝐻：3×3 的单应性矩阵，只有 8 个自由度（因为整体尺度不唯一）

单应性与相机投影关系

如果场景点位于某个平面（比如标定板所在平面），那么该平面在两个相机图像之间的关系可用 Homography 表示。

对于相机内参矩阵 𝐾，外参 [𝑅∣𝑡]，有：

当场景点约束在 Z = Z₀ 平面 时，可以消去一维，从而得到 平面到平面的关系，推导为：

其中：

𝐾1,𝐾2：两个相机的内参

𝑅,𝑡：相机间的相对姿态

𝑛,𝑑：平面法向量与距离

实现步骤

1. 取可见光框的角点 corners

        corners = [(x1,y1),(x2,y1),(x2,y2),(x1,y2)]

2. 先用一个点估计距离（map_point_with_depth → 得到 distance_cm）

3. 获取深度对应的homography

4. 把点从 vis 分辨率 → calib 分辨率
   

5. 应用 Homography：
   

6. 把点从 calib 分辨率 → 目标 IR 分辨率
   
   7.收集四个角点，取 min/max → IR 框

实现代码

  def map_bbox_with_depth(self, bbox_vis, real_width_mm, real_height_mm, 
                            vis_res=(1280,720), ir_res=(640,480), 
                            calib_vis_res=(1280,720), calib_ir_res=(1280,960)):
        """
        将可见光 bbox 映射到 IR bbox，返回 IR bbox 和距离（cm）
        """
        x1, y1, x2, y2 = bbox_vis
        corners = [(x1,y1),(x2,y1),(x2,y2),(x1,y2)]
        first_point, distance_cm = self.map_point_with_depth(corners[0], real_width_mm, real_height_mm, bbox_vis,
                                                            vis_res, ir_res, calib_vis_res, calib_ir_res)
        H = self.get_interpolated_homography(distance_cm)
        mapped_corners = []
        for pt in corners:
            scale_vis_x, scale_vis_y = calib_vis_res[0]/vis_res[0], calib_vis_res[1]/vis_res[1]
            p = np.array([pt[0]*scale_vis_x, pt[1]*scale_vis_y, 1.0], dtype=np.float64)
            p2 = H.dot(p)
            p2 /= p2[2]
            x_ir = p2[0] * ir_res[0]/calib_ir_res[0]
            y_ir = p2[1] * ir_res[1]/calib_ir_res[1]
            mapped_corners.append((x_ir, y_ir))
        xs = [p[0] for p in mapped_corners]
        ys = [p[1] for p in mapped_corners]
        return (min(xs), min(ys), max(xs), max(ys)), distance_cm

深度估计+Homography坐标转换代码

需要输入参数：

5. VIS_CALIB_JSON表示相机内参文件（vis_camera_calibration_{timestamp}.json）路径，获取详见上一章；

6. HOMOGRAPHY_YAML表示多个距离下的Homography单应性矩阵文件（homography.yaml）路径，获取详见上一章；

7. real_width_mm和real_height_mm表示带转换坐标的目标真实的宽高尺寸，用于深度估计，单位mm。

8. bbox_vis表示待转换的目标在可见光图像的bbox坐标，例如(585, 412, 600, 427)

完整代码：

import numpy as np
import cv2
import os
import json

class DepthAwareHomography:
    def __init__(self, homography_yaml_path, camera_intrinsics_path=None, camera_type="ir"):
        """
        深度感知 Homography 映射器（单位统一为毫米）
        """
        self.homography_dict = self.load_homography_matrices(homography_yaml_path)
        self.distances = sorted([int(k[1:]) for k in self.homography_dict.keys()])  # cm
        self.camera_type = camera_type
        
        # 加载相机内参
        if camera_intrinsics_path and os.path.exists(camera_intrinsics_path):
            self.camera_matrix, self.focal_length_px_per_mm = self.load_camera_intrinsics_from_json(camera_intrinsics_path)
            print(f"✅ 已从 {camera_intrinsics_path} 加载 {self.camera_type.upper()} 相机内参")
        else:
            self.focal_length_px_per_mm = 1000  # 默认焦距 px/mm
            self.camera_matrix = None
            print("⚠️ 使用默认焦距值，建议提供相机内参JSON文件")
        
        # 主点默认值
        self.principal_point = (640, 480)
        print(f"加载的标定距离范围: {min(self.distances)}cm ~ {max(self.distances)}cm")

    def load_homography_matrices(self, yaml_path):
        """加载不同距离的 Homography 矩阵"""
        fs = cv2.FileStorage(yaml_path, cv2.FILE_STORAGE_READ)
        homography_dict = {}
        for d in range(90, 170, 10):
            key = f'D{d}'
            node = fs.getNode(key)
            if not node.empty():
                H_node = node.getNode("H")
                if not H_node.empty():
                    homography_dict[key] = H_node.mat()
        fs.release()
        return homography_dict

    def load_camera_intrinsics_from_json(self, json_path):
        """加载相机内参"""
        with open(json_path, 'r') as f:
            calib_data = json.load(f)
        
        camera_matrix = np.array(calib_data["camera_matrix"])
        if "focal_length_px_per_mm" in calib_data:
            focal_length_px_per_mm = calib_data["focal_length_px_per_mm"]
        else:
            fx, fy = camera_matrix[0,0], camera_matrix[1,1]
            focal_length_px_per_mm = (fx + fy) / 2
        print(f"📊 标定焦距: {focal_length_px_per_mm:.2f} px/mm")
        return camera_matrix, focal_length_px_per_mm

    def estimate_distance_from_bbox(self, bbox_vis, real_width_mm, real_height_mm, use_width=True):
        """
        使用像素尺寸和真实尺寸估计距离
        单位统一为毫米
        返回距离 cm
        """
        x1, y1, x2, y2 = bbox_vis
        pixel_size = (x2 - x1) if use_width else (y2 - y1)
        real_size_mm = real_width_mm if use_width else real_height_mm

        # 深度公式：distance_mm = (real_size_mm * focal_length_px_per_mm) / pixel_size_px
        distance_mm = (real_size_mm * self.focal_length_px_per_mm) / pixel_size
        distance_cm = distance_mm / 10.0  # 转 cm
        return distance_cm

    def get_interpolated_homography(self, distance_cm):
        """
        根据距离插值得到 Homography 矩阵
        """
        distance_cm = max(min(distance_cm, self.distances[-1]), self.distances[0])
        distances = np.array(self.distances)
        idx = np.abs(distances - distance_cm).argmin()
        nearest_dist = distances[idx]
        if abs(distance_cm - nearest_dist) < 0.5:
            return self.homography_dict[f'D{int(nearest_dist)}']
        if distance_cm < nearest_dist:
            dist1, dist2 = nearest_dist - 10, nearest_dist
        else:
            dist1, dist2 = nearest_dist, nearest_dist + 10
        dist1, dist2 = max(dist1, self.distances[0]), min(dist2, self.distances[-1])
        if dist1 == dist2:
            return self.homography_dict[f'D{int(dist1)}']
        H1, H2 = self.homography_dict[f'D{int(dist1)}'], self.homography_dict[f'D{int(dist2)}']
        H1, H2 = H1 / H1[2,2], H2 / H2[2,2]
        alpha = (distance_cm - dist1) / (dist2 - dist1)
        H_interp = (1-alpha)*H1 + alpha*H2
        H_interp = H_interp / H_interp[2,2]
        return H_interp

    def map_point_with_depth(self, pt_vis, real_width_mm, real_height_mm, bbox_vis, 
                             vis_res=(1280,720), ir_res=(640,480), 
                             calib_vis_res=(1280,720), calib_ir_res=(1280,960)):
        """
        将可见光单点映射到 IR 坐标，返回 IR 坐标和估计距离（cm）
        """
        distance_cm = self.estimate_distance_from_bbox(bbox_vis, real_width_mm, real_height_mm)
        H = self.get_interpolated_homography(distance_cm)
        scale_vis_x, scale_vis_y = calib_vis_res[0]/vis_res[0], calib_vis_res[1]/vis_res[1]
        p = np.array([pt_vis[0]*scale_vis_x, pt_vis[1]*scale_vis_y, 1.0], dtype=np.float64)
        p2 = H.dot(p)
        p2 /= p2[2]
        x_ir = p2[0] * ir_res[0]/calib_ir_res[0]
        y_ir = p2[1] * ir_res[1]/calib_ir_res[1]
        return (float(x_ir), float(y_ir)), distance_cm

    def map_bbox_with_depth(self, bbox_vis, real_width_mm, real_height_mm, 
                            vis_res=(1280,720), ir_res=(640,480), 
                            calib_vis_res=(1280,720), calib_ir_res=(1280,960)):
        """
        将可见光 bbox 映射到 IR bbox，返回 IR bbox 和距离（cm）
        """
        x1, y1, x2, y2 = bbox_vis
        corners = [(x1,y1),(x2,y1),(x2,y2),(x1,y2)]
        first_point, distance_cm = self.map_point_with_depth(corners[0], real_width_mm, real_height_mm, bbox_vis,
                                                            vis_res, ir_res, calib_vis_res, calib_ir_res)
        H = self.get_interpolated_homography(distance_cm)
        mapped_corners = []
        for pt in corners:
            scale_vis_x, scale_vis_y = calib_vis_res[0]/vis_res[0], calib_vis_res[1]/vis_res[1]
            p = np.array([pt[0]*scale_vis_x, pt[1]*scale_vis_y, 1.0], dtype=np.float64)
            p2 = H.dot(p)
            p2 /= p2[2]
            x_ir = p2[0] * ir_res[0]/calib_ir_res[0]
            y_ir = p2[1] * ir_res[1]/calib_ir_res[1]
            mapped_corners.append((x_ir, y_ir))
        xs = [p[0] for p in mapped_corners]
        ys = [p[1] for p in mapped_corners]
        return (min(xs), min(ys), max(xs), max(ys)), distance_cm

    def show_vis2ir_result_with_depth(self, bbox, ir_path, distance_cm, actual_distance_cm=None):
        """
        可视化 IR 图像和映射结果
        """
        ir_image = cv2.imread(ir_path)
        if ir_image is None:
            print(f"❌ 无法读取图像: {ir_path}")
            return
        ir_image = cv2.resize(ir_image, (640,480))
        cv2.rectangle(ir_image, (int(bbox[0]), int(bbox[1])), (int(bbox[2]), int(bbox[3])), (255,255,255), 3)
        cv2.putText(ir_image, f"Est: {distance_cm:.1f}cm", (10,30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
        if actual_distance_cm:
            cv2.putText(ir_image, f"Actual: {actual_distance_cm}cm", (10,60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
            cv2.putText(ir_image, f"Error: {abs(distance_cm - actual_distance_cm):.1f}cm", (10,90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
        output_dir = 'output'
        os.makedirs(output_dir, exist_ok=True)
        output_path = os.path.join(output_dir, f"depth_{os.path.basename(ir_path)}")
        cv2.imwrite(output_path, ir_image)
        print(f"✅ 保存图像到: {output_path}")


# =================== 测试示例 ===================
if __name__ == "__main__":
    HOMOGRAPHY_YAML = "homography.yaml"
    VIS_CALIB_JSON = "vis_camera_calibration_20250923_160641.json"
    
    mapper = DepthAwareHomography(HOMOGRAPHY_YAML, camera_intrinsics_path=VIS_CALIB_JSON, camera_type="ir")
    
    
    # bbox_vis = (664, 242, 694, 271)  # 80cm 可见光 bbox
    # bbox_vis = (909, 257, 937, 283)  # 90 cm 可见光 bbox
    # bbox_vis = (707, 446, 729, 468)  # 100cm 可见光 bbox
    # bbox_vis = (839, 265, 858, 289)  # 110cm 可见光 bbox
    # bbox_vis = (621, 269, 640, 289)  # 120cm 可见光 bbox
    # bbox_vis = (748, 291, 766, 309)  # 130cm 可见光 bbox
    bbox_vis = (585, 412, 600, 427)  # 140cm 可见光 bbox
    # bbox_vis = (709, 410, 723, 425)  # 150 cm 可见光 bbox

    real_width_mm = 20.0   # mm
    real_height_mm = 20.0  # mm
    
    bbox_ir, estimated_distance_cm = mapper.map_bbox_with_depth(bbox_vis, real_width_mm, real_height_mm,
                                                                vis_res=(1280,720), ir_res=(640,480))
    
    print(f"估计距离: {estimated_distance_cm:.1f}cm, Vis bbox: {bbox_vis}, IR bbox: {bbox_ir}")
    
    # ir_path = '250922/80/ir/80_1.png'
    # ir_path = '250922/90/ir/90_4.png'
    # ir_path = '250922/100/ir/100_3.png'
    # ir_path = '250922/110/ir/110_3.png'
    # ir_path = '250922/120/ir/120_2.png'
    # ir_path = '250922/130/ir/130_2.png'
    ir_path = '250922/140/ir/140_2.png'
    # ir_path = '250922/150/ir/150_2.png'
    mapper.show_vis2ir_result_with_depth(bbox_ir, ir_path, estimated_distance_cm, actual_distance_cm=140)