当前位置：首页 > news >正文

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

news 2025/9/25 13:23:21

前置步骤

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

获取到内参文件
vis_camera_calibration_{timestamp}.json
需要计算出红外-可见光双光相机在不同距离下的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_vispixel_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_pxdistance_mm = (real_size_mm * self.focal_length_px_per_mm) / pixel_sizedistance_cm = distance_mm / 10.0  # 转 cmreturn distance_cm

方法2：独立计算，再取平均
在这里插入图片描述

 def estimate_distance_avg(self, bbox_vis, real_width_mm, real_height_mm):"""方法2: 宽度和高度分别计算距离, 然后取平均单位统一为毫米返回距离 cm"""x1, y1, x2, y2 = bbox_vispixel_w = x2 - x1pixel_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_vispixel_w = x2 - x1pixel_h = y2 - y1# 投影面积pixel_area = pixel_w * pixel_hreal_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.5return distance_mm / 10.0  # 转 cm

单应性矩阵（Homography）原理

Homography 的数学背景

在平面几何或透视投影下，两个图像之间的点对应关系可以用单应性矩阵 H 表示：
在这里插入图片描述
(𝑥,𝑦)：源图像的点坐标（齐次坐标表示为 [𝑥,𝑦,1]𝑇
(𝑥′,𝑦′)：目标图像的点坐标
𝐻：3×3 的单应性矩阵，只有 8 个自由度（因为整体尺度不唯一）

单应性与相机投影关系

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

对于相机内参矩阵 𝐾，外参 [𝑅∣𝑡]，有：
在这里插入图片描述
当场景点约束在 Z = Z₀ 平面时，可以消去一维，从而得到平面到平面的关系，推导为：

其中：
𝐾1,𝐾2：两个相机的内参
𝑅,𝑡：相机间的相对姿态
𝑛,𝑑：平面法向量与距离

实现步骤

取可见光框的角点 corners

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

先用一个点估计距离（map_point_with_depth → 得到 distance_cm）
获取深度对应的homography
把点从 vis 分辨率 → calib 分辨率
应用 Homography：
把点从 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_viscorners = [(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 jsonclass 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()])  # cmself.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/mmself.camera_matrix = Noneprint("⚠️ 使用默认焦距值，建议提供相机内参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_dictdef 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) / 2print(f"📊 标定焦距: {focal_length_px_per_mm:.2f} px/mm")return camera_matrix, focal_length_px_per_mmdef estimate_distance_from_bbox(self, bbox_vis, real_width_mm, real_height_mm, use_width=True):"""使用像素尺寸和真实尺寸估计距离单位统一为毫米返回距离 cm"""x1, y1, x2, y2 = bbox_vispixel_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_pxdistance_mm = (real_size_mm * self.focal_length_px_per_mm) / pixel_sizedistance_cm = distance_mm / 10.0  # 转 cmreturn distance_cmdef 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_distelse:dist1, dist2 = nearest_dist, nearest_dist + 10dist1, 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*H2H_interp = H_interp / H_interp[2,2]return H_interpdef 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_cmdef 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_viscorners = [(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_cmdef 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}")returnir_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 可见光 bboxbbox_vis = (585, 412, 600, 427)  # 140cm 可见光 bbox# bbox_vis = (709, 410, 723, 425)  # 150 cm 可见光 bboxreal_width_mm = 20.0   # mmreal_height_mm = 20.0  # mmbbox_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)

查看全文

http://www.dtcms.com/a/402908.html