人形机器人手眼标定（二）—— 眼在手上（保姆级教程）

人形机器人手眼标定（二）—— 眼在手上

• 一、什么是"眼在手上"的手眼标定
• 二、标定方法与步骤
• • 1. 在 URDF 中建立相机坐标系
  • 2. 启动机器人模型与 TF 树
  • 3. 启动相机并检查图像
  • 4. 准备并检测标定目标
  • 5. 验证相机到标定目标的 TF
  • 6. 采集多姿态样本
  • 7. 优化相机外参
• 三、实战演练
• • 1. 在 URDF 中建立相机坐标系
  • 2. 启动机器人模型与 TF 树
  • 3. 启动相机并检查图像
  • 4. 准备并检测标定目标
  • 5. 验证相机到标定目标的 TF
  • 6. 采集多姿态样本
  • 7. 优化相机外参
• 四、本文小结
• 五、标定用到的相关代码

一、什么是"眼在手上"的手眼标定

在机器人视觉系统中，“眼在手上”通常指相机安装在机器人运动部件上，相机会随着某个关节或末端一起运动。对于机械臂来说，常见形式是相机安装在末端执行器上；对于人形机器人来说，也可以理解为相机安装在头部，随着头部 yaw、pitch 电机一起转动。

本文中的系统是一个人形机器人头部相机标定场景。相机为 Intel RealSense D435i，相机安装在头部俯仰关节之后，因此相机姿态会随着 neck_down_rotate 和 neck_up_rotate 两个头部电机变化。标定目标是得到准确的相机安装外参，也就是 URDF 中 head_camera_joint 的 xyz 和 rpy 参数，使机器人在不同头部姿态下都能稳定地把视觉目标转换到 base_link 坐标系下。

简单来说，理想情况下，如果 AprilTag 固定在空间中不动，那么无论头部怎么转，base_link -> tag36h11:0 的位置都应该基本不变。如果这个值随着头部运动大幅变化，就说明机器人运动学模型、关节方向、关节零位或相机外参存在问题。

在这个过程中需要区分几个坐标系。camera_link 是 RealSense 相机机身坐标系，通常用于和机器人 URDF 相连；camera_color_optical_frame 是 RGB 相机的光学坐标系，AprilTag 检测得到的位姿一般是在这个坐标系下发布的；tag36h11:0 是被检测到的 AprilTag 坐标系。实际使用时，URDF 中主要标定的是 neck_up_rotate -> camera_link，而视觉检测使用的是 camera_color_optical_frame -> tag36h11:0，RealSense 驱动会自动发布 camera_link -> camera_color_optical_frame 的内部变换。

二、标定方法与步骤

“眼在手上”标定的核心目标，是确定相机相对于机器人运动部件的安装位姿。对于头部相机来说，就是标定相机相对于头部关节末端的固定变换；对于机械臂末端相机来说，就是标定相机相对于末端执行器的固定变换。

标定完成后，当机器人关节运动时，系统可以根据关节角度和相机观测结果，将视觉目标准确转换到机器人基坐标系下。

整个标定流程可以分为以下几个步骤。

1. 在 URDF 中建立相机坐标系

首先需要在机器人 URDF 中为相机添加一个固定坐标系，并通过 fixed joint 将相机连接到对应的运动部件上。

例如头部相机可以挂在头部俯仰关节之后，末端相机可以挂在机械臂末端 link 之后。这里填写的 xyz 和 rpy 不要求一开始非常准确，只需要根据实际安装位置进行粗略测量即可。

需要注意的是，URDF 中连接机器人本体的通常是相机机身坐标系，例如 camera_link，而不是人为估计的光心坐标系。对于 RealSense、USB 相机等设备，光学坐标系通常由相机驱动或视觉节点自动发布。

2. 启动机器人模型与 TF 树

完成 URDF 修改后，启动机器人模型发布节点，使 robot_state_publisher 正常发布 TF。

此时需要检查：

ros2 topic list

ros2 topic list

确认系统中存在：

/joint_states
/robot_description
/tf
/tf_static`

同时需要确认机器人关节状态能够正常发布。对于“眼在手上”标定来说，相机所在运动链条上的关节角度必须是正确的，否则后续计算出来的相机位姿会随关节运动产生系统误差。

3. 启动相机并检查图像

启动相机驱动后，检查 RGB 图像和相机内参是否正常发布。一般至少需要有：

/color/image_raw
/color/camera_info

或者与具体相机命名空间对应的图像和内参话题。

随后使用图像查看工具检查图像质量：

ros2 run rqt_image_view rqt_image_view

确认图像没有明显卡顿、花屏、过曝、欠曝或严重模糊。视觉标定对图像质量比较敏感，如果图像本身不稳定，后续 AprilTag 或标定板检测结果也会不稳定。

4. 准备并检测标定目标

常用的视觉标定目标包括 AprilTag、ArUco、棋盘格或圆点板。本文以 AprilTag 为例。

需要注意，AprilTag 和 ArUco 是两种不同的标记，不能混用。如果使用 apriltag_ros，就必须打印真正的 AprilTag，例如 tag36h11 系列。

配置 AprilTag 时，最重要的参数是标签尺寸：

family: 36h11
size: 0.1

其中 size 指 AprilTag 黑色边框的实际边长，单位是米。例如黑色边框为 100mm，就填写 0.1；如果是 55mm，就填写 0.055。尺寸填写错误会直接导致三维位置估计不准。

启动检测节点后，可以检查检测结果：

ros2 topic echo /detections

如果能够看到 tag 的 family 和 id，说明图像检测已经成功。

5. 验证相机到标定目标的 TF

视觉检测成功后，需要确认相机坐标系到标定目标坐标系的 TF 是否正常发布。例如：

ros2 run tf2_ros tf2_echo camera_color_optical_frame tag36h11:0

如果能够持续输出 Translation 和 Rotation，说明相机已经可以稳定观测标定目标。

这里要理解光学坐标系的含义。对于常见相机 optical frame： X 轴：图像右方 Y 轴：图像下方 Z 轴：镜头前方

因此，如果输出：

Translation: [0.04, 0.08, 0.50]

可以理解为标定目标位于相机前方约 0.5m，图像右侧约 4cm，下方约 8cm。

6. 采集多姿态样本

当 TF 树连通、图像检测正常、关节方向正确后，就可以开始采集标定样本。

每一组样本通常需要包含：当前关节角度、机器人基坐标系到相机安装末端的 TF和相机坐标系到标定目标的 TF。

采集时应注意： a、标定目标固定不动 b、每次运动后等待机器人稳定 c、尽量让标定目标位于图像中间区域 d、避免反光、遮挡和图像模糊 e、采集多个不同姿态

样本姿态应覆盖实际使用范围，但不建议一开始采集过大角度。对于头部相机，可以先采集中间、左右、上下的小范围姿态，再逐步扩大范围。

7. 优化相机外参

采集多组数据后，可以通过最小二乘方法优化相机外参。优化目标是让所有姿态下计算得到的 base_link -> tag 尽可能重合。

对于固定安装的相机，通常优化 6 个参数：

x y z roll pitch yaw

也就是相机安装 joint 的平移和旋转。

优化时建议采用“小步迭代”的方式。先用粗略外参优化一次，写入 URDF 后重新验证；如果效果变好，再以新参数作为初值继续微调。不要一次性允许参数大范围变化，否则优化结果可能会补偿其它错误，例如关节零位误差、机械回差或坏样本。

三、实战演练

本节以我自己的机器人头部 D435i 标定为例，完整记录一次“眼在手上”相机外参标定流程。本文使用的系统环境为 ROS2 Humble，相机为 Intel RealSense D435i，标定目标为 AprilTag。相机安装在机器人头部，会随着头部左右旋转和上下俯仰一起运动，因此本质上属于“眼在手上”的标定场景。

本次标定的目标不是标定 D435i 相机内参，而是标定相机相对于机器人头部的安装外参，也就是 URDF 中 head_camera_joint 的 xyz 和 rpy 参数。标定完成后，希望当头部在不同角度运动时，固定不动的 AprilTag 在 base_link 坐标系下的位置尽量保持不变。

1. 在 URDF 中建立相机坐标系

首先在机器人 URDF 中为 D435i 添加相机坐标系。我的相机安装在头部俯仰关节 neck_up_rotate 之后，因此将相机作为 neck_up_rotate 的子级 fixed joint 接入。

URDF 中添加如下结构：

<link name="head_camera_link"/>

<joint name="head_camera_joint" type="fixed">
<parent link="neck_up_rotate"/>
<child link="head_camera_link"/>

<!–– 这里需要根据实际安装位置调整 ––>
<origin xyz="-0.078669 0.118441 -0.022190"
rpy="-1.560010 -0.055818 1.572960"/>
</joint>
<link name="camera_link"/>

<joint name="camera_mount_joint" type="fixed">
<parent link="head_camera_link"/>
<child link="camera_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

这里真正需要标定的是 head_camera_joint 的 xyz 和 rpy。刚开始这些数值不需要完全准确，只要根据实际安装位置粗略填写即可，后面会通过采样和优化程序进一步修正。

需要注意的是，URDF 中连接机器人本体的通常是 D435i 的 camera_link，也就是相机机身坐标系，而不是人为估计的光心坐标系。D435i 启动后会自动发布自己的内部坐标系，例如： AprilTag 检测一般基于 camera_color_optical_frame，而不是 camera_link。因此，机器人 URDF 只需要正确接到 camera_link，相机内部的光学坐标系由 RealSense 驱动负责发布。

2. 启动机器人模型与 TF 树

URDF 修改完成后，启动机器人模型，使 robot_state_publisher 正常发布 TF。

我的机器人关节状态话题在命名空间 /right 下，因此实际使用的是：

ros2 topic echo /right/joint_states ––once

需要确认里面包含头部两个关节：

neck_down_rotate
neck_up_rotate

这两个关节分别对应头部左右旋转和上下俯仰。后续标定时，相机的位姿就是通过这两个关节的角度计算出来的。如果关节角度不正确，或者关节方向和 URDF 中定义的不一致，那么无论相机外参怎么优化，最终结果都会有系统误差。

启动完成后检查 ROS 话题：

ros2 topic list

输出如下图所示，重点看：

/tf
/tf_static
/robot_description
/right/joint_states

此时还可以检查头部关节在转动时数值是否变化。例如让头部左转、右转、抬头、低头，然后查看 /right/joint_states 中 neck_down_rotate 和 neck_up_rotate 的变化是否符合预期。

在我的实际标定过程中，这一步发现了一个非常关键的问题：URDF 中头部两个关节的 axis 方向写反了。原来写的是：

<axis xyz="0 0 1"/>

但实际电机正方向和 URDF 定义方向相反，导致头部向左转时，URDF 模型认为它在向右转。这样会造成固定不动的 AprilTag 在 base_link 下大幅漂移。

最后将 neck_down_rotate 和 neck_up_rotate 的 axis 都改为：

<axis xyz="0 0 -1"/>

修改后，固定 Tag 在不同头部姿态下的位置立刻稳定了很多。因此，在正式优化相机外参之前，必须先确认机器人运动学方向是正确的。

3. 启动相机并检查图像

接下来启动 D435i 相机。我的环境中先加载 ROS2 和 RealSense 工作空间：

source /opt/ros/humble/setup.bash
source ~/realsense_ws/install/setup.bash

然后启动 RealSense：

ros2 launch realsense2_camera rs_launch.py

启动后检查话题：

ros2 topic list

如下图所示，应能看到类似：

/camera/camera/color/image_raw
/camera/camera/color/camera_info
/camera/camera/depth/image_rect_raw

其中 AprilTag 检测主要使用 RGB 图像和 RGB 相机内参：

/camera/camera/color/image_raw
/camera/camera/color/camera_info

然后打开图像查看工具：

ros2 run rqt_image_view rqt_image_view

选择：

/camera/camera/color/image_raw

图像页面如下图所示：

确认图像正常显示，没有卡顿、花屏、严重过曝、欠曝或模糊。图像质量会直接影响 AprilTag 位姿估计，如果图像本身不稳定，后续采集的数据也会不稳定。

4. 准备并检测标定目标

本文使用 AprilTag 作为标定目标。需要注意，AprilTag 和 ArUco 是两种不同的标记，不能混用。最开始我打印的是 ArUco，导致 apriltag_ros 一直检测不到。后来重新打印真正的 AprilTag 后，检测才正常，我打印 AprilTag 的网址为：https://chaitanyantr.github.io/apriltag.html?utm_source=chatgpt.com。

我使用的是 tag36h11，id 为 0。创建配置文件：

mkdir –p ~/apriltag_test/config
nano ~/apriltag_test/config/tags.yaml

内容如下：

/apriltag:
ros__parameters:
family: 36h11
size: 0.1
max_hamming: 1

启动 AprilTag 节点：

ros2 run apriltag_ros apriltag_node ––ros–args \\
–r image_rect:=/camera/camera/color/image_raw \\
–r camera_info:=/camera/camera/color/camera_info \\
––params–file ~/apriltag_test/config/tags.yaml

检查检测结果：

ros2 topic echo /detections

如果能看到类似：

family: tag36h11
id: 0

说明 AprilTag 已经被成功识别，下图是我的实际输出：

5. 验证相机到标定目标的 TF

AprilTag 检测成功后，先验证相机是否能稳定测量 Tag 的三维位置：

ros2 run tf2_ros tf2_echo camera_color_optical_frame tag36h11:0

如果能够持续输出 Translation 和 Rotation，说明相机到 Tag 的 TF 正常。如图： 对于相机光学坐标系 camera_color_optical_frame，通常可以这样理解：

X 轴：图像右方 Y 轴：图像下方 Z 轴：镜头前方

接着需要验证整条 TF 链是否连通：

ros2 run tf2_ros tf2_echo base_link tag36h11:0

如果能够持续输出 Translation 和 Rotation，说明base_link到 Tag 的 TF 正常。如图： 这一步非常重要。因为我们的目标不是只知道 Tag 在相机坐标系下的位置，而是要把 Tag 转换到机器人 base_link 坐标系下。如果这条 TF 不能输出，说明机器人本体、相机和 Tag 还没有连成完整 TF 树。

在标定时，判断外参是否准确，应该主要看：

base_link –> tag36h11:0

而不是：

camera_link –> tag36h11:0

因为相机安装在头部，头部运动时相机本身会动，所以 camera_link -> tag 一定会变化。真正固定不动的是 Tag 在 base_link 下的位置。

6. 采集多姿态样本

当 TF 树连通、AprilTag 检测正常、头部关节方向正确后，就可以开始采集多姿态样本。

采样时，AprilTag 必须固定不动，可以贴在墙面或固定支架上。然后让头部依次运动到多个姿态，例如：

中间 左转 右转 抬头 低头 左转 + 抬头 右转 + 低头

每次移动头部后，需要等待画面稳定，再采集样本。我的采样脚本head_camera_6dof_calibration_v2.py（本文结尾附上）会记录：

/right/joint_states 中的 neck_down_rotate 和 neck_up_rotate
base_link –> neck_up_rotate
camera_link –> tag36h11:0

采集命令如下：

python3 head_camera_6dof_calibration_v3.py collect \\
––out ~/head_camera_samples.jsonl \\
––num–samples 10

采集过程中(如下图所示)，脚本会在每次按下 Enter 后等待新的 AprilTag TF 时间戳，避免采到旧 TF。这个改进很重要，因为如果采到 stale TF，不同姿态可能保存了相同的 Tag 位姿，会严重影响优化结果。

采集完成后，可以检查样本（如下图所示）：

python3 head_camera_6dof_calibration_v3.py inspect \\
––samples ~/head_camera_samples.jsonl \\
––limit 10

采样时需要注意以下几点：

如果想做初步标定，可以先采集中间、左 10°、右 10°、抬头 5°、低头 5° 等小范围姿态。等结果稳定后，再逐步扩大到左 15°、右 15°、抬头 10°、低头 10°。

7. 优化相机外参

采集完成后，就可以优化 head_camera_joint 的 6 个参数：

x y z roll pitch yaw

优化的核心思想是：AprilTag 固定不动，因此所有姿态下计算出来的 base_link -> tag36h11:0 应该尽量重合。优化程序会调整 head_camera_joint 的 xyz/rpy，让这些点的离散程度最小。

第一次优化可以使用当前 URDF 中的参数作为初始值：

python3 head_camera_6dof_calibration_v2.py optimize \\
––samples ~/head_camera_samples.jsonl \\
––initial "-0.081420 0.123323 -0.024044 -1.566609 -0.070817 1.587960" \\
––max–xyz–delta 0.01 \\
––max–rpy–delta 0.03 \\
––print–sample–errors

其中：

––max–xyz–delta 0.01

表示单轮优化中 xyz 最多变化 10mm；

––max–rpy–delta 0.03

表示单轮优化中 rpy 最多变化约 1.7°。

脚本会先计算每个样本对应的 base_link -> tag 位置，并自动剔除明显坏点。例如某一次采样中，脚本自动剔除了：

sample 5, 6, 8, 9, 11, 12

剔除后再参与优化，避免坏点把结果带偏。

优化输出类似：

优化前 RMS：5.79 mm 优化后 RMS：4.31 mm 优化前 MAX：10.49 mm 优化后 MAX：6.74 mm

最终得到新的 URDF 参数：

<joint name="head_camera_joint" type="fixed">
<parent link="neck_up_rotate"/>
<child link="head_camera_link"/>
<origin xyz="-0.081420 0.123323 -0.024044"
rpy="-1.566609 -0.070817 1.587960"/>
</joint>

将这组参数写回 URDF，重新启动 robot_state_publisher 后，再固定 AprilTag 不动，验证多个未参与优化的姿态：

头正中间 向左 15° 向右 15° 抬头 10° 低头 10°

每个姿态查看：

ros2 run tf2_ros tf2_echo base_link tag36h11:0

某次验证结果如下：

头正中间： Translation: [0.938, -0.045, 0.955]

向左15度： Translation: [0.925, -0.044, 0.958]

向右15度： Translation: [0.931, -0.044, 0.956]

抬头10度： Translation: [0.938, -0.042, 0.956]

低头10度： Translation: [0.933, -0.044, 0.952]

可以看到，Y 方向非常稳定，Z 方向也比较稳定，主要剩余误差集中在左转时的 X 方向。后续分析发现，机器人头部电机本身存在约 0.15° 的角度误差。对于 1m 左右的观测距离，0.15° 的角度误差理论上就可能带来约 2.6mm 的位置误差。如果再叠加机械回差、AprilTag 检测误差、支架刚性和时间同步误差，最终出现几毫米到一厘米的误差是正常的。

因此，标定后期不应该继续盲目大幅修改 URDF，而应该采用小步迭代方式继续微调。例如：

python3 head_camera_6dof_calibration_v2.py optimize \\
––samples ~/head_camera_samples.jsonl \\
––initial "-0.081420 0.123323 -0.024044 -1.566609 -0.070817 1.587960" \\
––max–xyz–delta 0.005 \\
––max–rpy–delta 0.015 \\
––print–sample–errors

如果优化后结果变好，就保留新参数；如果验证结果变差，就回退上一版参数。

通过以上步骤，最终可以把头部相机在不同头部姿态下的目标定位误差压到毫米到厘米级范围。

四、本文小结

本文记录了一次基于 ROS2 Humble、Intel RealSense D435i 和 AprilTag 的人形机器人头部相机标定过程。虽然本文讨论的是头部相机，但从标定形式上看，它本质上属于“眼在手上”的手眼标定：相机安装在机器人运动部件上，会随着关节运动一起改变位姿。

本次标定的核心目标不是标定 D435i 的相机内参，而是标定相机相对于机器人头部的安装外参，也就是 URDF 中 head_camera_joint 的 xyz 和 rpy。判断标定是否有效的关键标准是：当 AprilTag 固定不动、头部处于不同姿态时，base_link -> tag36h11:0 的 Translation 应尽量保持稳定。

整个过程中最重要的经验是：在优化相机外参之前，必须先保证机器人运动学模型是正确的。TF 树要完整连通，joint_states 要正常发布，头部关节的 parent、child、origin 和 axis 都要符合真实机械结构。尤其是关节 axis 方向，如果实际电机正方向和 URDF 定义方向相反，即使相机外参再怎么优化，也会出现固定目标在 base_link 下大幅漂移的问题。

在实际标定中，我先完成 URDF 相机坐标系接入，然后启动机器人模型、D435i 相机和 AprilTag 检测节点，确认 camera_color_optical_frame -> tag36h11:0 和 base_link -> tag36h11:0 都能正常输出。随后固定 AprilTag，采集多个头部姿态下的样本，并通过最小二乘方法优化 head_camera_joint 的 6 个参数。

采样和优化过程中还需要注意数据质量。机器人每次运动后应等待稳定，AprilTag 尽量保持在图像中间区域，避免反光、遮挡、模糊和旧 TF。对于明显偏离其它样本的坏点，应在优化前剔除，否则少量异常数据就可能把最终外参带偏。后期优化时也不建议一次性大幅修改参数，而应采用“小步迭代”的方式：优化、写入 URDF、重新验证，如果效果变好再继续微调。

最终标定结果可以使固定 AprilTag 在不同头部姿态下的 base_link 坐标稳定到毫米到厘米级范围。剩余误差主要来自电机角度精度、机械回差、相机支架刚性、AprilTag 检测噪声和时间同步等因素。当目标精度接近 3mm 时，问题已经不只是相机外参本身，还需要进一步考虑关节零位补偿、运动方向一致性、多帧平均和机械结构稳定性。

总的来说，“眼在手上”的标定不能只看相机，也不能只调外参。它需要同时保证三件事：机器人运动学模型正确、视觉观测稳定、采样数据可靠。只有这三部分都做好，最终得到的相机外参才具有实际使用价值。

五、标定用到的相关代码

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
head_camera_6dof_calibration_v2.py

用途：
1. 交互式采集头部相机标定样本：
– /right/joint_states 中的 neck_down_rotate、neck_up_rotate
– TF: base_link -> neck_up_rotate
– TF: camera_link -> tag36h11:0

2. 对 head_camera_joint 的 6 个参数做最小二乘优化：
– x y z roll pitch yaw

v2 关键改动：
– collect 采样时会等待 AprilTag 的 TF 更新时间戳发生变化，避免采到 stale TF / 旧 TF。
– 每次按 Enter 后，会等待新的 camera_link -> tag36h11:0 变换。
– 如果 Tag TF 超时、不新鲜或没有更新，会拒绝保存该样本。
"""

import argparse
import json
import math
import os
import sys
import time
from typing import Dict, List, Optional, Tuple

import numpy as np

def import_scipy():
try:
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as R
return least_squares, R
except Exception as e:
print("错误：需要 scipy。请安装：sudo apt install python3-scipy", file=sys.stderr)
print(f"原始错误：{e}", file=sys.stderr)
sys.exit(1)

def make_T(xyz: np.ndarray, rpy: np.ndarray) –> np.ndarray:
_, R = import_scipy()
T = np.eye(4)
T[:3, :3] = R.from_euler("xyz", rpy).as_matrix()
T[:3, 3] = xyz
return T

def normalize_quat_xyzw(q: np.ndarray) –> np.ndarray:
n = np.linalg.norm(q)
if n < 1e-12:
raise ValueError("Quaternion norm too small")
return q / n

def T_from_xyz_quat(xyz: List[float], quat_xyzw: List[float]) –> np.ndarray:
_, R = import_scipy()
T = np.eye(4)
T[:3, 3] = np.array(xyz, dtype=float)
T[:3, :3] = R.from_quat(normalize_quat_xyzw(np.array(quat_xyzw, dtype=float))).as_matrix()
return T

def T_to_json(T: np.ndarray) –> List[List[float]]:
return [[float(v) for v in row] for row in T]

def T_from_json(obj: List[List[float]]) –> np.ndarray:
return np.array(obj, dtype=float)

def builtin_time_to_sec(stamp) –> float:
return float(stamp.sec) + float(stamp.nanosec) * 1e-9

def transform_msg_to_T(msg) –> np.ndarray:
t = msg.transform.translation
q = msg.transform.rotation
return T_from_xyz_quat([t.x, t.y, t.z], [q.x, q.y, q.z, q.w])

def load_samples(path: str) –> List[Dict]:
samples = []
with open(path, "r", encoding="utf-8") as f:
for line in f:
line = line.strip()
if not line or line.startswith("#"):
continue
samples.append(json.loads(line))
if len(samples) < 5:
print(f"警告：样本数只有 {len(samples)}，建议至少采集 20~50 组。", file=sys.stderr)
return samples

def compute_base_tag_positions(params: np.ndarray, samples: List[Dict]) –> Tuple[np.ndarray, List[np.ndarray]]:
T_neck_camera = make_T(params[:3], params[3:])
Ts_base_tag = []
positions = []
for s in samples:
T_base_neck = T_from_json(s["T_base_neck"])
T_camera_tag = T_from_json(s["T_camera_tag"])
T_base_tag = T_base_neck @ T_neck_camera @ T_camera_tag
Ts_base_tag.append(T_base_tag)
positions.append(T_base_tag[:3, 3])
return np.vstack(positions), Ts_base_tag

def position_stats(params: np.ndarray, samples: List[Dict], title: str = "") –> Dict[str, float]:
positions, _ = compute_base_tag_positions(params, samples)
mean = positions.mean(axis=0)
diffs = positions – mean
norms = np.linalg.norm(diffs, axis=1)
stats = {
"mean_x": float(mean[0]),
"mean_y": float(mean[1]),
"mean_z": float(mean[2]),
"rms_m": float(np.sqrt(np.mean(norms ** 2))),
"max_m": float(np.max(norms)),
"std_x_m": float(np.std(positions[:, 0])),
"std_y_m": float(np.std(positions[:, 1])),
"std_z_m": float(np.std(positions[:, 2])),
}
if title:
print(f"\\n===== {title} =====")
print(f"Tag 平均位置 base_link 下: [{stats['mean_x']:.6f}, {stats['mean_y']:.6f}, {stats['mean_z']:.6f}] m")
print(f"位置 RMS 误差: {stats['rms_m']*1000:.2f} mm")
print(f"位置 MAX 误差: {stats['max_m']*1000:.2f} mm")
print(f"XYZ 标准差: X={stats['std_x_m']*1000:.2f} mm, Y={stats['std_y_m']*1000:.2f} mm, Z={stats['std_z_m']*1000:.2f} mm")
return stats

def sample_id(s: Dict, idx: int) –> int:
return int(s.get("sample", idx + 1))

def compute_position_errors(params: np.ndarray, samples: List[Dict], use_median: bool = True) –> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""返回 positions, center, 每个样本到中心的距离。"""
positions, _ = compute_base_tag_positions(params, samples)
center = np.median(positions, axis=0) if use_median else positions.mean(axis=0)
errors = np.linalg.norm(positions – center, axis=1)
return positions, center, errors

def print_sample_error_table(params: np.ndarray, samples: List[Dict], title: str = "样本误差表", limit: Optional[int] = None):
positions, center, errors = compute_position_errors(params, samples, use_median=True)
order = np.argsort(errors)[::–1]
print(f"\\n===== {title} =====")
print(f"误差中心 median: [{center[0]:.6f}, {center[1]:.6f}, {center[2]:.6f}] m")
print("按误差从大到小排列：")
print("sample | error(mm) | base_link 下 tag 位置 [x y z] m | neck_down | neck_up")
shown = 0
for idx in order:
s = samples[idx]
p = positions[idx]
print(
f"{sample_id(s, idx):>6} | {errors[idx]*1000:>9.2f} | "
f"[{p[0]: .6f} {p[1]: .6f} {p[2]: .6f}] | "
f"{s.get('neck_down')} | {s.get('neck_up')}"
)
shown += 1
if limit is not None and shown >= limit:
break

def filter_outlier_samples(
samples: List[Dict],
params: np.ndarray,
threshold_m: float = 0.03,
mad_factor: float = 3.5,
min_samples: int = 6,
iterations: int = 2,
) –> Tuple[List[Dict], List[Dict]]:
"""
根据当前 params 计算 base_link -> tag 的聚类误差，自动剔除明显离群样本。

规则：
1. 以 median 作为中心，比 mean 更不容易被坏点带偏。
2. 每轮计算每个样本到 median 的距离。
3. 阈值 = max(threshold_m, median_error + mad_factor * robust_sigma)
其中 robust_sigma = 1.4826 * MAD。
4. 保证至少保留 min_samples 个样本。
"""
kept = list(samples)
removed: List[Dict] = []

if len(kept) <= min_samples:
print("样本数太少，不进行离群点剔除。")
return kept, removed

for it in range(iterations):
positions, center, errors = compute_position_errors(params, kept, use_median=True)
med = float(np.median(errors))
mad = float(np.median(np.abs(errors – med)))
robust_sigma = 1.4826 * mad
adaptive_threshold = med + mad_factor * robust_sigma
threshold = max(float(threshold_m), adaptive_threshold)

bad_idx = np.where(errors > threshold)[0].tolist()
if not bad_idx:
print(f"\\n离群点检测第 {it+1} 轮：没有发现需要剔除的样本。阈值 {threshold*1000:.2f} mm")
break

# 如果剔除后样本太少，只剔除误差最大的几个，保证 min_samples。
max_remove = max(0, len(kept) – min_samples)
if len(bad_idx) > max_remove:
bad_idx = sorted(bad_idx, key=lambda i: errors[i], reverse=True)[:max_remove]

print(f"\\n离群点检测第 {it+1} 轮：")
print(f"median error = {med*1000:.2f} mm, robust_sigma = {robust_sigma*1000:.2f} mm, 阈值 = {threshold*1000:.2f} mm")
print("将剔除：")
for i in sorted(bad_idx, key=lambda j: errors[j], reverse=True):
print(f" sample {sample_id(kept[i], i)}: error = {errors[i]*1000:.2f} mm")

bad_set = set(bad_idx)
new_kept = []
for i, s in enumerate(kept):
if i in bad_set:
removed.append(s)
else:
new_kept.append(s)

if len(new_kept) == len(kept):
break
kept = new_kept

if len(kept) <= min_samples:
print(f"已达到最小保留样本数 {min_samples}，停止剔除。")
break

return kept, removed

def save_samples_jsonl(samples: List[Dict], path: str):
with open(path, "w", encoding="utf-8") as f:
for s in samples:
f.write(json.dumps(s, ensure_ascii=False) + "\\n")

def optimize_mode(args):
least_squares, R = import_scipy()
samples = load_samples(args.samples)
initial = np.array([float(v) for v in args.initial.split()], dtype=float)
if initial.shape[0] != 6:
raise ValueError("–initial 必须是 6 个数字：x y z roll pitch yaw")

print(f"读取样本数: {len(samples)}")
print("初始 head_camera_joint:")
print(f"xyz = {initial[0]:.6f} {initial[1]:.6f} {initial[2]:.6f}")
print(f"rpy = {initial[3]:.6f} {initial[4]:.6f} {initial[5]:.6f}")

raw_samples = list(samples)
if args.print_sample_errors:
print_sample_error_table(initial, samples, "剔除前样本误差表")

if args.auto_remove_outliers:
samples, removed_samples = filter_outlier_samples(
samples,
initial,
threshold_m=args.outlier_threshold_mm / 1000.0,
mad_factor=args.outlier_mad_factor,
min_samples=args.min_samples_after_filter,
iterations=args.outlier_iterations,
)
print(f"\\n离群点剔除结果：原始 {len(raw_samples)} 组，保留 {len(samples)} 组，剔除 {len(removed_samples)} 组。")
if removed_samples:
print("剔除的 sample 编号：", [s.get("sample") for s in removed_samples])
if args.cleaned_output:
save_samples_jsonl(samples, args.cleaned_output)
print(f"已保存剔除坏点后的样本文件：{args.cleaned_output}")
if args.print_sample_errors:
print_sample_error_table(initial, samples, "剔除后样本误差表")
else:
print("\\n未启用自动离群点剔除。")

position_stats(initial, samples, "优化前")

def residual(p):
positions, Ts = compute_base_tag_positions(p, samples)
mean_pos = positions.mean(axis=0)
res = (positions – mean_pos).reshape(–1)
if args.orientation_weight > 0:
rots = R.from_matrix(np.stack([T[:3, :3] for T in Ts], axis=0))
mean_rot = rots.mean()
rot_res = []
for rot in rots:
rv = (mean_rot.inv() * rot).as_rotvec()
rot_res.extend((args.orientation_weight * rv).tolist())
res = np.concatenate([res, np.array(rot_res)])
return res

lower = initial – np.array([args.max_xyz_delta, args.max_xyz_delta, args.max_xyz_delta,
args.max_rpy_delta, args.max_rpy_delta, args.max_rpy_delta])
upper = initial + np.array([args.max_xyz_delta, args.max_xyz_delta, args.max_xyz_delta,
args.max_rpy_delta, args.max_rpy_delta, args.max_rpy_delta])

result = least_squares(
residual,
initial,
bounds=(lower, upper),
loss=args.loss,
f_scale=args.f_scale,
max_nfev=args.max_nfev,
verbose=1 if args.verbose else 0,
)

opt = result.x
position_stats(opt, samples, "优化后")
print("\\n===== 优化结果 =====")
print(f"success: {result.success}")
print(f"message: {result.message}")
print("\\n建议写入 URDF：")
print('<joint name="head_camera_joint" type="fixed">')
print(' <parent link="neck_up_rotate"/>')
print(' <child link="head_camera_link"/>')
print(f' <origin xyz="{opt[0]:.6f} {opt[1]:.6f} {opt[2]:.6f}"')
print(f' rpy="{opt[3]:.6f} {opt[4]:.6f} {opt[5]:.6f}"/>')
print('</joint>')
print("\\n参数变化量：")
delta = opt – initial
print(f"dx dy dz = {delta[0]*1000:.3f} {delta[1]*1000:.3f} {delta[2]*1000:.3f} mm")
print(f"droll dpitch dyaw = {math.degrees(delta[3]):.4f} {math.degrees(delta[4]):.4f} {math.degrees(delta[5]):.4f} deg")
if args.output:
out = {"optimized_xyz_rpy": opt.tolist(), "initial_xyz_rpy": initial.tolist(), "success": bool(result.success), "message": str(result.message)}
with open(args.output, "w", encoding="utf-8") as f:
json.dump(out, f, ensure_ascii=False, indent=2)
print(f"\\n优化结果已保存：{args.output}")

def collect_mode(args):
try:
import rclpy
from sensor_msgs.msg import JointState
import tf2_ros
except Exception as e:
print("错误：collect 模式需要 ROS2 Python 环境。请先 source /opt/ros/humble/setup.bash，并不要使用 conda base。", file=sys.stderr)
print(f"原始错误：{e}", file=sys.stderr)
sys.exit(1)

class Collector(rclpy.node.Node):
def __init__(self):
super().__init__("head_camera_calib_collector_v2")
self.last_joint_state = None
self.last_tag_stamp_sec = None
self.sub = self.create_subscription(JointState, args.joint_topic, self.joint_cb, 10)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)

def joint_cb(self, msg):
self.last_joint_state = msg

def get_joint_value(self, name: str) –> Optional[float]:
if self.last_joint_state is None:
return None
names = list(self.last_joint_state.name)
try:
idx = names.index(name)
return float(self.last_joint_state.position[idx])
except ValueError:
return None

def lookup_transform_msg(self, target: str, source: str, timeout_sec: float = 2.0):
start = time.time()
last_err = None
while time.time() – start < timeout_sec:
rclpy.spin_once(self, timeout_sec=0.03)
try:
return self.tf_buffer.lookup_transform(
target, source, rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1)
)
except Exception as e:
last_err = e
raise RuntimeError(f"无法获取 TF: {target} -> {source}. 最后错误: {last_err}")

def wait_fresh_tag_transform(self):
start = time.time()
last_err = None
printed_wait = False
while time.time() – start < args.tag_wait_timeout:
rclpy.spin_once(self, timeout_sec=0.03)
try:
msg = self.tf_buffer.lookup_transform(
args.camera_frame, args.tag_frame, rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1)
)
except Exception as e:
last_err = e
continue

stamp_sec = builtin_time_to_sec(msg.header.stamp)
now_sec = self.get_clock().now().nanoseconds * 1e-9
age = now_sec – stamp_sec

if stamp_sec <= 0.0:
last_err = f"Tag TF 时间戳为 0，疑似无效或静态 TF。stamp={stamp_sec}"
continue
if args.require_new_tag and self.last_tag_stamp_sec is not None:
if stamp_sec <= self.last_tag_stamp_sec + args.min_stamp_delta:
if not printed_wait:
print(" 等待新的 AprilTag TF，不保存旧 TF…")
printed_wait = True
last_err = f"Tag TF 还未更新：stamp={stamp_sec:.6f}, last={self.last_tag_stamp_sec:.6f}"
continue
if age > args.max_tag_age:
last_err = f"Tag TF 太旧：age={age:.3f}s，大于 {args.max_tag_age:.3f}s"
continue
self.last_tag_stamp_sec = stamp_sec
return msg, stamp_sec, age
raise RuntimeError(f"等待新鲜 Tag TF 超时：{args.camera_frame} -> {args.tag_frame}. 最后错误: {last_err}")

rclpy.init()
node = Collector()
out_dir = os.path.dirname(os.path.abspath(args.out))
if out_dir:
os.makedirs(out_dir, exist_ok=True)

print("\\n开始采集。采集前确认：")
print("1. AprilTag 固定不动。")
print("2. D435i、AprilTag、robot_state_publisher 都已启动。")
print("3. neck_down_rotate 和 neck_up_rotate 的 axis 已经改正确。")
print("4. 每次移动头部到新姿态后，等画面稳定，再按 Enter 采样。")
print("5. v2 会等待 AprilTag TF 更新时间戳，避免采到旧 TF；optimize 默认自动剔除明显坏点。\\n")
print(f"采集 TF：{args.base_frame} -> {args.neck_frame}")
print(f"采集 TF：{args.camera_frame} -> {args.tag_frame}")
print(f"采集关节：{args.joint_topic} 中的 {args.neck_down_joint}, {args.neck_up_joint}")
print(f"输出文件：{args.out}")
print(f"Tag TF 最大允许年龄：{args.max_tag_age:.3f}s，等待超时：{args.tag_wait_timeout:.1f}s\\n")

print("等待 joint_states…")
t0 = time.time()
while node.last_joint_state is None and time.time() – t0 < 5.0:
rclpy.spin_once(node, timeout_sec=0.1)
if node.last_joint_state is None:
print(f"警告：5秒内没有收到 {args.joint_topic}，joint 值会为空。")
else:
names = list(node.last_joint_state.name)
if args.neck_down_joint not in names or args.neck_up_joint not in names:
print("警告：joint_states 中没有找到指定头部关节名。")
print(f"当前 joint names: {names}")

count = 0
with open(args.out, "a", encoding="utf-8") as f:
while True:
if args.num_samples > 0 and count >= args.num_samples:
break
cmd = input(f"[{count+1}] 移动到新姿态并稳定后按 Enter 采样，输入 q 结束：").strip().lower()
if cmd in ("q", "quit", "exit"):
break
if args.settle_sec > 0:
time.sleep(args.settle_sec)
try:
tag_msg, tag_stamp_sec, tag_age = node.wait_fresh_tag_transform()
T_camera_tag = transform_msg_to_T(tag_msg)
base_neck_msg = node.lookup_transform_msg(args.base_frame, args.neck_frame, args.tf_timeout)
T_base_neck = transform_msg_to_T(base_neck_msg)
except Exception as e:
print(f"采样失败：{e}")
print("请确认 Tag 在画面中、/tf 正在更新，然后再次按 Enter。")
continue
neck_down = node.get_joint_value(args.neck_down_joint)
neck_up = node.get_joint_value(args.neck_up_joint)
sample = {
"sample": count + 1,
"time_wall": time.time(),
"tag_tf_stamp": tag_stamp_sec,
"tag_tf_age": tag_age,
"neck_down": neck_down,
"neck_up": neck_up,
"frames": {"base_frame": args.base_frame, "neck_frame": args.neck_frame, "camera_frame": args.camera_frame, "tag_frame": args.tag_frame},
"T_base_neck": T_to_json(T_base_neck),
"T_camera_tag": T_to_json(T_camera_tag),
}
f.write(json.dumps(sample, ensure_ascii=False) + "\\n")
f.flush()
p_base_neck = T_base_neck[:3, 3]
p_camera_tag = T_camera_tag[:3, 3]
print(f"已采样 {count+1}: neck_down={neck_down}, neck_up={neck_up}")
print(f" tag_tf_stamp = {tag_stamp_sec:.6f}, age = {tag_age:.3f}s")
print(f" base->neck t = [{p_base_neck[0]: .5f} {p_base_neck[1]: .5f} {p_base_neck[2]: .5f}]")
print(f" camera->tag t = [{p_camera_tag[0]: .5f} {p_camera_tag[1]: .5f} {p_camera_tag[2]: .5f}]")
count += 1
print(f"\\n采集完成，共 {count} 组，保存到：{args.out}")
rclpy.shutdown()

def inspect_mode(args):
samples = load_samples(args.samples)
print(f"样本数: {len(samples)}")
prev_stamp = None
prev_camera_tag = None
for s in samples[:args.limit]:
Tbn = T_from_json(s["T_base_neck"])
Tct = T_from_json(s["T_camera_tag"])
stamp = s.get("tag_tf_stamp")
age = s.get("tag_tf_age")
p = Tct[:3, 3]
warn = ""
if prev_stamp is not None and stamp is not None and stamp <= prev_stamp:
warn += " [警告: tag_tf_stamp 未增加]"
if prev_camera_tag is not None and np.linalg.norm(p – prev_camera_tag) < args.same_translation_threshold:
warn += " [注意: camera_tag_t 与上一组几乎相同]"
print(
f"sample {s.get('sample')}: neck_down={s.get('neck_down')}, neck_up={s.get('neck_up')}, "
f"tag_stamp={stamp}, age={age}, base_neck_t={Tbn[:3,3]}, camera_tag_t={p}{warn}"
)
prev_stamp = stamp
prev_camera_tag = p.copy()

def main():
parser = argparse.ArgumentParser(description="头部 D435i 相机 head_camera_joint 6参数最小二乘优化工具 v2")
sub = parser.add_subparsers(dest="mode", required=True)

p_collect = sub.add_parser("collect", help="交互式采集 TF 样本，v2 会等待新鲜 Tag TF")
p_collect.add_argument("–out", default="head_camera_samples.jsonl", help="输出样本文件")
p_collect.add_argument("–num-samples", type=int, default=0, help="采样数量；0 表示手动 q 结束")
p_collect.add_argument("–joint-topic", default="/right/joint_states")
p_collect.add_argument("–neck-down-joint", default="neck_down_rotate")
p_collect.add_argument("–neck-up-joint", default="neck_up_rotate")
p_collect.add_argument("–base-frame", default="base_link")
p_collect.add_argument("–neck-frame", default="neck_up_rotate")
p_collect.add_argument("–camera-frame", default="camera_link")
p_collect.add_argument("–tag-frame", default="tag36h11:0")
p_collect.add_argument("–tf-timeout", type=float, default=2.0)
p_collect.add_argument("–tag-wait-timeout", type=float, default=5.0, help="等待新 Tag TF 的最长时间")
p_collect.add_argument("–max-tag-age", type=float, default=0.40, help="Tag TF 最大允许年龄，秒")
p_collect.add_argument("–min-stamp-delta", type=float, default=1e-6, help="判断 TF 更新时间戳变化的最小秒数")
p_collect.add_argument("–settle-sec", type=float, default=0.30, help="按 Enter 后额外等待稳定时间")
p_collect.add_argument("–require-new-tag", action="store_true", default=True, help="要求每个样本使用新的 Tag TF")
p_collect.set_defaults(func=collect_mode)

p_opt = sub.add_parser("optimize", help="读取样本并优化 head_camera_joint")
p_opt.add_argument("–samples", default="head_camera_samples.jsonl")
p_opt.add_argument("–initial", default="-0.0570 0.0752 -0.0313 -1.5699 0.0143 1.5939", help='初始值："x y z roll pitch yaw"，单位 m/rad')
p_opt.add_argument("–max-xyz-delta", type=float, default=0.05, help="xyz 最大允许变化量，单位 m")
p_opt.add_argument("–max-rpy-delta", type=float, default=0.20, help="rpy 最大允许变化量，单位 rad")
p_opt.add_argument("–orientation-weight", type=float, default=0.0, help="姿态一致性权重，默认0表示只优化位置")
p_opt.add_argument("–loss", default="soft_l1", choices=["linear", "soft_l1", "huber", "cauchy", "arctan"])
p_opt.add_argument("–f-scale", type=float, default=0.01)
p_opt.add_argument("–max-nfev", type=int, default=2000)
p_opt.add_argument("–verbose", action="store_true")
p_opt.add_argument("–output", default="head_camera_optimized.json")
p_opt.add_argument("–auto-remove-outliers", dest="auto_remove_outliers", action="store_true", default=True, help="优化前自动剔除明显坏点，默认开启")
p_opt.add_argument("–no-auto-remove-outliers", dest="auto_remove_outliers", action="store_false", help="关闭自动剔除坏点")
p_opt.add_argument("–outlier-threshold-mm", type=float, default=10.0, help="坏点剔除的最小阈值，单位 mm，默认 10mm")
p_opt.add_argument("–outlier-mad-factor", type=float, default=3.5, help="MAD 自适应阈值系数，默认 3.5")
p_opt.add_argument("–outlier-iterations", type=int, default=2, help="离群点剔除迭代次数，默认 2")
p_opt.add_argument("–min-samples-after-filter", type=int, default=6, help="剔除后至少保留多少组样本，默认 6")
p_opt.add_argument("–cleaned-output", default="head_camera_samples_clean.jsonl", help="保存自动剔除坏点后的样本文件")
p_opt.add_argument("–print-sample-errors", action="store_true", help="打印每个样本的误差排序，便于人工检查")
p_opt.set_defaults(func=optimize_mode)

p_inspect = sub.add_parser("inspect", help="快速查看样本文件，并检查 TF 是否 stale")
p_inspect.add_argument("–samples", default="head_camera_samples.jsonl")
p_inspect.add_argument("–limit", type=int, default=10)
p_inspect.add_argument("–same-translation-threshold", type=float, default=1e-5)
p_inspect.set_defaults(func=inspect_mode)

args = parser.parse_args()
args.func(args)

if __name__ == "__main__":
main()