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

一、背景介绍

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

二、设计思路

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

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}$) ]; then
    echo "容器 '$container_name' 已经存在。"
else
    echo "容器 '$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/null

apt 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.0
      io_method: "mmap"
      frame_id: "camera"
      pixel_format: "mjpeg2rgb"
      av_device_format: "YUV422P"
      image_width: 1920
      image_height: 1080
      camera_name: "test_camera"
      brightness: 70
      white_balance_temperature_auto: false
      exposure_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
1920

height
1080

[narrow_stereo]

camera matrix
1368.692265 0.000000 975.426678
0.000000 1375.519467 423.645254
0.000000 0.000000 1.000000

distortion
0.005129 -0.083813 -0.015080 0.001276 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
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.0
        scale_y = h / 1080.0
        scaled_camera_matrix = camera_matrix.copy()
        scaled_camera_matrix[0, 0] *= scale_x
        scaled_camera_matrix[1, 1] *= scale_y
        scaled_camera_matrix[0, 2] *= scale_x
        scaled_camera_matrix[1, 2] *= scale_y
        use_matrix = scaled_camera_matrix
        print(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 * 4
    axis_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_path
    cv2.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_commander

class 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 positions
    
    def 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相机标定环境的完整流程。通过容器化方案，我们实现了：

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

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