机器人 / 系统课时

读取 Image 与相机内参信息

读取 Image 与相机内参信息

本篇使用 ROS 2 Jazzy 同时订阅原始图像与相机标定消息,读出分辨率、像素值、焦距、主点和畸变参数,再用具体坐标验证投影关系。完成后可判断图像与内参是否适合用于后续视觉测量。

学习目标

  • 找到 Image 和 CameraInfo 话题,辨别图像数据与几何标定信息。
  • 读懂内参矩阵 k、畸变系数 d,计算空间点的理想像素位置。
  • 用 Python 节点读取首帧数据并检查标定有效性。

确认相机的话题和消息字段

适用环境为 Ubuntu 24.04、ROS 2 Jazzy,且相机驱动或仿真传感器已经启动。先在新终端执行:

bash
source /opt/ros/jazzy/setup.bash
ros2 topic list -t

预期输出(示例仅摘取相关两行,实际名称可能不同):

text
/camera/camera_info [sensor_msgs/msg/CameraInfo]
/camera/image_raw [sensor_msgs/msg/Image]

Image 保存真实像素数据:width、height 是图像尺寸;encoding 如 rgb8、bgr8 或 mono8 决定通道顺序;step 是每行字节数,不是像素宽度;data 是逐行排列的字节。header.stamp 表示采集时间,header.frame_id 标明相机坐标系。

CameraInfo 则存放几何信息:k 是 9 个数的内参矩阵,d 是畸变系数,distortion_model 指定畸变模型;r 与 p 用于校正及校正图像的投影。代码中的字段名是小写的 k、d、p。

从三维相机坐标计算像素位置

像素原点在图像左上角,横向为 uu、向下为 vv;标准相机光学坐标系的 XX 向右、YY 向下、ZZ 朝前。对深度 Z>0Z>0 的点,忽略畸变时:

u=fxXZ+cx,v=fyYZ+cyu=f_x\frac{X}{Z}+c_x,\qquad v=f_y\frac{Y}{Z}+c_y

k 按行优先展开,因此 k[0] 是 fxf_x、k[4] 是 fyf_y、k[2] 是 cxc_x、k[5] 是 cyc_y。焦距和主点在这里都以像素为单位,而 X,Y,ZX,Y,Z 可用米表示。

例如图像为 640×480,fx=fy=600f_x=f_y=600、cx=320c_x=320、cy=240c_y=240;空间点 (X,Y,Z)=(0.10,0.05,1.00)(X,Y,Z)=(0.10,0.05,1.00) 米,投影为 (u,v)=(380,270)(u,v)=(380,270),落在有效像素范围内。

相机光学坐标、主点与空间点投影关系

订阅图像与内参并读取中心像素

将以下完整程序保存为 camera_probe.py。示例话题名需按本机 ros2 topic list -t 修改;使用传感器 QoS,可兼容常见的 BEST_EFFORT 相机发布者。程序仅打印收到的第一帧图像和第一份内参,避免持续刷屏。保存后运行 python3 camera_probe.py,按 Ctrl+C 退出。

python
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CameraInfo, Image

class CameraProbe(Node):
    def __init__(self):
        super().__init__("camera_probe")
        self.seen_image = False
        self.seen_info = False
        self.create_subscription(
            Image, "/camera/image_raw", self.on_image, qos_profile_sensor_data
        )
        self.create_subscription(
            CameraInfo, "/camera/camera_info", self.on_info, qos_profile_sensor_data
        )

    def on_image(self, msg):
        if self.seen_image:
            return
        self.seen_image = True
        self.get_logger().info(
            f"Image {msg.width}x{msg.height}, encoding={msg.encoding}, "
            f"step={msg.step}, bytes={len(msg.data)}, frame={msg.header.frame_id}, "
            f"stamp={msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
        )
        # 8位编码可按每行字节跨度定位;其他编码需要单独解码
        channels = {"mono8": 1, "rgb8": 3, "bgr8": 3}.get(msg.encoding)
        if channels and msg.width and msg.height:
            u, v = msg.width // 2, msg.height // 2
            offset = v * msg.step + u * channels
            if offset + channels <= len(msg.data):
                self.get_logger().info(
                    f"pixel({u},{v})={list(msg.data[offset:offset + channels])}"
                )

    def on_info(self, msg):
        if self.seen_info:
            return
        self.seen_info = True
        # k是按行优先排列的3×3矩阵
        self.get_logger().info(
            f"CameraInfo {msg.width}x{msg.height}, fx={msg.k[0]:.1f}, "
            f"fy={msg.k[4]:.1f}, cx={msg.k[2]:.1f}, cy={msg.k[5]:.1f}, "
            f"frame={msg.header.frame_id}, "
            f"stamp={msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
        )
        self.get_logger().info(
            f"model={msg.distortion_model}, D={list(msg.d)}"
        )
        if msg.k[0] == 0.0:
            self.get_logger().warn("Uncalibrated: k[0] is zero")

def main():
    rclpy.init()
    node = CameraProbe()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

if __name__ == "__main__":
    main()

预期输出(假设设备发布 640×480 的 RGB8 图像及下列示例标定值;时间、图像内容和回调先后顺序因设备而异):

text
[INFO] [<TIME>] [camera_probe]: Image 640x480, encoding=rgb8, step=1920, bytes=921600, frame=camera_optical_frame, stamp=<TIME>
[INFO] [<TIME>] [camera_probe]: pixel(320,240)=[12, 34, 56]
[INFO] [<TIME>] [camera_probe]: CameraInfo 640x480, fx=600.0, fy=600.0, cx=320.0, cy=240.0, frame=camera_optical_frame, stamp=<TIME>
[INFO] [<TIME>] [camera_probe]: model=plumb_bob, D=[0.0, 0.0, 0.0, 0.0, 0.0]

核对是否具备可用的几何数据

  1. 检查标定:k[0] == 0 通常表示相机尚未标定。应加载该镜头对应的标定配置,不能自行填入看似合理的焦距。
  2. 检查分辨率:无裁剪、无像素合并时,Image 与 CameraInfo 的宽高应匹配;如启用了 ROI 或 binning,须按标定坐标和实际输出尺寸正确处理,不能简单沿用旧参数。
  3. 检查像素存储:通常应满足 len(data) == step × height;rgb8 每像素占 3 字节,但行尾可能有填充,因此索引使用 step 而不是 width × 3。
  4. 检查时间与坐标:实际融合时比较两类消息的 header.stamp 和 frame_id。本例只读取各自的首条消息,没有保证它们来自同一帧;需要逐帧关联时应使用时间戳同步。
  5. 检查原始与校正图像:原始画面使用 k、d 解释畸变;经过去畸变或立体校正的图像,应使用与其实际处理过程匹配的 p、r 及图像尺寸。

能收到两类消息,只证明数据通路存在;只有标定有效、图像尺寸对应、时间与坐标一致,后续的像素测距和三维定位才有可信的几何依据。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

读取 Image 与相机内参信息

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通