机器人 / 系统课时
读取 Image 与相机内参信息
读取 Image 与相机内参信息
本篇使用 ROS 2 Jazzy 同时订阅原始图像与相机标定消息,读出分辨率、像素值、焦距、主点和畸变参数,再用具体坐标验证投影关系。完成后可判断图像与内参是否适合用于后续视觉测量。
学习目标
- 找到
Image和CameraInfo话题,辨别图像数据与几何标定信息。 - 读懂内参矩阵
k、畸变系数d,计算空间点的理想像素位置。 - 用 Python 节点读取首帧数据并检查标定有效性。
确认相机的话题和消息字段
适用环境为 Ubuntu 24.04、ROS 2 Jazzy,且相机驱动或仿真传感器已经启动。先在新终端执行:
source /opt/ros/jazzy/setup.bash
ros2 topic list -t预期输出(示例仅摘取相关两行,实际名称可能不同):
/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。
从三维相机坐标计算像素位置
像素原点在图像左上角,横向为 、向下为 ;标准相机光学坐标系的 向右、 向下、 朝前。对深度 的点,忽略畸变时:
k 按行优先展开,因此 k[0] 是 、k[4] 是 、k[2] 是 、k[5] 是 。焦距和主点在这里都以像素为单位,而 可用米表示。
例如图像为 640×480,、、;空间点 米,投影为 ,落在有效像素范围内。

订阅图像与内参并读取中心像素
将以下完整程序保存为 camera_probe.py。示例话题名需按本机 ros2 topic list -t 修改;使用传感器 QoS,可兼容常见的 BEST_EFFORT 相机发布者。程序仅打印收到的第一帧图像和第一份内参,避免持续刷屏。保存后运行 python3 camera_probe.py,按 Ctrl+C 退出。
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 图像及下列示例标定值;时间、图像内容和回调先后顺序因设备而异):
[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]核对是否具备可用的几何数据
- 检查标定:
k[0] == 0通常表示相机尚未标定。应加载该镜头对应的标定配置,不能自行填入看似合理的焦距。 - 检查分辨率:无裁剪、无像素合并时,
Image与CameraInfo的宽高应匹配;如启用了 ROI 或 binning,须按标定坐标和实际输出尺寸正确处理,不能简单沿用旧参数。 - 检查像素存储:通常应满足
len(data) == step × height;rgb8每像素占 3 字节,但行尾可能有填充,因此索引使用step而不是width × 3。 - 检查时间与坐标:实际融合时比较两类消息的
header.stamp和frame_id。本例只读取各自的首条消息,没有保证它们来自同一帧;需要逐帧关联时应使用时间戳同步。 - 检查原始与校正图像:原始画面使用
k、d解释畸变;经过去畸变或立体校正的图像,应使用与其实际处理过程匹配的p、r及图像尺寸。
能收到两类消息,只证明数据通路存在;只有标定有效、图像尺寸对应、时间与坐标一致,后续的像素测距和三维定位才有可信的几何依据。