camera_info_manager 源码详细分析

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

camera_info_manager 是 ROS 相机驱动的 标定数据管理库：负责从 URL 加载/保存 sensor_msgs/CameraInfo，提供 set_camera_info 服务供 camera_calibration 写入标定结果，并缓存当前内参。文件格式读写委托给 camera_calibration_parsers。

---

1. 仓库结构

camera_info_manager/
├── include/camera_info_manager/
│   ├── camera_info_manager.hpp   # 公共 API（主头文件）
│   ├── camera_info_manager.h     # 遗留 C 风格头
│   └── visibility_control.h
├── src/
│   ├── camera_info_manager.cpp   # 全部实现（~620 行）
│   └── split.hpp                 # 正则 split 工具（未使用）
├── tests/
│   ├── unit_test.cpp             # ROS 1 风格测试（CMake 中已禁用）
│   ├── test_calibration.yaml     # 测试标定样例
│   └── unit_test.test
├── CMakeLists.txt
└── package.xml

Python 对应包：camera_info_manager_py（独立 ament 包，逻辑与 C++ 版平行）。

---

2. 在 image_common 栈中的位置

组件	职责
camera_info_manager	URL 管理、懒加载、服务、线程安全缓存
camera_calibration_parsers	YAML/INI 文件序列化
camera_calibration	标定并调用 set_camera_info
image_pipeline	使用 CameraInfo 做 rectification

---

3. 类设计：CameraInfoManager

3.1 构造与成员

CameraInfoManager::CameraInfoManager(
  rclcpp::Node * node,
  const std::string & cname,
  const std::string & url)
: logger_(node->get_logger()),
  camera_name_(cname),
  url_(url),
  loaded_cam_info_(false)
{
  info_service_ = node->create_service<SetCameraInfo>(
    "set_camera_info",
    std::bind(&CameraInfoManager::setCameraInfoService, this, ...));
}

成员	作用
camera_name_	相机唯一标识（写入标定文件并校验）
url_	标定文件 Uniform Resource Locator
cam_info_	当前 CameraInfo 缓存
loaded_cam_info_	是否已尝试加载（懒加载标志）
info_service_	set_camera_info 服务
mutex_	保护上述可变状态

3.2 公共 API

方法	作用
getCameraInfo()	返回缓存；首次调用触发加载
isCalibrated()	cam_info_.k[0] != 0 判断是否已标定
loadCameraInfo(url)	设置新 URL 并立即加载
setCameraName(cname)	改相机名，强制下次重新加载
setCameraInfo(info)	手动设置内参（不保存文件）
validateURL(url)	检查 URL 语法是否支持
resolveURL(url, cname)	展开 ${NAME} / ${ROS_HOME}

---

4. 懒加载机制

自 Fuerte 起，构造函数不加载文件，避免无效 URL 产生误导性错误日志。

CameraInfo CameraInfoManager::getCameraInfo(void)
{
  while (rclcpp::ok()) {
    std::lock_guard<std::mutex> lock(mutex_);
    if (loaded_cam_info_) {
      return cam_info_;
    }
    loaded_cam_info_ = true;
    url = url_;
    cname = camera_name_;
  }  // 释放锁
  loadCalibration(url, cname);  // I/O 在锁外
  }
  return CameraInfo();
}

要点：

• 首次 getCameraInfo() / isCalibrated() 才触发 loadCalibration()
• 即使加载失败，loaded_cam_info_ 也为 true，避免重复尝试
• I/O 从不持锁，防止死锁与长时间阻塞

未标定时返回 全零 CameraInfo，image_pipeline 将其视为 uncalibrated。

---

5. URL 系统

5.1 支持的 URL 类型

if (iequals(url.substr(0, 8), "file:///"))   → URL_file
if (iequals(url.substr(0, 9), "flash:///"))  → URL_flash (未实现)
if (iequals(url.substr(0, 10), "package://")) → URL_package
else → URL_invalid

类型	示例	行为
空	""	使用默认 URL
file://	file:///home/user/cam.yaml	本地绝对路径（去掉 file:// 前缀 7 字符）
package://	package://my_pkg/config/cam.yaml	经 ament_index 解析 share 路径
flash://	—	警告，未实现

5.2 默认 URL

const std::string default_camera_info_url =
  "file://${ROS_HOME}/camera_info/${NAME}.yaml";

空 URL 或无效 URL 保存时，回退到此默认路径（通常为 ~/.ros/camera_info/<camera_name>.yaml）。

5.3 变量替换（resolveURL）

变量	替换为
${NAME}	当前 camera_name_
${ROS_HOME}	环境变量 ROS_HOME，否则 $HOME/.ros

单遍扫描，不支持递归替换。非法 ${...} 保留 $ 并 ERROR 日志。

示例：

package://my_robot/calibrations/${NAME}.yaml
  → package://my_robot/calibrations/left_camera.yaml

file://${ROS_HOME}/camera_info/${NAME}.yaml
  → file:///home/user/.ros/camera_info/left_camera.yaml

5.4 package:// 解析

std::string package = url.substr(prefix_len, rest - prefix_len);
std::string pkgPath = ament_index_cpp::get_package_share_directory(package);
return pkgPath + url.substr(rest);

---

6. 加载与保存流程

6.1 加载

loadCalibration(url, cname)
  → resolveURL(url, cname)
  → parseURL() 分派
  → loadCalibrationFile(filename, cname)
      → readCalibration()  [camera_calibration_parsers]
      → 校验 camera_name（不匹配仅 WARN）
      → cam_info_ = cam_info

6.2 保存（set_camera_info 服务）

void setCameraInfoService(...)
{
  cam_info_ = req->camera_info;   // 总是更新内存
  rsp->success = saveCalibration(req->camera_info, url_copy, cname);
}

saveCalibration(info, url, cname)
  → resolveURL + parseURL
  → saveCalibrationFile(filename, cname)
      → 创建父目录（rcpputils::fs::create_directories）
      → writeCalibration()  [camera_calibration_parsers]

即使保存失败，内存中的 cam_info_ 也已更新。

---

7. set_camera_info 服务

属性	值
服务名	set_camera_info（相对节点 namespace）
类型	sensor_msgs/srv/SetCameraInfo
调用方	camera_calibration 标定工具

典型驱动集成：

class MyCameraNode : public rclcpp::Node {
  camera_info_manager::CameraInfoManager cinfo_;

  MyCameraNode() : Node("camera"), cinfo_(this, "my_camera", camera_info_url) {}

  void publishFrame() {
    auto msg = std::make_unique<sensor_msgs::msg::CameraInfo>(
      cinfo_.getCameraInfo());
    msg->header.stamp = ...;
    msg->header.frame_id = "camera_optical_frame";
    pub_ci_->publish(std::move(msg));
  }
};

驱动需提供 camera_info_url 参数（在驱动层处理，非本类内置）。

---

8. 相机名校验

bool setCameraName(const std::string & cname)
{
  if (cname.empty()) return false;
  for (char c : cname)
    if (!isalnum(c) && c != '_') return false;
  camera_name_ = cname;
  loaded_cam_info_ = false;  // 强制重新加载
  return true;
}

合法字符：[a-zA-Z0-9_]。建议用设备序列号、型号等唯一标识。

---

9. 标定判定

return cam_info_.k[0] != 0.0;

以 K 矩阵第一个元素非零 作为“已标定”启发式判断，而非检查 distortion 或 P 矩阵。

---

10. 依赖关系

camera_info_manager
  ├── rclcpp              # Node、Service、Logger
  ├── sensor_msgs       # CameraInfo、SetCameraInfo
  ├── camera_calibration_parsers  # 文件读写
  ├── ament_index_cpp   # package:// URL
  └── rcpputils         # 文件系统、环境变量

---

11. 与 Python 版对比

特性	C++ camera_info_manager	Python camera_info_manager_py
包名	本包	独立包
YAML 解析	camera_calibration_parsers	内置 yaml 模块
URL/服务逻辑	基本相同	基本相同
扩展	—	ZoomCameraInfoManager

Python 驱动应使用 camera_info_manager_py，而非 C++ 包。

---

12. 测试状态

文件	状态
tests/unit_test.cpp	CMake 中注释禁用（仍用 ROS 1 ros::NodeHandle API）
tests/test_calibration.yaml	有效样例数据（640×480 plumb_bob）
lint	cpplint / uncrustify 启用

ROS 2 端口后单元测试尚未完全迁移。

---

13. 遗留与未使用代码

项	说明
split.hpp	正则 split 工具，未被 camera_info_manager.cpp 引用
camera_info_manager.h	遗留头文件，新代码应用 .hpp
parse_wrapper 等	不在本包

---

14. 典型使用场景

场景 1：launch 指定标定文件

# 驱动参数
camera_info_url: "package://my_robot/config/left_camera.yaml"
camera_name: "left_camera"

场景 2：标定后自动保存

ros2 run camera_calibration cameracalibrator ...
# 标定完成后调用 /<namespace>/set_camera_info
# 默认保存到 ~/.ros/camera_info/<camera_name>.yaml

场景 3：多相机

camera_info_manager::CameraInfoManager left_ci(this, "left", left_url);
camera_info_manager::CameraInfoManager right_ci(this, "right", right_url);
// 或在子 namespace 下：left/camera, right/camera

---

15. 设计特点与局限

特点	说明
懒加载	构造时不读文件，减少启动噪声
URL 抽象	file/package + 变量替换，灵活配置
线程安全	短持锁 + I/O 锁外
服务集成	与 camera_calibration 工作流无缝对接
默认路径约定	~/.ros/camera_info/ 社区惯例

局限	说明
flash:// 未实现	仅 WARN
package:// 写权限	保存到 package share 需可写（通常不推荐）
k[0] 标定判断	粗糙启发式
单元测试未启用	ROS 2 测试待迁移
Header 未填	getCameraInfo() 不设置 header，驱动必须填
resolveURL 公开	API 略暴露内部细节

---

16. 推荐阅读顺序

1. camera_info_manager.hpp 文档注释 — URL/相机名/服务约定
2. 构造函数 + getCameraInfo() — 懒加载模式
3. loadCalibration / saveCalibration — URL 分派
4. resolveURL / parseURL — 变量替换与语法
5. setCameraInfoService — 标定写入路径
6. camera_calibration_parsers 分析 — 文件格式细节
7. camera_info_manager_py — Python 驱动参考

---

17. 小结

camera_info_manager 是 ROS 相机标定的 运行时管理器：通过 URL 定位标定文件，懒加载 CameraInfo，暴露 set_camera_info 供标定工具写入，并委托 camera_calibration_parsers 做持久化。它是连接 camera_calibration ↔ 相机驱动 ↔ image_pipeline 的关键中间层。

如果你希望，我可以把本文写入 ros2doc/ros-perception/image_common/camera_info_manager 源码详细分析.md，或继续分析 camera_calibration 如何调用 set_camera_info 的完整流程。