基于ROS2进行相机标定,并通过测试相机到棋盘格之间的距离进行验证

一、背景介绍

在视觉系统中，相机标定是获取准确空间信息的关键步骤。本文介绍基于Docker创建可移植、可复现的ROS2相机标定环境，并通过测试相机到棋盘格之间的距离进行验证。也可用于某些场景下的单目测距离。

---

二、设计思路

1. 环境搭建理念是容器化封装,主要解决以下问题：

挑战	解决方案
复杂环境配置	Docker容器封装ROS2完整环境
硬件资源限制	最小化Ubuntu基础镜像(22.04)
相机设备访问	特权模式+设备直通
GUI显示需求	X11转发机制

2. 单目测距离原理

相机坐标系▲ Z轴（相机光轴）│●───> X轴/ ▼ Y轴┌──────────────┐│   棋盘格图像  │ → 检测角点 → PnP求解 → 距离投影└──────────────┘
世界坐标系（棋盘平面）

通过计算机视觉技术 计算相机到棋盘格平面的垂直距离,包括以下几个步骤

2.1 相机标定参数

camera_matrix = [...]  # 相机内参（焦距和光学中心）
dist_coeffs = [...]    # 镜头畸变系数

• 这些是预先通过相机标定获得的参数，用于消除镜头畸变并建立3D空间到2D图像的映射关系。

2.2 棋盘格参数

board_size = (8, 6)  # 棋盘格内部角点数（行，列）
square_size = 0.024   # 每个格子的实际边长（米）

• 提供棋盘格的物理尺寸信息，用于建立真实世界的坐标系。

2.3 核心计算流程

步骤1：图像预处理

• 读取图像并自适应缩放相机参数（若图像分辨率变化）
• 转换为灰度图以便角点检测

步骤2：检测棋盘格角点

found, corners = cv2.findChessboardCorners(gray, board_size)

• 在图像中定位棋盘格的内部角点（如图中红点所示）
• 使用亚像素优化提升精度至亚像素级

步骤3：构建3D坐标

obj_points = np.mgrid[...] * square_size

• 生成棋盘格角点的真实3D坐标（假设棋盘在Z=0平面上）

步骤4：求解相机位姿（PnP算法）

success, rvec, tvec = cv2.solvePnP(obj_points, corners, use_matrix, dist_coeffs)

• 关键原理：通过3D点（棋盘角真实位置）和2D点（图像中的角点像素位置），反推相机的空间位置和朝向
• 输出结果：
  • rvec：相机旋转向量
  • tvec：相机相对于棋盘中心的3D平移向量

步骤5：计算垂直距离

rotation_mat = cv2.Rodrigues(rvec)  # 旋转向量→旋转矩阵
normal_vector = rotation_mat[:, 2]   # 提取棋盘格平面法向量
distance = abs(np.dot(tvec.flatten(), normal_vector))

• 核心数学原理：
  • 距离 = 相机位置向量（tvec）在棋盘法向量上的投影长度
  • 欧氏距离（直线距离）作为辅助参考：np.linalg.norm(tvec)

---

三、操作步骤

1. 创建ROS2开发容器(在桌面系统里创建)

mkdir ros2_humble_camera_calibration
cd ros2_humble_camera_calibration
cat> run_ros2.sh <<-'EOF'
#!/bin/bash
image_name="ubuntu:22.04"
echo $image_name
container_name="ros2_humble_camera_calibration"
# 允许容器访问X11显示
xhost +
if [ $(docker ps -a -q -f name=^/${container_name}$) ]; thenecho "容器 '$container_name' 已经存在。"
elseecho "容器 '$container_name' 不存在。正在创建..."docker run -id --privileged --net=host \-v /etc/localtime:/etc/localtime:ro \-v $PWD:/home -e DISPLAY=$DISPLAY -w /home \-v /tmp/.X11-unix:/tmp/.X11-unix \-e GDK_SCALE -e GDK_DPI_SCALE \--name $container_name --hostname=$container_name $image_name /bin/bash
fi
docker start $container_name
docker exec -ti $container_name bash
EOF
chmod +x run_ros2.sh
bash run_ros2.sh

2. 在容器里安装ROS2 Humble

# 基础工具
apt update
apt install x11-apps -y
xclock  # 测试GUI显示apt install lsb-core -y
apt install locales -y# 设置语言环境
locale-gen en_US en_US.UTF-8
update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8# 添加ROS2仓库
apt install curl gnupg2 lsb-release vim -y
curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/nullapt update# 安装ROS2核心
apt install ros-humble-desktop -y
apt install python3-argcomplete -y
apt install ros-dev-tools -y
apt install v4l-utils -y# 安装开发工具
apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential -y
apt install ros-humble-vision-opencv && apt install ros-humble-message-filters -y
apt install ros-humble-tf2-geometry-msgs -y# 安装Python依赖
apt install python3-pip -y
apt install ros-humble-sensor-msgs-py ros-humble-cv-bridge -y
apt install libepoxy-dev -y# 初始化ROS依赖系统
rosdep init
rosdep update# 初始化ROS依赖
source /opt/ros/humble/setup.bash
echo "127.0.0.1       ros2_humble_camera_calibration" >> /etc/hosts# 安装ffmpeg,获取相机抓图
apt install ffmpeg -y# 安装标定工具
apt install ros-humble-camera-calibration -y
apt install ros-humble-usb-cam -y

---

3. 棋盘格标定板制作

使用在线生成器创建棋盘格：calib.io

制作要点：

• 选择9x7格（角点数量=8x6）
• 方格尺寸建议20-30mm（A4纸打印）
• 使用哑光纸避免反光(其表面有一定的粗燥感,当光线照射在哑光纸表面时，就会产生漫反射，光泽柔和、不刺眼)
• 保持标定板平整
• 用A4纸按实际尺寸打印

---

4. 相机配置与数据采集

4.1 相机参数配置

# 创建配置文件：
cat> params_1.yaml <<-'EOF'
/**:ros__parameters:video_device: "/dev/video23"framerate: 10.0io_method: "mmap"frame_id: "camera"pixel_format: "mjpeg2rgb"av_device_format: "YUV422P"image_width: 1920image_height: 1080camera_name: "test_camera"brightness: 70white_balance_temperature_auto: falseexposure_auto: 0
EOF

4.2 启动相机节点

ros2 run usb_cam usb_cam_node_exe --ros-args --params-file ./params_1.yaml

5. 执行相机标定

ros2 run camera_calibration cameracalibrator \--size 8x5 --square 0.024 --no-service-check \--ros-args --remap image:=/image_raw \--ros-args --remap camera:=/camera

标定数据采集技巧：

移动方向	目的	视觉反馈
水平移动	覆盖X轴视野	X进度条变绿
垂直移动	覆盖Y轴视野	Y进度条变绿
前后移动	覆盖不同深度	Size进度条变绿
倾斜旋转	覆盖不同角度	Skew进度条变绿

操作要点：

1. 距离范围：0.5m - 2m
2. 棋盘占比：画面1/3 - 2/3
3. 位置分布：覆盖图像中心和边缘
4. 有效样本：50-100张

6. 保存命令行终端打印的标定参数

# oST version 5.0 parameters
[image]width
1920height
1080[narrow_stereo]camera matrix
1368.692265 0.000000 975.426678
0.000000 1375.519467 423.645254
0.000000 0.000000 1.000000distortion
0.005129 -0.083813 -0.015080 0.001276 0.000000rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000projection
1345.237061 0.000000 979.457725 0.000000
0.000000 1362.495850 411.647966 0.000000
0.000000 0.000000 1.000000 0.000000

7. 计算相机到棋盘格距离的脚本

cat> calculate_distance.py <<-'EOF'
import cv2
import numpy as np# 相机标定参数
camera_matrix = np.array([[1368.692265, 0.000000, 975.426678],[0.000000, 1375.519467, 423.645254],[0.000000, 0.000000, 1.000000]
])dist_coeffs = np.array([0.005129, -0.083813, -0.015080, 0.001276, 0.000000])# 棋盘格参数
board_size = (8, 6)  # 内角点数量
square_size = 0.024   # 修改为实际测量的每个格子边长（单位：米）def calculate_distance(image_path):# 读取图像img = cv2.imread(image_path)if img is None:print(f"错误：无法读取图像 {image_path}")return None# 获取图像分辨率h, w = img.shape[:2]print(f"图像分辨率: {w}x{h}")# 分辨率适配if w != 1920 or h != 1080:scale_x = w / 1920.0scale_y = h / 1080.0scaled_camera_matrix = camera_matrix.copy()scaled_camera_matrix[0, 0] *= scale_xscaled_camera_matrix[1, 1] *= scale_yscaled_camera_matrix[0, 2] *= scale_xscaled_camera_matrix[1, 2] *= scale_yuse_matrix = scaled_camera_matrixprint(f"已缩放相机矩阵以适应分辨率")else:use_matrix = camera_matrix# 转换为灰度图gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)# 查找棋盘格角点（增强检测）found, corners = cv2.findChessboardCorners(gray, board_size,flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK +cv2.CALIB_CB_NORMALIZE_IMAGE)# 备用检测方法if not found:found, corners = cv2.findChessboardCorners(gray, board_size,flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FILTER_QUADS)if not found:print("错误：未检测到棋盘格角点")cv2.imwrite("debug_chessboard.jpg", gray)return None# 亚像素级角点精确化criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.0001)cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)# 生成世界坐标系中的3D点obj_points = np.zeros((board_size[0] * board_size[1], 3), np.float32)obj_points[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2) * square_size# 使用PnP算法求解位姿success, rvec, tvec = cv2.solvePnP(obj_points, corners, use_matrix, dist_coeffs)if not success:print("错误：无法求解相机位姿")return None# 计算到平面的垂直距离rotation_mat, _ = cv2.Rodrigues(rvec)normal_vector = rotation_mat[:, 2]  # 平面法向量distance = abs(np.dot(tvec.flatten(), normal_vector))# 调试信息euclidean_dist = np.linalg.norm(tvec)print(f"欧氏距离: {euclidean_dist:.3f}米")print(f"垂直距离: {distance:.3f}米")print(f"平移向量: X={tvec[0][0]:.3f}, Y={tvec[1][0]:.3f}, Z={tvec[2][0]:.3f}")# 可视化结果cv2.drawChessboardCorners(img, board_size, corners, found)# 绘制坐标系axis_length = square_size * 4axis_points, _ = cv2.projectPoints(np.float32([[0,0,0], [axis_length,0,0], [0,axis_length,0], [0,0,-axis_length]]),rvec, tvec, use_matrix, dist_coeffs)origin = tuple(axis_points[0].ravel().astype(int))cv2.line(img, origin, tuple(axis_points[1].ravel().astype(int)), (0,0,255), 3)  # X轴(红)cv2.line(img, origin, tuple(axis_points[2].ravel().astype(int)), (0,255,0), 3)  # Y轴(绿)cv2.line(img, origin, tuple(axis_points[3].ravel().astype(int)), (255,0,0), 3)  # Z轴(蓝)text = f"Distance: {distance:.2f} meters"cv2.putText(img, text, (30, 60), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3)# 保存并显示结果output_path = "result_" + image_pathcv2.imwrite(output_path, img)cv2.imshow("Result", img)cv2.waitKey(0)cv2.destroyAllWindows()return distance# 使用示例
if __name__ == "__main__":image_file = "chessboard.jpg"  # 替换为你的图片路径    distance = calculate_distance(image_file)
EOF

8. 用原始分辨率测距

# 捕获标定板图像
ffmpeg -f v4l2 -y -video_size 1920x1080 -i /dev/video23 -vframes 1 chessboard.jpg# 计算距离
python3.10 calculate_distance.py

输出

图像分辨率: 1920x1080
欧氏距离: 1.104米
垂直距离: 1.046米
平移向量: X=0.034, Y=0.029, Z=1.103

9. 降低分辨率测距

ffmpeg -f v4l2 -y -video_size 1280x720 -i /dev/video23 -vframes 1 chessboard.jpg
python3.10 calculate_distance.py

输出

图像分辨率: 1280x720
已缩放相机矩阵以适应分辨率
欧氏距离: 1.104米
垂直距离: 1.046米
平移向量: X=0.034, Y=0.029, Z=1.103

---

四、注意事项

1. 标定板使用要点

   • 保持标定板平整无褶皱
   • 避免强光反射（使用哑光纸）
   • 不同距离采集时保持棋盘格清晰

2. 分辨率处理陷阱

   # 错误做法：直接使用原始相机矩阵
   # 正确做法：按比例缩放矩阵参数
   scaled_camera_matrix[0, 0] = camera_matrix[0, 0] * scale_x
   

---

五、扩展应用

1. 双目相机标定

修改启动参数支持双目标定：

ros2 run camera_calibration cameracalibrator \--size 8x5 --square 0.024 \--right /right/image_raw --left /left/image_raw \--camera_name left_camera right_camera

2. 自动标定系统

结合机械臂实现自动化标定：

import moveit_commanderclass AutoCalibrator:def __init__(self):self.arm = moveit_commander.MoveGroupCommander("manipulator")self.calib_positions = self.generate_positions()def generate_positions(self):# 生成机械臂标定路径positions = []for z in np.linspace(0.5, 1.5, 5):for x in np.linspace(-0.3, 0.3, 5):positions.append([x, 0, z])return positionsdef run_calibration(self):for pos in self.calib_positions:self.arm.set_position_target(pos)self.arm.go(wait=True)time.sleep(1)  # 等待稳定capture_image(f"pos_{x}_{z}.jpg")

3. 标定参数热更新

创建ROS2服务动态更新参数：

// calibration_update.srv
bool request
---
float64[9] camera_matrix
float64[5] dist_coeffs// 服务端实现
rclcpp::Service<calibration_update>::SharedPtr service =create_service<calibration_update>("update_calibration", [this](const std::shared_ptr<rmw_request_id_t> request_header,const std::shared_ptr<calibration_update::Request> request,const std::shared_ptr<calibration_update::Response> response) {// 更新相机参数this->camera_matrix = request->camera_matrix;this->dist_coeffs = request->dist_coeffs;response->success = true;});

4. 标定质量评估系统

def evaluate_calibration(images, camera_matrix, dist_coeffs):errors = []for img in images:# 检测角点found, corners = find_chessboard(img)if not found: continue# 计算重投影误差img_points, _ = cv2.projectPoints(obj_points, rvec, tvec, camera_matrix, dist_coeffs)error = cv2.norm(corners, img_points, cv2.NORM_L2) / len(img_points)errors.append(error)# 生成评估报告report = {"mean_error": np.mean(errors),"max_error": np.max(errors),"error_distribution": plt.hist(errors, bins=20)}return report

---

六、结语

本文详细介绍了使用Docker容器化技术搭建ROS2相机标定环境的完整流程。通过容器化方案，我们实现了：

• ✅ 环境隔离：避免污染主机系统
• ✅ 一键部署：简化复杂环境配置
• ✅ 跨平台兼容：可在不同设备无缝迁移
• ✅ 流程标准化：确保标定结果可复现

该方案不仅适用于单目相机标定，通过扩展还可支持双目相机、多相机阵列等复杂视觉系统。结合自动化标定脚本和在线参数更新机制，可大幅提升机器人视觉系统的开发效率。