ROS2►Perception

camera_calibration_parsers 源码详细分析

camera_calibration_parsers 源码详细分析

工作区路径：/home/cp/work2/ros2Learn/ros2_humble/src/ros-perception/image_common/camera_calibration_parsers
版本：3.1.12，许可证 BSD。

camera_calibration_parsers 是 ROS 相机标定文件的 读写库：把磁盘上的 INI（Videre 遗留格式） 或 YAML（OpenCV/camera_calibration 格式） 与 sensor_msgs/msg/CameraInfo 互相转换。主要消费者是 camera_info_manager，相机驱动和 image_pipeline 通过它加载/保存内参。

1. 仓库结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
camera_calibration_parsers/
├── include/camera_calibration_parsers/
│   ├── parse.hpp              # 统一入口 API
│   ├── parse_ini.hpp          # INI 格式
│   ├── parse_yml.hpp          # YAML 格式
│   ├── parse.h / parse_ini.h / parse_yml.h   # 遗留 C 风格头（兼容）
│   └── visibility_control.hpp
├── src/
│   ├── parse.cpp              # 按扩展名分发
│   ├── parse_ini.cpp          # INI 解析/写入
│   ├── parse_yml.cpp          # YAML 解析/写入（yaml-cpp）
│   ├── convert.cpp            # CLI 格式转换工具
│   ├── parse_wrapper.cpp      # Python Boost 绑定（已禁用）
│   └── camera_calibration_parsers/__init__.py  # Python API（依赖已禁用的 wrapper）
├── test/
│   ├── test_parse_ini.cpp
│   ├── test_parse_yml.cpp
│   ├── calib5.ini / calib8.ini
│   └── make_calibs.hpp
├── CMakeLists.txt
└── package.xml

2. 在 image_common 栈中的位置

标定文件camera_calibration_parsersROS 2 消息消费者*.ini\nVidere 格式*.yml / *.yaml\nOpenCV 格式readCalibrationwriteCalibrationsensor_msgs/CameraInfo\nK, D, R, P, distortion_modelcamera_info_manager相机驱动\nusb_cam, v4l2...convert CLI
角色 说明
本包 文件 ↔ CameraInfo 序列化
camera_info_manager URL 解析、缓存、set_camera_info 服务
下游 图像 rectification、投影、立体视觉

3. 公共 API

3.1 统一入口（parse.hpp）

函数 作用
readCalibration(file, camera_name, cam_info) 按扩展名读 .ini / .yml / .yaml
writeCalibration(file, camera_name, cam_info) 按扩展名写
parseCalibration(buffer, format, ...) 从内存字符串解析（仅 ini）

目标类型统一为：

1
using CameraInfo = sensor_msgs::msg::CameraInfo;

3.2 格式分发（parse.cpp）

1
2
3
4
5
6
7
8
9
10
bool writeCalibration(...)
{
  if (p.extension() == ".ini")
    return writeCalibrationIni(...);
  else if (p.extension() == ".yml" || p.extension() == ".yaml")
    return writeCalibrationYml(...);
  else
    RCLCPP_ERROR(..., "calibration must be '.ini', '.yml', or '.yaml'");
  return false;
}

parseCalibration 的限制：

1
2
3
4
5
6
7
bool parseCalibration(const std::string & buffer, const std::string & format, ...)
{
  if (format != "ini") {
    return false;
  }
  return parseCalibrationIni(buffer, camera_name, cam_info);
}

内存解析不支持 YAML，只有 INI。

4. CameraInfo 字段映射

两种格式都填充 sensor_msgs/CameraInfo 的核心矩阵：

CameraInfo 字段 含义 尺寸
width, height 图像分辨率 —
k 相机内参矩阵 K 3×3
d 畸变系数 5 或 8
r 矫正矩阵 R 3×3
p 投影矩阵 P 3×4
distortion_model 畸变模型名 string

畸变模型（sensor_msgs/distortion_models.hpp）：

模型 常量 D 系数个数
Plumb Bob plumb_bob 5
Rational Polynomial rational_polynomial 8

5. INI 格式（Videre 遗留）

5.1 文件结构

示例 test/calib5.ini：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
[image]
width
640
height
480

[mono_left]
camera matrix
369.344588 0.000000 320.739078
...
distortion
0.189544 -0.018229 ...
rectification
1.000000 0.000000 0.000000
...
projection
262.927429 0.000000 ...

节（section）类型：

Section 作用
[image] width / height
[camera_name] 以节名作为相机名，含 K/D/R/P
[externals] translation/rotation（读取但不使用）

5.2 解析流程（parse_ini.cpp）

1
2
3
4
5
6
readCalibrationIni(stream)
  → split_lines()           # 按行分割
  → split_sections()        # 按 [section] 分组，跳过 # ; 注释
  → parse_image_section()   # width/height
  → parse_camera_section()  # K, D, R, P
  → parse_externals_section() # 可选，仅警告

矩阵解析：parse_matrix<rows, cols>() 从后续行按空格读浮点数。

5.3 畸变模型推断（INI 读）

1
2
3
4
5
6
7
8
auto d = parse_matrix<1, 8>(++distortion);
if (std::isnan(d[5])) {
  cam_info.d = vector(d[0..4]);
  cam_info.distortion_model = PLUMB_BOB;
} else {
  cam_info.d = vector(d[0..7]);
  cam_info.distortion_model = RATIONAL_POLYNOMIAL;
}

读 8 个系数，若第 6 个为 NaN 则截为 5 系数 plumb_bob。

5.4 INI 写限制

1
2
3
4
5
if (cam_info.distortion_model != PLUMB_BOB || cam_info.d.size() != 5)
{
  RCLCPP_ERROR(..., "Videre INI format can only save plumb bob ... Use YAML");
  return false;
}

INI 写出仅支持 plumb_bob + 5 系数；rational_polynomial 必须用 YAML。

6. YAML 格式（OpenCV / camera_calibration）

6.1 文件结构

测试用例格式：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
image_width: 640
image_height: 480
camera_name: mono_left
camera_matrix:
  rows: 3
  cols: 3
  data: [1, 2, 3, 4, 5, 6, 7, 8, 9]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [1, 2, 3, 4, 5]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]

与 OpenCV FileStorage / camera_calibration 工具链兼容。

6.2 实现（parse_yml.cpp）

  • 依赖 yaml-cpp（通过 yaml_cpp_vendor）
  • 写：YAML::Emitter + 自定义 SimpleMatrix 序列化器（rows/cols/data 结构）
  • 读：YAML::Load() → 逐字段填充 CameraInfo
  • 缺 distortion_model 时默认 plumb_bob 并 WARN

YAML 读写均支持任意 D 长度和显式 distortion_model。

7. 格式对比

特性 INI YAML
来源 Videre 工业相机遗留 OpenCV / camera_calibration
读 ✅ plumb_bob / rational_polynomial ✅ 全模型
写 仅 plumb_bob (D=5) ✅ 全模型
解析库 手写 line/section yaml-cpp
distortion_model 读时推断，写时不存 显式字段
externals 有节但不使用 无

8. CLI 工具 convert

1
2
ros2 run camera_calibration_parsers convert input.yml output.ini
ros2 run camera_calibration_parsers convert input.ini output.yml

流程：readCalibration → writeCalibration。
注意：INI 输出时若源为 rational_polynomial 会失败。

9. camera_info_manager 集成

1
2
3
4
5
6
if (readCalibration(filename, cam_name, cam_info)) {
  if (cname != cam_name) {
    RCLCPP_WARN(..., "camera name mismatch");
  }
  cam_info_ = cam_info;
}

典型 URL：

1
2
file:///home/user/.ros/camera_info/my_camera.yaml
package://my_robot/config/camera_left.ini

保存标定（set_camera_info 服务回调）调用 writeCalibration()。

10. 依赖关系

1
2
3
4
5
camera_calibration_parsers
  ├── sensor_msgs          # CameraInfo, distortion_models
  ├── yaml_cpp_vendor      # YAML 读写
  ├── rcpputils            # 文件系统、路径扩展名
  └── rclcpp               # 日志（PRIVATE 链接 parse.cpp，PUBLIC 未导出）

构建产物：

  • libcamera_calibration_parsers.so
  • convert 可执行文件（安装到 lib/camera_calibration_parsers/）

11. Python 支持状态

组件 状态
parse_wrapper.cpp 已注释禁用（仍用 ROS 1 boost::python + ros::serialization）
setup.py 遗留 catkin 风格，未接入 ament CMakeLists
__init__.py 引用不存在的 camera_calibration_parsers_wrapper

ROS 2 Humble 中 Python API 不可用；应直接用 C++ API 或通过 camera_info_manager Python 绑定（若有）。

12. 测试覆盖

测试 内容
test_parse_ini 有效/无效 INI、5/8 系数、round-trip 读写
test_parse_yml plumb_bob / rational_polynomial YAML、round-trip
calib5.ini / calib8.ini 真实标定样例

make_calibs.hpp 提供标准 640×480 测试矩阵和 check_calib() 断言。

13. 设计特点与局限

特点 说明
双格式兼容 兼顾老 INI 与新 YAML 生态
流式 + 文件 API istream/ostream 与路径 overload
自动格式检测 扩展名驱动，无需手动指定
畸变模型感知 INI 读时推断，YAML 显式存储
局限 说明
INI 写能力弱 仅 plumb_bob
parseCalibration 无 YAML 内存 buffer 只支持 ini
externals 节无效 读但不写入 CameraInfo
Python 绑定废弃 ROS 2 未重建
日志依赖 rclcpp 纯库场景也需链接 rclcpp
遗留 .h 头文件 与 .hpp 并存，TODO 统一

14. 典型使用

C++

1
2
3
4
5
6
7
#include "camera_calibration_parsers/parse.hpp"

std::string camera_name;
sensor_msgs::msg::CameraInfo cam_info;
if (camera_calibration_parsers::readCalibration("left.yaml", camera_name, cam_info)) {
  // 使用 cam_info.k, cam_info.d, ...
}

与 camera_calibration 工具链

1
2
# camera_calibration 保存的 YAML 可直接被 readCalibration 读取
ros2 run camera_info_manager caminfo_manager ...

15. 推荐阅读顺序

  1. parse.hpp — 三个公共函数
  2. parse.cpp — 扩展名分发逻辑
  3. parse_yml.cpp — 现代 YAML 路径（优先）
  4. parse_ini.cpp — Videre INI 解析细节
  5. test/calib5.ini + test_parse_yml.cpp — 格式样例
  6. camera_info_manager.cpp — loadCalibrationFile / saveCalibrationFile
  7. convert.cpp — 格式互转 CLI

16. 小结

camera_calibration_parsers 是 ROS 相机标定的 文件 I/O 适配层：INI 兼容 Videre 老格式，YAML 对接 OpenCV/camera_calibration 生态，统一输出 sensor_msgs/CameraInfo。camera_info_manager 是其主要调用者；convert 提供格式转换。ROS 2 中 Python 绑定已移除，核心能力在 C++ 库中。

如果你希望，我可以把本文写入 ros2doc/ros-perception/camera_calibration_parsers 源码详细分析.md，或继续分析 camera_info_manager 的 URL 解析与 set_camera_info 服务流程。

文章互动

阅读 --

留言

0 条留言

正在加载留言…