第 8 章 相机标定与机器视觉¶
相机内参（焦距、主点、畸变系数）决定图像像素与三维光线的映射关系，是所有视觉算法的输入前提。外参（相机与机器人基座之间的位姿关系）决定检测到的目标如何映射到机器人坐标系。本章给出内参标定的操作流程、cv_bridge 将 ROS 图像接入 OpenCV 的标准路径，以及 AprilTag 检测作为定位基准的最小实操。
1. 针孔模型与畸变¶
针孔相机模型将三维点投影到像素平面：
\(K\) 为内参矩阵（focal length + principal point）。实际镜头引入径向畸变（桶形/枕形）与切向畸变，OpenCV 用 5 个系数描述：
sensor_msgs/CameraInfo 消息中 k 存储 \(K\)（9 元素行展开），d 存储畸变系数，projection_matrix 是 3×4 投影矩阵（含平移，用于双目视差计算）。
2. camera_calibration 内参标定¶
2.1 标定板与采集¶
标准做法使用棋盘格（checkerboard）或圆点板（Circle Grid）。棋盘格参数：内角点数（--size，如 8x6 表示横向 8 内角点、纵向 6 内角点）与方格边长（--square，单位米）。
2.2 操作步骤¶
## 安装标定包（Humble：ros-humble-camera-calibration）
sudo apt install -y ros-humble-camera-calibration
## 启动标定器（可执行名 cameracalibrator，非包名）——棋盘格参数为内角点数与方格边长
## 图像与相机信息经标准话题 image / camera_info 重映射注入
ros2 run camera_calibration cameracalibrator \
--size 8x6 --square 0.025 \
--ros-args -r image:=/camera/image_raw -r camera_info:=/camera/camera_info
--size NxM 数的是棋盘格内角点（非方格数），--square 为方格边长（米）；标定板图案用 -p（chessboard / circles / acircles / charuco），畸变阶数等由 -k/--zero-tangent-dist 等修饰。cameracalibrator 是 OpenCV GUI 工具，需显示与实时图像流。
操作界面中：
1. 从多角度、多距离采集约 20~50 帧标定板图像（点 Add 或直接空格）；
2. 采集完成后点 Save → 自动求解并显示结果；
3. 点 Write 导出标定产物（Humble 版输出 camera_info.yaml 与打包的标定数据，文件名以实际版本为准）。
标定结果的评判标准：重投影误差（reprojection error） 小于 0.5 像素视为合格。误差来源：标定板未平整贴合、拍摄角度变化不够（全部正面平行拍摄导致参数耦合）、帧数不足。
2.3 标定结果应用¶
标定参数可通过 launch 文件或参数服务注入相机驱动节点，使 /camera_info 实时携带正确的内参：
## camera_params.yaml（由标定结果导出；节点名按实际驱动节点调整）
camera_camera:
ros__parameters:
camera_info_url: "file:///home/user/calibration.yaml"
camera_info_url 参数是否存在由具体相机驱动决定（usb_cam 与 realsense2 的实现不同）；更通用的做法是直接发布带正确内参的 CameraInfo 话题。下游算法（点云投影、视觉测距）从 /camera_info 读取内参，标定参数不准则三维重建有系统性偏差。
标定结果的几何含义在建模侧：相机模型、畸变与标定的数学推导见 camera-note 第 1 章「相机模型与标定（算法侧）」。
坑：camera_calibration 与 NumPy 2 不兼容
Humble 的 cameracalibrator 依赖按 NumPy 1.x 编译的 cv_bridge，NumPy 2 环境下报 _ARRAY_API not found；装得上不等于能跑，需 numpy<2 或重建 cv_bridge。
3. 外参与手眼标定概览¶
外参描述相机与机器人基座（或末端）之间的固定位姿关系。两种典型配置：
- Eye-to-Hand：相机固定在基座上，观测工作空间。标定即确定
base_link → camera_link的 TF。 - Eye-in-Hand：相机安装在机械臂末端。标定目标为
tool_frame → camera_frame（手眼变换）。
手眼标定求解 \(AX = XB\) 问题：机器人经过 N 次运动，每次给出末端位姿变化 \(A_i\)（基座/工具系下）与相机观测的标定板位姿变化 \(B_i\)（相机系下），求固定的相机-末端变换 \(X\)。两条经典求解路线：分离式（先由旋转部分解出角轴、再解平移，如 Tsai-Lenz、Park-Martin）与对偶四元数法（把旋转平移耦合求解，如 Daniilidis、Chou-Liang）。标定质量取决于运动的激励：多次采样的旋转轴方向要足够分散且不平行（否则 \(AX=XB\) 欠定），一般需 ≥ 3 组有效运动对；位姿越分散、覆盖越接近真实工况，残差越小。
ROS 生态可用 easy_handeye2（Humble 无同名 apt 包，需源码构建）驱动整套采集与求解。更简化的场景——若相机固定且视野内可见已知尺寸的 AprilTag（「AprilTag 检测与定位」节），可通过 PnP 解算直接获得相机到基座的 6-DoF 位姿。
4. cv_bridge 与 OpenCV 接入¶
cv_bridge 在 ROS 图像消息与 OpenCV cv::Mat / NumPy 数组之间转换，是视觉算法接入 ROS 话题的标准胶水层。
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
class ImageProcessor(Node):
def __init__(self):
super().__init__('image_processor')
self.bridge = CvBridge()
self.sub = self.create_subscription(Image, '/camera/image_raw', self.cb, 10)
self.pub = self.create_publisher(Image, '/camera/edges', 10)
def cb(self, msg):
# ROS Image → OpenCV/NumPy 矩阵
cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# 处理：Canny 边缘检测
gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
edges = cv2.Canny(gray, 50, 150)
# OpenCV 矩阵 → ROS Image 再发布
out_msg = self.bridge.cv2_to_imgmsg(edges, encoding='mono8')
out_msg.header = msg.header # 必须透传时间戳与 frame_id
self.pub.publish(out_msg)
rclpy.init()
rclpy.spin(ImageProcessor())
encoding 字段指定通道与位深——bgr8、rgb8、mono8、32FC1（深度图）等。desired_encoding=None 时保持原编码，可能导致 uint16 深度图被当作 bgr16 处理——应显式指定。
不修改 header 是最常见的下游错误：处理后的图像丢失 stamp 与 frame_id，导致 message_filters 同步失败、TF 查询报错。
5. AprilTag 检测与定位¶
AprilTag 是视觉基准标记（类似二维码但专为位姿估计设计），已知物理尺寸 + 高鲁棒性检测使其成为机器人定位与标定的常用辅助手段。
5.1 ROS 2 接入¶
apriltag_ros（Humble 包 ros-humble-apriltag-ros）订阅图像并检测 AprilTag，节点名 /apriltag，在 /detections 发布 apriltag_msgs/msg/AprilTagDetectionArray：
## 节点订阅的是去畸变后的 image_rect 与 camera_info
ros2 run apriltag_ros apriltag_node \
--ros-args -r image_rect:=/camera/image_rect \
-p size:=0.065 -p family:=36h11
参数名是 size（单个标签边长，米）与 family（取值如 36h11、25h9，不带 tag 前缀）；不是 tag_size/tag_family。多标签带已知位姿时用 tag.ids/tag.sizes/tag.frames 列表，检测器调参在 detector.*（如 detector.decimate）。原始图像需先经 image_proc 去畸变，image_rect 才有数据。
5.2 从检测到位姿¶
检测到 AprilTag 的四个角点像素坐标后，结合内参与已知 tag 物理尺寸，用 PnP（solvePnP）解算 tag 在相机坐标系下的 6-DoF 位姿：
import numpy as np, cv2
## tag 四角在自身坐标系下的 3D 坐标（已知）
obj_pts = np.array([[-s/2, s/2, 0], [s/2, s/2, 0],
[s/2, -s/2, 0], [-s/2, -s/2, 0]]) # s = tag_size
## 检测到的四角像素坐标
img_pts = np.array(detected_corners)
## 内参矩阵 K 与畸变系数 d（来自 camera_info）
K = np.array(msg.K).reshape(3, 3)
d = np.array(msg.D)
## solvePnP 得到旋转与平移
_, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, d, flags=cv2.SOLVEPNP_IPPE_SQUARE)
## rvec → 旋转矩阵
R, _ = cv2.Rodrigues(rvec)
从相机系变换到机器人基座系需再乘以 base_link → camera_link 的 TF 变换（第 5 章）。
5.3 典型应用¶
- 多相机外参标定：两个相机同时看到同一 tag，分别算出 tag 在各自坐标系中的位姿，联立求解相机-相机变换。
- 绝对定位修正：底盘在路径关键点贴 tag，视觉检测到后修正里程计累积误差（与第 10 章 AMCL 类似但无需占用栅格地图）。
判据：检测输入先去畸变
检测与位姿解算都应在 image_rect 上做，内参与畸变系数从 camera_info 拿；拿原始畸变图做几何解算，框得再准位姿也是偏的。
6. 目标检测概览¶
基于深度学习的方法（YOLO、RT-DETR 等）在 ROS 中的典型接入模式：
flowchart LR
img["/image 话题"] --> cv["cv_bridge"] --> inf["推理引擎<br/>TensorRT / ONNX Runtime"] --> det["检测结果"] --> out["自定义话题 / Marker"]深度学习推理栈的版本依赖（CUDA、cuDNN、TensorRT）与 ROS 环境存在冲突风险，且模型选型与训练超出本板块范围。相关内容保持概览级，具体选型待核实。
工程侧需要关注的 ROS 接口约束：检测结果消息的 header.stamp 与原始图像一致，frame_id 正确——下游跟踪与融合依赖这两项。推理耗时超过图像发布间隔时，应使用 Best Effort QoS 主动丢帧，不堆积延迟。
7. 视觉可视化¶
视觉结果在 RViz2 与 Foxglove 中的展示方式：
| 需求 | 工具 | 实现方式 |
|---|---|---|
| 实时图像 | RViz2 Image Display / Foxglove Image Panel | 直接订阅 /camera/image_raw 或 /compressed |
| 检测结果标注 | image_markers 或自写 overlay → 新话题发布 |
在图像上画框/文字后发布 |
| 3D 定位标记 | visualization_msgs/Marker |
PnP 结果发布为 cube/arrow Marker（第 4 章） |
| 点云着色 | PointCloud2 + pcl 颜色通道 |
深度图 + 彩色图融合为 RGB 点云 |
| 标定过程监控 | OpenCV imshow（cv_bridge 直出） |
标定工具内部使用，不占 ROS 话题 |
Marker 可视化 AprilTag 定位结果的最小片段：
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Pose
marker = Marker()
marker.header.frame_id = 'base_link'
marker.ns = 'detected_tags'
marker.id = tag_id
marker.type = Marker.CUBE
marker.pose = Pose(position=tvec_in_base, orientation=quat_from_R)
marker.scale.x = marker.scale.y = 0.065 # tag 尺寸
marker.scale.z = 0.005
marker.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
8. 要点¶
- 内参标定（棋盘格 + camera_calibration）是视觉系统的起点；重投影误差 < 0.5 像素为合格判据，结果通过
/camera_info发布供全部下游使用。 - 外参/手眼标定将相机坐标系与机器人坐标系关联——Eye-to-Hand 是一次性标定，Eye-in-Hand 需每次更换末端后重标。
cv_bridge是 ROS 图像与 OpenCV 的唯一接口层——encoding必须显式指定、header必须透传，否则同步与 TF 查询失败。- AprilTag 提供已知尺寸的视觉基准，检测 + PnP 即可得到 6-DoF 相对位姿——适合定位修正与多相机标定。
- 目标检测推理在 ROS 中通过独立节点实现；推理超时时用 Best Effort QoS 丢帧，不堆延迟。
- 视觉结果可视化走 Marker（三维空间标注）或图像 overlay（二维画面标注），与第 4 章工具链衔接。