HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

seperate left/right arm control

Browse Source
This commit is contained in:
david
2025-10-16 19:14:29 +08:00
parent 5d4aaff556
commit e342eebb1c
11 changed files with 1480 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -3,3 +3,4 @@ install/
log/
.vscode/
.venv/

7
src/brain/include/brain/skill_manager.hpp
View File

@@ -264,6 +264,7 @@ private:
auto it = action_override_registrars_.find(base);
if (it != action_override_registrars_.end()) {
it->second(topic, internal_skill);
RCLCPP_INFO(node_->get_logger(), "Overridden action client registration: %s", base.c_str());
return;
}
using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
@@ -272,7 +273,7 @@ private:
internal_skill,
std::function<typename ActionT::Goal()>{},
std::function<void(typename GoalHandle::SharedPtr,
const std::shared_ptr<const typename ActionT::Feedback>)>{},
const std::shared_ptr<const typename ActionT::Feedback>)>{},
std::function<void(const typename GoalHandle::WrappedResult &)> {},
std::function<void(const std::shared_ptr<GoalHandle> &)> {});
}
@@ -283,7 +284,7 @@ private:
const std::string & internal_skill,
const std::function<typename ActionT::Goal()> & make_goal_override,
const std::function<void(typename ActionClientRegistry::GoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const typename ActionT::Feedback>)> & on_feedback_override,
const std::shared_ptr<const typename ActionT::Feedback>)> & on_feedback_override,
const std::function<void(const typename ActionClientRegistry::GoalHandle<ActionT>::WrappedResult &)> & on_result_override,
const std::function<void(const std::shared_ptr<typename ActionClientRegistry::GoalHandle<ActionT>> &)> & on_goal_response_override)
{
@@ -344,7 +345,7 @@ template<>
struct SkillActionTrait<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static bool success(const interfaces::action::Arm::Result & r) {return r.result;}
static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0) ? true : false;}
static std::string message(const interfaces::action::Arm::Result & r)
{
(void)r;

71
src/brain/src/cerebellum_node.cpp
View File

@@ -35,6 +35,7 @@
#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/leg_control.hpp"
#include "interfaces/action/arm.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "brain_interfaces/action/execute_bt_action.hpp" // ExecuteBtAction bridge
#include "interfaces/action/slam_mode.hpp"
@@ -50,6 +51,7 @@
namespace brain
{
static thread_local int tls_arm_body_id = -1;
/**
* @brief Construct the CerebellumNode.
@@ -162,14 +164,16 @@ void CerebellumNode::ConfigureActionHooks()
arm_hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::Arm::Goal goal{};
goal.body_id = 0; //LEFT_ARM=0, RIGHT_ARM=1
goal.body_id = (tls_arm_body_id == 0 || tls_arm_body_id == 1) ? tls_arm_body_id : 0; // LEFT_ARM=0, RIGHT_ARM=1
goal.data_type = 3; //ARM_COMMAND_TYPE_POSE_DIRECT_MOVE
goal.command_id = ++command_id_;
if (target_pose_.empty()) {
RCLCPP_WARN(this->get_logger(), "[%s] 未检测到目标物体，使用默认位置", skill_name.c_str());
// default position
goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
// goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
goal.data_array = {0.222853, 0.514124, 0.261742,
-0.65115316, 0.05180144, -0.19539139, 0.73153153};
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
} else {
RCLCPP_INFO(this->get_logger(), "[%s] 使用检测到的目标物体位置", skill_name.c_str());
@@ -177,7 +181,9 @@ void CerebellumNode::ConfigureActionHooks()
if (pose_it == target_pose_.end()) {
RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
skill_name.c_str(), target_frame_.c_str());
goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
// goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
goal.data_array = {0.222853, 0.514124, 0.261742,
-0.65115316, 0.05180144, -0.19539139, 0.73153153};
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
} else {
geometry_msgs::msg::PoseStamped pose_in_arm;
@@ -229,7 +235,7 @@ void CerebellumNode::ConfigureActionHooks()
arm_hooks.on_result = [this](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result;
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0);
const std::string message = res.result ? std::string("action end") : std::string("无返回信息");
if (success) {
RCLCPP_INFO(this->get_logger(), "[%s] 完成: %s", skill_name.c_str(), message.c_str());
@@ -1088,6 +1094,60 @@ bool CerebellumNode::ExecuteActionSkill(
}
auto timeout = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(GetTimeoutForSkill(skill)));
if (skill == "Arm") {
RCLCPP_INFO(this->get_logger(), "[Arm] 并发发送两个 goal: body_id=1 与 body_id=0");
std::atomic<bool> finished1{false}, finished2{false};
bool ok1 = false, ok2 = false;
std::string d1, d2;
auto worker = [this, &ok_out = ok1, &done_flag = finished1, &d_out = d1,
skill, topic, timeout](int body_id) {
tls_arm_body_id = body_id;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
done_flag.store(true, std::memory_order_release);
};
std::thread t1(worker, 1); // RIGHT_ARM=1
auto worker2 = [this, &ok_out = ok2, &done_flag = finished2, &d_out = d2,
skill, topic, timeout](int body_id) {
tls_arm_body_id = body_id;
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
skill, topic, action_clients_.get(), this->get_logger(), timeout, d_out);
done_flag.store(true, std::memory_order_release);
};
std::thread t2(worker2, 0); // LEFT_ARM=0
const auto start_steady = std::chrono::steady_clock::now();
const double timeout_sec = timeout.count() / 1e9;
while (!(finished1.load(std::memory_order_acquire) && finished2.load(std::memory_order_acquire))) {
auto now = std::chrono::steady_clock::now();
double elapsed = std::chrono::duration<double>(now - start_steady).count();
double phase = timeout_sec > 0.0 ? std::min(elapsed / timeout_sec, 0.99) : 0.0;
double sub_done = (finished1.load(std::memory_order_relaxed) ? 1.0 : phase)
+ (finished2.load(std::memory_order_relaxed) ? 1.0 : phase);
double avg_phase = 0.5 * std::min(sub_done, 2.0);
float seq_progress =
(total_skills > 0) ? static_cast<float>((index + avg_phase) / total_skills) : 0.f;
PublishFeedbackStage(goal_handle, "RUN", skill, seq_progress, "");
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
if (t1.joinable()) t1.join();
if (t2.joinable()) t2.join();
if (!ok1 || !ok2) {
std::ostringstream oss;
if (!ok1) { oss << "Arm(body_id=1) 失败"; if (!d1.empty()) oss << ": " << d1; }
if (!ok2) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) 失败"; if (!d2.empty()) oss << ": " << d2; }
detail = oss.str();
return false;
}
detail.clear();
return true;
}
// 其他动作按一次发送
return RunActionSkillWithProgress(
skill, topic, timeout, index, total_skills, goal_handle, detail);
}
@@ -1338,7 +1398,8 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
}
const auto & skill = skills[seq_index];
const std::string topic = ResolveTopicForSkill(skill);
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s", skill.c_str(), topic.c_str());
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d",
skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
bool ok = false; std::string detail;
auto it = skill_manager_->skills().find(skill);
if (it == skill_manager_->skills().end()) {

27
src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
View File

@@ -44,7 +44,7 @@ private:
// 伪造两类目标 (示例: cup / bottle)，每类给出 1~2 个 Pose
interfaces::msg::PoseClassAndID obj1;
obj1.class_name = "bottle";
obj1.class_name = "bottlex";
obj1.class_id = 1;
//{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
@@ -59,13 +59,24 @@ private:
//geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523),
//orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
pose1.position.x = -0.2851924786746129;
pose1.position.y = -0.056353812689333635;
pose1.position.z = 1.073772448259523;
pose1.orientation.x = -0.16713377464857188;
pose1.orientation.y = -0.42460237568763715;
pose1.orientation.z = -0.26706232441180955;
pose1.orientation.w = 0.8487972895879773;
// pose1.position.x = -0.2851924786746129;
// pose1.position.y = -0.056353812689333635;
// pose1.position.z = 1.073772448259523;
// pose1.orientation.x = -0.16713377464857188;
// pose1.orientation.y = -0.42460237568763715;
// pose1.orientation.z = -0.26706232441180955;
// pose1.orientation.w = 0.8487972895879773;
//222.853, 514.124, 261.742
//-0.65115316, 0.05180144, -0.19539139, 0.73153153
//-0.63142093, 0.18186004, -0.12407289, 0.74353241
pose1.position.x = 222.853;
pose1.position.y = 514.124;
pose1.position.z = 261.742;
pose1.orientation.x = -0.65115316;
pose1.orientation.y = 0.05180144;
pose1.orientation.z = -0.19539139;
pose1.orientation.w = 0.73153153;
obj1.pose_list.push_back(pose1);

46
src/scripts/euler_to_equa.py Normal file
View File

@@ -0,0 +1,46 @@
import numpy as np
from scipy.spatial.transform import Rotation as R
# # 欧拉角
# rx, ry, rz = -1.433, 0.114, -0.430
# # 尝试不同的旋转顺序
# orders = ['xyz', 'zyx', 'xzy', 'yxz', 'yzx', 'zyx']
# print("尝试不同旋转顺序的结果：")
# for order in orders:
# try:
# rot = R.from_euler(order, [rx, ry, rz], degrees=False)
# quat = rot.as_quat() # [x, y, z, w]
# print(f"顺序 {order}: [{quat[0]:.8f}, {quat[1]:.8f}, {quat[2]:.8f}, {quat[3]:.8f}]")
# except:
# continue
# # 特别检查XYZ顺序
# print("\nXYZ顺序的详细计算：")
# rot_xyz = R.from_euler('xyz', [rx, ry, rz], degrees=False)
# quat_xyz = rot_xyz.as_quat()
# print(f"XYZ顺序: [{quat_xyz[0]:.8f}, {quat_xyz[1]:.8f}, {quat_xyz[2]:.8f}, {quat_xyz[3]:.8f}]")
# 输入欧拉角 (rx, ry, rz)，单位弧度
# euler_angles_rad = [-1.433, 0.114, -0.430]
# euler_angles_rad = [-1.622, -0.016, -0.4]
# euler_angles_rad = [-0.972, -0.557, -0.901]
# euler_angles_rad = [-1.674, 0.223, -0.747]
# euler_angles_rad = [-1.484, 0.152, -0.26]
# euler_angles_rad = [-1.691, -0.443, -0.452]
# euler_angles_rad = [-1.72, -0.189, -0.223]
# euler_angles_rad = [-2.213, -0.202, -0.391]
# euler_angles_rad = [-1.358, -0.284, -0.255]
# euler_angles_rad = [-1.509, -0.234, -0.416]
# euler_angles_rad = [-1.79, -0.447, -0.133]
euler_angles_rad = [-1.468, 0.199, -0.361]
# 创建旋转对象，指定欧拉角顺序为 'xyz'
rot = R.from_euler('xyz', euler_angles_rad, degrees=False)
# 转换为四元数，格式为 [x, y, z, w]
quaternion = rot.as_quat()
print(f"四元数 (x, y, z, w): [{quaternion[0]:.8f}, {quaternion[1]:.8f}, {quaternion[2]:.8f}, {quaternion[3]:.8f}]")

207
src/scripts/euler_to_quaternion.py Normal file
View File

@@ -0,0 +1,207 @@
#!/usr/bin/env python3
"""
Convert poses to quaternions (qx, qy, qz, qw) from either:
- Euler angles (rx, ry, rz)
- Rotation vector (Rodrigues) (rx, ry, rz) where |r| is angle in rad
Supports:
- CSV input/output with configurable column names
- Unit options: radians/degrees for angles, mm->m for position
- Euler order: xyz (default) or zyx
Examples:
python src/scripts/euler_to_quaternion.py \
--in poses.csv --out poses_quat.csv --mode euler \
--rx rx --ry ry --rz rz --order xyz \
--x x --y y --z z --mm-to-m
# Single value
python src/scripts/euler_to_quaternion.py --mode euler --single 0.1 0.2 -0.3 --order zyx
# Rotation vector (e.g., from many robots):
python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
"""
from __future__ import annotations
import argparse
import csv
import math
from typing import Tuple, List
def euler_to_quaternion(rx: float, ry: float, rz: float, order: str = "xyz", degrees: bool = False) -> Tuple[float, float, float, float]:
"""Convert Euler angles to quaternion [x, y, z, w].
Args:
rx, ry, rz: Euler angles. If degrees=True, values are in degrees; else radians.
order: Rotation order. Supported: 'xyz' (default), 'zyx'. Intrinsic rotations.
degrees: Whether the input angles are in degrees.
Returns:
(qx, qy, qz, qw)
"""
if degrees:
rx, ry, rz = math.radians(rx), math.radians(ry), math.radians(rz)
def q_from_axis_angle(ax: str, angle: float) -> Tuple[float, float, float, float]:
s = math.sin(angle / 2.0)
c = math.cos(angle / 2.0)
if ax == 'x':
return (s, 0.0, 0.0, c)
if ax == 'y':
return (0.0, s, 0.0, c)
if ax == 'z':
return (0.0, 0.0, s, c)
raise ValueError('Invalid axis')
def q_mul(q1: Tuple[float, float, float, float], q2: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
return (x, y, z, w)
if order == 'xyz':
qx = q_from_axis_angle('x', rx)
qy = q_from_axis_angle('y', ry)
qz = q_from_axis_angle('z', rz)
q = q_mul(q_mul(qx, qy), qz)
elif order == 'zyx':
qz = q_from_axis_angle('z', rz)
qy = q_from_axis_angle('y', ry)
qx = q_from_axis_angle('x', rx)
q = q_mul(q_mul(qz, qy), qx)
else:
raise ValueError("Unsupported Euler order. Use 'xyz' or 'zyx'.")
# Normalize
norm = math.sqrt(sum(v * v for v in q))
return tuple(v / (norm + 1e-12) for v in q) # type: ignore
def rotvec_to_quaternion(rx: float, ry: float, rz: float) -> Tuple[float, float, float, float]:
"""Convert rotation vector (Rodrigues) to quaternion [x,y,z,w].
The vector direction is the rotation axis and its norm is the rotation angle in radians.
"""
angle = math.sqrt(rx * rx + ry * ry + rz * rz)
if angle < 1e-12:
return (0.0, 0.0, 0.0, 1.0)
ax = rx / angle
ay = ry / angle
az = rz / angle
s = math.sin(angle / 2.0)
c = math.cos(angle / 2.0)
q = (ax * s, ay * s, az * s, c)
norm = math.sqrt(sum(v * v for v in q))
return tuple(v / (norm + 1e-12) for v in q) # type: ignore
def convert_csv(in_path: str, out_path: str,
rx_col: str = 'rx', ry_col: str = 'ry', rz_col: str = 'rz',
x_col: str | None = None, y_col: str | None = None, z_col: str | None = None,
order: str = 'xyz', degrees: bool = False, mm_to_m: bool = False,
mode: str = 'euler') -> None:
"""Read CSV, append quaternion columns (qx,qy,qz,qw) and write to output.
If x/y/z columns are provided, optionally convert mm->m.
"""
with open(in_path, 'r', newline='') as f_in:
reader = csv.DictReader(f_in)
fieldnames: List[str] = list(reader.fieldnames or [])
# Ensure quaternion columns at the end
for c in ['qx', 'qy', 'qz', 'qw']:
if c not in fieldnames:
fieldnames.append(c)
# If converting position units, overwrite or add x_m/y_m/z_m
if x_col and y_col and z_col and mm_to_m:
for c in ['x_m', 'y_m', 'z_m']:
if c not in fieldnames:
fieldnames.append(c)
with open(out_path, 'w', newline='') as f_out:
writer = csv.DictWriter(f_out, fieldnames=fieldnames)
writer.writeheader()
for row in reader:
try:
rx = float(row[rx_col])
ry = float(row[ry_col])
rz = float(row[rz_col])
except KeyError as e:
raise KeyError(f"Missing rotation columns in CSV: {e}")
if mode == 'euler':
qx, qy, qz, qw = euler_to_quaternion(rx, ry, rz, order=order, degrees=degrees)
elif mode == 'rotvec':
qx, qy, qz, qw = rotvec_to_quaternion(rx, ry, rz)
else:
raise ValueError("mode must be 'euler' or 'rotvec'")
row.update({'qx': f"{qx:.8f}", 'qy': f"{qy:.8f}", 'qz': f"{qz:.8f}", 'qw': f"{qw:.8f}"})
if x_col and y_col and z_col and (x_col in row and y_col in row and z_col in row):
try:
x = float(row[x_col])
y = float(row[y_col])
z = float(row[z_col])
if mm_to_m:
row['x_m'] = f"{x / 1000.0:.6f}"
row['y_m'] = f"{y / 1000.0:.6f}"
row['z_m'] = f"{z / 1000.0:.6f}"
except ValueError:
pass
writer.writerow(row)
def main():
parser = argparse.ArgumentParser(description='Convert pose angles to quaternion (qx,qy,qz,qw) from Euler or rotation vector.')
parser.add_argument('--mode', type=str, default='euler', choices=['euler', 'rotvec'], help='Input angle format')
parser.add_argument('--in', dest='in_path', type=str, help='Input CSV file path')
parser.add_argument('--out', dest='out_path', type=str, help='Output CSV file path')
parser.add_argument('--rx', type=str, default='rx', help='Column name for rx')
parser.add_argument('--ry', type=str, default='ry', help='Column name for ry')
parser.add_argument('--rz', type=str, default='rz', help='Column name for rz')
parser.add_argument('--x', type=str, default=None, help='Column name for x (position)')
parser.add_argument('--y', type=str, default=None, help='Column name for y (position)')
parser.add_argument('--z', type=str, default=None, help='Column name for z (position)')
parser.add_argument('--order', type=str, default='xyz', choices=['xyz', 'zyx'], help='Euler order')
parser.add_argument('--degrees', action='store_true', help='Input angles are in degrees (default radians)')
parser.add_argument('--mm-to-m', action='store_true', help='Convert position units from mm to m when x/y/z provided')
# single conversion mode
parser.add_argument('--single', nargs=3, type=float, metavar=('RX', 'RY', 'RZ'),
help='Convert a single Euler triplet to quaternion (prints to stdout)')
args = parser.parse_args()
if args.single is not None:
rx, ry, rz = args.single
if args.mode == 'euler':
q = euler_to_quaternion(rx, ry, rz, order=args.order, degrees=args.degrees)
else:
q = rotvec_to_quaternion(rx, ry, rz)
print(f"qx,qy,qz,qw = {q[0]:.8f}, {q[1]:.8f}, {q[2]:.8f}, {q[3]:.8f}")
return
if not args.in_path or not args.out_path:
parser.error('CSV mode requires --in and --out')
convert_csv(
in_path=args.in_path,
out_path=args.out_path,
rx_col=args.rx,
ry_col=args.ry,
rz_col=args.rz,
x_col=args.x,
y_col=args.y,
z_col=args.z,
order=args.order,
degrees=args.degrees,
mm_to_m=args.mm_to_m,
mode=args.mode,
)
print(f"Written: {args.out_path}")
if __name__ == '__main__':
main()

588
src/scripts/hand_eye_cali.py Normal file
View File

@@ -0,0 +1,588 @@
import numpy as np
import cv2
from typing import List, Tuple
import argparse
import json
import os
import glob
import csv
def hand_eye_calibration(robot_poses: List[np.ndarray],
camera_poses: List[np.ndarray],
mode: str = 'eye_in_hand') -> np.ndarray:
"""
执行手眼标定，解决AX = XB问题。
参数:
robot_poses (list of np.array): 机械臂末端在基座坐标系中的位姿列表 (4x4齐次矩阵)。
camera_poses (list of np.array): 标定板在相机坐标系中的位姿列表 (4x4齐次矩阵)。注意：这是 T_c_t（target->camera），与OpenCV的 R_target2cam/t_target2cam 一致。
mode (str): 标定模式。'eye_in_hand' 或 'eye_to_hand'。
返回:
X (np.array): 求解出的变换矩阵X (4x4齐次矩阵)。
对于 eye_in_hand: X = camera^T_end_effector (相机在机械臂末端坐标系中的位姿)
对于 eye_to_hand: X = base^T_camera (相机在机器人基座坐标系中的位姿)
"""
# 输入验证
n = len(robot_poses)
if n != len(camera_poses):
raise ValueError("机器人位姿数量与相机位姿数量必须相同。")
if n < 3:
raise ValueError("至少需要三组数据（且包含显著旋转）才能进行稳定标定。")
# OpenCV calibrateHandEye 需要每一帧的绝对位姿：
# - R_gripper2base, t_gripper2base: T_g^b（夹爪到基座）
# - R_target2cam, t_target2cam: T_t^c（标定板到相机）
# 传入时请确保 robot_poses 为 T_b^g（基座到夹爪），camera_poses 为 T_c^t（目标到相机）
R_gripper2base: List[np.ndarray] = []
t_gripper2base: List[np.ndarray] = []
R_target2cam: List[np.ndarray] = []
t_target2cam: List[np.ndarray] = []
for i in range(n):
T_b_g = robot_poses[i]
if T_b_g.shape != (4, 4):
raise ValueError("robot_poses 中的矩阵必须是 4x4")
T_g_b = np.linalg.inv(T_b_g)
R_gripper2base.append(T_g_b[:3, :3])
t_gripper2base.append(T_g_b[:3, 3].reshape(3, 1))
T_c_t = camera_poses[i]
if T_c_t.shape != (4, 4):
raise ValueError("camera_poses 中的矩阵必须是 4x4")
R_target2cam.append(T_c_t[:3, :3])
t_target2cam.append(T_c_t[:3, 3].reshape(3, 1))
# 选择算法，失败时自动尝试其他方法
methods = [
getattr(cv2, 'CALIB_HAND_EYE_TSAI', None),
getattr(cv2, 'CALIB_HAND_EYE_PARK', None),
getattr(cv2, 'CALIB_HAND_EYE_HORAUD', None),
getattr(cv2, 'CALIB_HAND_EYE_ANDREFF', None),
getattr(cv2, 'CALIB_HAND_EYE_DANIILIDIS', None),
]
methods = [m for m in methods if m is not None]
last_err = None
for method in methods:
try:
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
R_gripper2base, t_gripper2base,
R_target2cam, t_target2cam,
method=method,
)
# 成功返回
X_cam_gripper = np.eye(4)
X_cam_gripper[:3, :3] = R_cam2gripper
X_cam_gripper[:3, 3] = t_cam2gripper.flatten()
if mode == 'eye_in_hand':
# 直接返回 camera^T_gripper
return X_cam_gripper
else:
# eye_to_hand：需要 base^T_camera。已知：
# 对每一帧 i，有 T_b^c_i = T_b^g_i * T_g^c（其中 T_g^c = (T_c^g)^{-1}）
T_g_c = np.linalg.inv(X_cam_gripper)
T_b_c_list = []
for T_b_g in robot_poses:
T_b_c_list.append(T_b_g @ T_g_c)
return average_SE3(T_b_c_list)
except Exception as e:
last_err = e
continue
raise RuntimeError(f"hand-eye 标定失败：{last_err}")
def create_homogeneous_matrix(rvec, tvec):
"""
根据旋转向量和平移向量创建4x4齐次变换矩阵。
参数:
rvec: 3x1旋转向量
tvec: 3x1平移向量
返回:
T: 4x4齐次变换矩阵
"""
R, _ = cv2.Rodrigues(rvec)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = tvec.flatten()
return T
def rot_to_quat(R: np.ndarray) -> np.ndarray:
"""将旋转矩阵转为四元数 [x, y, z, w]（右手，单位四元数）。"""
tr = np.trace(R)
if tr > 0.0:
S = np.sqrt(tr + 1.0) * 2.0
qw = 0.25 * S
qx = (R[2, 1] - R[1, 2]) / S
qy = (R[0, 2] - R[2, 0]) / S
qz = (R[1, 0] - R[0, 1]) / S
else:
if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
qx = 0.25 * S
qy = (R[0, 1] + R[1, 0]) / S
qz = (R[0, 2] + R[2, 0]) / S
qw = (R[2, 1] - R[1, 2]) / S
elif R[1, 1] > R[2, 2]:
S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
qx = (R[0, 1] + R[1, 0]) / S
qy = 0.25 * S
qz = (R[1, 2] + R[2, 1]) / S
qw = (R[0, 2] - R[2, 0]) / S
else:
S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
qx = (R[0, 2] + R[2, 0]) / S
qy = (R[1, 2] + R[2, 1]) / S
qz = 0.25 * S
qw = (R[1, 0] - R[0, 1]) / S
q = np.array([qx, qy, qz, qw])
return q / (np.linalg.norm(q) + 1e-12)
def quat_to_rot(q: np.ndarray) -> np.ndarray:
"""四元数 [x, y, z, w] 转旋转矩阵。"""
x, y, z, w = q
xx, yy, zz = x*x, y*y, z*z
xy, xz, yz = x*y, x*z, y*z
wx, wy, wz = w*x, w*y, w*z
R = np.array([
[1 - 2*(yy + zz), 2*(xy - wz), 2*(xz + wy)],
[2*(xy + wz), 1 - 2*(xx + zz), 2*(yz - wx)],
[2*(xz - wy), 2*(yz + wx), 1 - 2*(xx + yy)]
])
return R
def average_SE3(T_list: List[np.ndarray]) -> np.ndarray:
"""对一组 SE(3) 变换做简单平均（四元数+平移平均）。"""
if not T_list:
raise ValueError("T_list 为空")
# 平移均值
t_stack = np.stack([T[:3, 3] for T in T_list], axis=0)
t_mean = np.mean(t_stack, axis=0)
# 旋转均值（单位四元数平均后归一化）
q_list = [rot_to_quat(T[:3, :3]) for T in T_list]
# 对齐四元数符号到第一个四元数的半球，避免相互抵消
q0 = q_list[0]
aligned = []
for q in q_list:
if np.dot(q, q0) < 0:
aligned.append(-q)
else:
aligned.append(q)
q = np.mean(np.stack(aligned, axis=0), axis=0)
q = q / (np.linalg.norm(q) + 1e-12)
R_mean = quat_to_rot(q)
T = np.eye(4)
T[:3, :3] = R_mean
T[:3, 3] = t_mean
return T
def hand_eye_residual(robot_poses: List[np.ndarray], camera_poses: List[np.ndarray], X_cam_gripper: np.ndarray) -> Tuple[float, float]:
"""基于 AX ≈ X B 的关系计算残差（旋转角度均值，平移均值）。
返回 (rot_err_deg, trans_err_m)。"""
rot_errs = []
trans_errs = []
n = len(robot_poses)
# 构造相对运动对，避免偏置
for i in range(n - 1):
A = np.linalg.inv(robot_poses[i + 1]) @ robot_poses[i] # T_b^g_{i+1}^{-1} * T_b^g_i
B = np.linalg.inv(camera_poses[i + 1]) @ camera_poses[i] # T_c^t_{i+1}^{-1} * T_c^t_i
# OpenCV定义 X 为 T_c^g（camera->gripper）
lhs = np.linalg.inv(A) # OpenCV论文中常用 A 为 g2b，但这里用的相对位姿定义略有差异
# 直接用 AX 与 XB 的等价残差度量
AX = A @ np.linalg.inv(X_cam_gripper)
XB = np.linalg.inv(X_cam_gripper) @ B
Delta = np.linalg.inv(AX) @ XB
R = Delta[:3, :3]
t = Delta[:3, 3]
# 旋转角度
angle = np.rad2deg(np.arccos(np.clip((np.trace(R) - 1) / 2.0, -1.0, 1.0)))
rot_errs.append(abs(angle))
trans_errs.append(np.linalg.norm(t))
return float(np.mean(rot_errs)), float(np.mean(trans_errs))
# ==================== 示例用法 ====================
def generate_synthetic_dataset(num_poses: int = 12, seed: int = 42) -> Tuple[List[np.ndarray], List[np.ndarray]]:
"""生成可解的模拟数据集 (T_b^g 列表, T_c^t 列表)。"""
rng = np.random.default_rng(seed)
def hat(v):
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
def exp_so3(w):
th = np.linalg.norm(w)
if th < 1e-12:
return np.eye(3)
k = w / th
K = hat(k)
return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
def make_T(R, t):
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T
# 真实（未知）相机在夹爪中的位姿：T_c^g
R_c_g = exp_so3(np.deg2rad([10, -5, 15]))
t_c_g = np.array([0.03, -0.02, 0.15])
T_c_g_true = make_T(R_c_g, t_c_g)
# 世界（=基座）到标定板的固定位姿：T_b^t
R_b_t = exp_so3(np.deg2rad([0, 0, 0]))
t_b_t = np.array([0.4, 0.0, 0.3])
T_b_t = make_T(R_b_t, t_b_t)
robot_poses: List[np.ndarray] = [] # T_b^g
camera_poses: List[np.ndarray] = [] # T_c^t
for _ in range(num_poses):
# 生成多轴、较大幅度的运动（避免退化）
ang = np.deg2rad(rng.uniform([-30, -30, -30], [30, 30, 30]))
# 让 SO(3) 采样更丰富：
R_b_g = (np.eye(3) @ rot_perturb(ang))
t_b_g = rng.uniform([-0.2, -0.2, 0.3], [0.2, 0.2, 0.6])
T_b_g = make_T(R_b_g, t_b_g)
robot_poses.append(T_b_g)
# 相机位姿：T_b^c = T_b^g * T_g^c，其中 T_g^c = (T_c^g)^{-1}
T_g_c = np.linalg.inv(T_c_g_true)
T_b_c = T_b_g @ T_g_c
# 目标在相机坐标系：T_c^t = (T_b^c)^{-1} * T_b^t
T_c_t = np.linalg.inv(T_b_c) @ T_b_t
camera_poses.append(T_c_t)
return robot_poses, camera_poses
def rot_perturb(ang_vec: np.ndarray) -> np.ndarray:
"""给定欧拉角近似（rad）的向量，输出旋转矩阵（用罗德里格公式按轴角）。"""
axis = ang_vec
th = np.linalg.norm(axis)
if th < 1e-12:
return np.eye(3)
k = axis / th
K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
def to_json_SE3(T: np.ndarray) -> dict:
R = T[:3, :3]
t = T[:3, 3]
q = rot_to_quat(R)
return {
"matrix": T.tolist(),
"rotation_matrix": R.tolist(),
"translation": t.tolist(),
"quaternion_xyzw": q.tolist(),
}
def load_se3_matrix(path: str) -> np.ndarray:
"""加载 4x4 齐次矩阵。支持 .npy / .npz(json-like)/.json。"""
if not os.path.exists(path):
raise FileNotFoundError(path)
ext = os.path.splitext(path)[1].lower()
if ext == ".npy":
M = np.load(path)
elif ext == ".npz":
d = np.load(path)
key = 'matrix' if 'matrix' in d.files else d.files[0]
M = d[key]
elif ext == ".json":
with open(path, 'r') as f:
data = json.load(f)
M = np.array(data.get('matrix', data))
else:
raise ValueError(f"不支持的文件类型: {ext}")
M = np.array(M)
if M.shape != (4, 4):
raise ValueError(f"矩阵形状应为(4,4)，实际为{M.shape}")
return M
def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
"""加载相机内参，支持 OpenCV YAML/JSON（cameraMatrix, distCoeffs）或简单 JSON：{"K":[], "D":[]}。"""
import yaml
with open(path, 'r') as f:
txt = f.read()
data = None
try:
data = yaml.safe_load(txt)
except Exception:
try:
data = json.loads(txt)
except Exception:
raise ValueError("无法解析相机标定文件，支持 YAML/JSON，包含 camera_matrix/cameraMatrix/K 和 dist_coeffs/distCoeffs/D 字段")
# 支持多种键名
K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
if isinstance(K, dict) and 'data' in K:
K = K['data']
D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
if isinstance(D, dict) and 'data' in D:
D = D['data']
if K is None or D is None:
raise ValueError("标定文件需包含 camera_matrix/cameraMatrix/K 与 dist_coeffs/distCoeffs/D")
K = np.array(K, dtype=float).reshape(3, 3)
D = np.array(D, dtype=float).reshape(-1, 1)
return K, D
def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
"""生成棋盘格 3D 角点（单位：米），Z=0 平面，原点在棋盘左上角角点。"""
objp = np.zeros((rows * cols, 3), np.float32)
grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
objp[:, :2] = grid * square
return objp
def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
if not ok:
return False, None
# 亚像素优化
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
return True, corners.reshape(-1, 2)
def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float, dict_name: str = 'DICT_4X4_50') -> Tuple[bool, np.ndarray, np.ndarray]:
"""检测 Charuco 角点，返回 (ok, charuco_corners, charuco_ids)。需要 opencv-contrib-python。"""
if not hasattr(cv2, 'aruco'):
raise RuntimeError("需要 opencv-contrib-python 才能使用 Charuco。请安装 opencv-contrib-python==4.11.0.86")
aruco = cv2.aruco
dictionary = getattr(aruco, dict_name)
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, dict_name))
board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
params = aruco.DetectorParameters()
detector = aruco.ArucoDetector(aruco_dict, params)
corners, ids, _ = detector.detectMarkers(gray)
if ids is None or len(ids) == 0:
return False, None, None
retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
ok = retval is not None and retval >= 4
return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
def solve_pnp_from_image(image_path: str, K: np.ndarray, D: np.ndarray,
pattern: str, rows: int, cols: int, square: float,
charuco_marker: float = None) -> np.ndarray:
img = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
if img is None:
raise FileNotFoundError(f"无法读取图像: {image_path}")
if pattern == 'chessboard':
ok, corners = detect_chessboard(img, rows, cols)
if not ok:
raise RuntimeError(f"棋盘检测失败: {image_path}")
objp = build_chessboard_object_points(rows, cols, square)
# PnP
retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
if not retval:
raise RuntimeError(f"solvePnP 失败: {image_path}")
return create_homogeneous_matrix(rvec, tvec)
elif pattern == 'charuco':
if charuco_marker is None:
raise ValueError("使用 charuco 时必须提供 --charuco-marker（米）")
ok, charuco_corners, charuco_ids = detect_charuco(img, rows, cols, square, charuco_marker)
if not ok:
raise RuntimeError(f"Charuco 检测失败: {image_path}")
# 从 board 索引构建 3D 角点
aruco = cv2.aruco
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
obj_pts = []
img_pts = []
for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
obj_pts.append(board.getChessboardCorners()[idx][0])
img_pts.append(corner)
obj_pts = np.array(obj_pts, dtype=np.float32)
img_pts = np.array(img_pts, dtype=np.float32)
retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
if not retval:
raise RuntimeError(f"solvePnP 失败: {image_path}")
return create_homogeneous_matrix(rvec, tvec)
else:
raise ValueError("pattern 仅支持 chessboard 或 charuco")
def load_robot_poses_file(path: str) -> List[np.ndarray]:
"""从文件加载 robot_poses 列表。支持 .npz(robot_poses), .json(list or dict.matrix), .csv( image, tx,ty,tz,qx,qy,qz,qw )。"""
ext = os.path.splitext(path)[1].lower()
if ext == '.npz':
d = np.load(path)
arr = d['robot_poses']
assert arr.ndim == 3 and arr.shape[1:] == (4, 4)
return [arr[i] for i in range(arr.shape[0])]
if ext == '.json':
with open(path, 'r') as f:
data = json.load(f)
if isinstance(data, dict) and 'robot_poses' in data:
mats = data['robot_poses']
else:
mats = data
return [np.array(M).reshape(4, 4) for M in mats]
if ext == '.csv':
poses = []
with open(path, 'r') as f:
reader = csv.DictReader(f)
for row in reader:
tx, ty, tz = float(row['tx']), float(row['ty']), float(row['tz'])
qx, qy, qz, qw = float(row['qx']), float(row['qy']), float(row['qz']), float(row['qw'])
R = quat_to_rot(np.array([qx, qy, qz, qw]))
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = np.array([tx, ty, tz])
poses.append(T)
return poses
raise ValueError(f"不支持的 robot poses 文件类型: {ext}")
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Hand-Eye calibration (eye-in-hand / eye-to-hand)")
parser.add_argument("--data", type=str, default=None,
help=".npz dataset with arrays 'robot_poses' and 'camera_poses' of shape (N,4,4)")
parser.add_argument("--mode", type=str, default=None, choices=["eye_in_hand", "eye_to_hand"],
help="Calibration mode. If omitted, both modes are computed")
parser.add_argument("--out", type=str, default=None, help="Output JSON file for the calibration result")
parser.add_argument("--export-dataset", type=str, default=None,
help="Export the dataset used (robot_poses,camera_poses) to .npz at this path")
parser.add_argument("--seed", type=int, default=42, help="Synthetic dataset RNG seed when no --data is provided")
parser.add_argument("--num-poses", type=int, default=12, help="Synthetic dataset pose count when no --data is provided")
parser.add_argument("--eval", action="store_true", help="Compute simple AX≈XB residual metrics (eye-in-hand)")
# Image-based camera pose estimation
parser.add_argument("--images", type=str, default=None,
help="Directory or glob for images captured by the depth camera (color/IR). Sorted alphanumerically.")
parser.add_argument("--camera-calib", type=str, default=None, help="Camera intrinsic file (YAML/JSON)")
parser.add_argument("--pattern", type=str, default="chessboard", choices=["chessboard", "charuco"], help="Calibration board pattern")
parser.add_argument("--board-rows", type=int, default=6, help="Inner rows of chessboard/charuco")
parser.add_argument("--board-cols", type=int, default=9, help="Inner cols of chessboard/charuco")
parser.add_argument("--square-size", type=float, default=0.024, help="Square size in meters")
parser.add_argument("--charuco-marker", type=float, default=None, help="Charuco marker size in meters (for charuco only)")
parser.add_argument("--robot-poses-file", type=str, default=None, help="Path to robot poses file (.npz/.json/.csv)")
# Frames handling
parser.add_argument("--robot-poses-frame", type=str, default="robot_base", choices=["robot_base", "arm_base"],
help="Frame of robot_poses in the dataset: robot_base (default) or arm_base")
parser.add_argument("--arm-to-robot-base", type=str, default=None,
help="Optional path to 4x4 matrix of T_robot_base^arm_base (compose robot poses to robot base)")
args = parser.parse_args()
# Load or generate dataset
if args.data:
d = np.load(args.data)
robot_arr = d["robot_poses"]
cam_arr = d["camera_poses"]
assert robot_arr.ndim == 3 and robot_arr.shape[1:] == (4, 4)
assert cam_arr.ndim == 3 and cam_arr.shape[1:] == (4, 4)
robot_poses = [robot_arr[i] for i in range(robot_arr.shape[0])]
camera_poses = [cam_arr[i] for i in range(cam_arr.shape[0])]
print(f"已从 {args.data} 加载数据集，共 {len(robot_poses)} 组")
elif args.images and args.robot_poses_file and args.camera_calib:
# Build dataset from images + robot poses
# 1) images
if os.path.isdir(args.images):
img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
else:
img_paths = sorted(glob.glob(args.images))
if not img_paths:
raise FileNotFoundError(f"未找到图像: {args.images}")
# 2) robot poses
robot_poses = load_robot_poses_file(args.robot_poses_file)
if len(robot_poses) != len(img_paths):
raise ValueError(f"robot poses 数量({len(robot_poses)})与图像数量({len(img_paths)})不一致。请确保一一对应且顺序匹配。")
# 3) camera intrinsics
K, D = load_camera_calib(args.camera_calib)
# 4) detect and solvePnP
camera_poses = []
for img in img_paths:
T_c_t = solve_pnp_from_image(img, K, D, args.pattern, args.board_rows, args.board_cols, args.square_size, args.charuco_marker)
camera_poses.append(T_c_t)
print(f"已从图像与位姿构建数据集，共 {len(camera_poses)} 组")
else:
robot_poses, camera_poses = generate_synthetic_dataset(num_poses=args.num_poses, seed=args.seed)
print(f"生成了 {len(robot_poses)} 组模拟位姿数据 (seed={args.seed})")
# Optional export of dataset for reproducibility
if args.export_dataset:
np.savez(args.export_dataset,
robot_poses=np.stack(robot_poses, axis=0),
camera_poses=np.stack(camera_poses, axis=0))
print(f"已导出数据集到: {args.export_dataset}")
# Frame composition: if robot_poses are in arm_base, optionally compose to robot_base
reference_base = "robot_base"
if args.robot_poses_frame == "arm_base":
reference_base = "arm_base"
if args.arm_to_robot_base:
T_rb_ab = load_se3_matrix(args.arm_to_robot_base) # T_robot_base^arm_base
robot_poses = [T_rb_ab @ T_ab_g for T_ab_g in robot_poses]
reference_base = "robot_base"
print("已将 arm_base 框架下的末端位姿转换到 robot_base 框架")
def run_mode(mode_name: str) -> np.ndarray:
print(f"\n=== 标定模式: {mode_name} ===")
X = hand_eye_calibration(robot_poses, camera_poses, mode=mode_name)
print("标定结果 X:")
print(X)
if mode_name == 'eye_to_hand':
print(f"参考基座: {reference_base}")
print(f"平移部分: [{X[0, 3]:.4f}, {X[1, 3]:.4f}, {X[2, 3]:.4f}]")
return X
results = {}
try:
if args.mode:
X = run_mode(args.mode)
results[args.mode] = X
else:
X_eih = run_mode('eye_in_hand')
X_eth = run_mode('eye_to_hand')
results['eye_in_hand'] = X_eih
results['eye_to_hand'] = X_eth
if args.eval:
# 仅计算 eye-in-hand 残差（需要 X_cam_gripper），若未跑则先跑一次
if 'eye_in_hand' in results:
X_cam_gripper = results['eye_in_hand']
else:
X_cam_gripper = hand_eye_calibration(robot_poses, camera_poses, mode='eye_in_hand')
rot_err_deg, trans_err = hand_eye_residual(robot_poses, camera_poses, X_cam_gripper)
print(f"残差评估：旋转均值 {rot_err_deg:.3f} 度，平移均值 {trans_err:.4f} m")
if args.out:
# 若选择了特定模式，写该模式；否则写两者
payload = {"metadata": {"reference_base": reference_base,
"robot_poses_frame": args.robot_poses_frame,
"composed_with_arm_to_robot_base": bool(args.arm_to_robot_base)}}
if args.mode:
payload[args.mode] = to_json_SE3(results[args.mode])
else:
payload.update({k: to_json_SE3(v) for k, v in results.items()})
with open(args.out, 'w') as f:
json.dump(payload, f, indent=2)
print(f"已保存标定结果到: {args.out}")
except Exception as e:
print(f"标定过程中发生错误: {e}")
# ==================== 实际应用提示 ====================
if not args.data:
print("\n=== 实际应用说明 ===")
print("1. 从机器人控制器读取末端位姿 (通常是4x4齐次矩阵或位姿向量，注意这是 基座->末端 T_b^g)")
print("2. 使用 OpenCV solvePnP 从标定板图像解算 T_c^t (标定板到相机):")
print(" retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)")
print(" camera_pose = create_homogeneous_matrix(rvec, tvec) # 即 T_c^t")
print("3. 收集多组对应的 T_b^g 和 T_c^t（包含多轴较大旋转，>= 8-12 组）")
print("4. 运行本脚本 --data dataset.npz --mode eye_to_hand --out result.json --eval")
print("\n若 robot_poses 在 arm_base 框架下：")
print("- 如无 arm_base->robot_base 变换，脚本将输出 arm_base^T_camera（eye_to_hand）")
print("- 若提供 --arm-to-robot-base T_robot_base_from_arm_base.npy，将自动转换并输出 robot_base^T_camera")

224
src/scripts/hand_eye_calibration.md Normal file
View File

@@ -0,0 +1,224 @@
# 手眼标定（Eye-in-Hand / Eye-to-Hand）操作指南
本指南配合脚本 `src/scripts/hand_eye_cali.py` 使用，完成相机与机器人之间的外参（手眼）标定。脚本基于 OpenCV 的 `calibrateHandEye` 实现，支持“眼在手上 (eye-in-hand)”与“眼在手外 (eye-to-hand)”两种模式。
---
## 一、原理与坐标定义
- 机器人末端在基座坐标系下的位姿：`T_b^g`（4x4 齐次矩阵）
- 标定板在相机坐标系下的位姿：`T_c^t`（4x4 齐次矩阵），可由 `solvePnP` 得到
- 需求目标：
- 眼在手上 (eye-in-hand)：求 `T_c^g`（相机在末端坐标系下的位姿）
- 眼在手外 (eye-to-hand)：求 `T_b^c`（相机在基座坐标系下的位姿）
OpenCV `calibrateHandEye` 的输入为每一帧的“绝对位姿”：
- `R_gripper2base, t_gripper2base` 对应 `T_g^b`（由 `T_b^g` 求逆获得）
- `R_target2cam, t_target2cam` 对应 `T_t^c`（`T_c^t` 的旋转和平移部分）
---
## 二、前置条件
- 已有相机的内参（fx, fy, cx, cy, 畸变等），并能在图像中稳定检测到标定板角点或 AprilTag/ArUco 标记
- 能够从机器人系统读取末端在基座下的位姿 `T_b^g`
- 至少采集 8–12 组数据，包含多轴、较大幅度旋转和平移（信息量不足会导致求解失败或误差大）
---
## 三、数据采集
1. 固定标定板（推荐刚性固定在稳定位置），或在“眼在手上”场景中保持相机跟随末端运动
2. 对每一帧：
- 记录机器人末端位姿 `T_b^g`（4x4）
- 在对应图像中用 `solvePnP` 算法求解 `T_c^t`（4x4）
3. 采集方式一：直接保存数据集 `.npz`
- 数组名必须是 `robot_poses` 和 `camera_poses`
- 形状为 `(N, 4, 4)`
数据格式示例（Python 生成）：
```python
np.savez('dataset.npz',
robot_poses=np.stack(robot_pose_list, axis=0),
camera_poses=np.stack(camera_pose_list, axis=0))
```
4. 采集方式二：仅保存图像与机器人位姿
- 保存 N 张图像（彩色或红外），用于检测棋盘/Charuco 角点
- 同时保存 N 条机器人末端位姿 `T_b^g`（或 `T_ab^g`）到 `.csv/.json/.npz`
- 运行时脚本会：
1) 从相机内参文件（YAML/JSON）读入 `K,D`
2) 对每张图像用 `solvePnP` 求解 `T_c^t`
3) 组合成数据集并求解手眼外参
> 仅有机械臂基座（arm_base）的位姿可以吗？可以。
> - 如果记录的是 `T_ab^g`（arm_base->末端），也能完成手眼标定。
> - `eye_to_hand` 模式下，脚本将输出 `arm_base^T_camera`。若 `T_rb^ab`（robot_base->arm_base），可传给脚本自动合成 `robot_base^T_camera`。
## 四、脚本使用方法
脚本路径：`src/scripts/hand_eye_cali.py`
- 计算两种模式并导出 JSON：
```bash
python src/scripts/hand_eye_cali.py \
--data dataset.npz \
--out handeye_result.json \
--eval
```
- 仅计算眼在手外 (eye-to-hand)：
```bash
python src/scripts/hand_eye_cali.py \
--data dataset.npz \
--mode eye_to_hand \
--out handeye_result.json
```
- 没有数据时，可生成模拟数据用于测试：
```bash
python src/scripts/hand_eye_cali.py \
--num-poses 12 --eval \
--out /tmp/handeye_result.json \
--export-dataset /tmp/handeye_dataset.npz
```
### 仅用图像 + 末端位姿进行标定
前提：准备好相机标定文件（YAML/JSON，包含 `cameraMatrix`/`camera_matrix`/`K` 和 `distCoeffs`/`dist_coeffs`/`D`）。
- 棋盘格（默认 6x9 内角点，格长 0.024 m）：
```bash
python src/scripts/hand_eye_cali.py \
--images data/images/*.png \
--camera-calib data/camera.yaml \
--robot-poses-file data/robot_poses.csv \
--pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
--robot-poses-frame arm_base \
--mode eye_to_hand \
--out handeye_result.json --eval
```
- Charuco（需要 opencv-contrib-python）：
```bash
python src/scripts/hand_eye_cali.py \
--images data/images/*.png \
--camera-calib data/camera.yaml \
--robot-poses-file data/robot_poses.csv \
--pattern charuco --board-rows 6 --board-cols 9 --square-size 0.024 --charuco-marker 0.016 \
--robot-poses-frame robot_base \
--mode eye_to_hand \
--out handeye_result.json --eval
```
robot_poses.csv 示例（头部为列名，按行对齐图像顺序）：
```csv
image,tx,ty,tz,qx,qy,qz,qw
0001.png,0.10,0.00,0.45,0.00,0.00,0.00,1.00
0002.png,0.10,0.05,0.45,0.05,0.00,0.00,0.9987
...
```
相机 YAML 示例（可兼容 OpenCV 格式）：
```yaml
camera_matrix: {data: [fx, 0, cx, 0, fy, cy, 0, 0, 1]}
dist_coeffs: {data: [k1, k2, p1, p2, k3]}
```
### 参数说明
- `--data`: 读取 `.npz` 数据集（包含 `robot_poses`、`camera_poses`）
- `--mode`: `eye_in_hand` 或 `eye_to_hand`（缺省则两者都算）
- `--out`: 输出 JSON 路径（包含 4x4 矩阵、旋转矩阵、平移、四元数）
- `--eval`: 计算 AX≈XB 的简单残差（眼在手上），用于快速自检
- `--export-dataset`: 将当前使用的数据集导出（便于复现）
- `--num-poses`, `--seed`: 生成模拟数据的数量与种子（无 `--data` 时生效）
- `--robot-poses-frame`: `robot_base`（默认）或 `arm_base`，用于指明 `robot_poses` 的基座框架
- `--arm-to-robot-base`: `T_robot_base^arm_base` 的 4x4 矩阵文件路径（.npy/.npz/.json），若提供则在内部进行 `T_b^g = T_rb^ab @ T_ab^g` 组合
-. `--images`: 图像目录或通配（按文件名排序）
-. `--camera-calib`: 相机内参文件（YAML/JSON）
-. `--pattern`: `chessboard` 或 `charuco`
-. `--board-rows/--board-cols/--square-size`: 标定板参数（单位：米）
-. `--charuco-marker`: Charuco 的方块内 marker 尺寸（米）
-. `--robot-poses-file`: 末端位姿文件（.csv/.json/.npz），与图像一一对应
---
## 五、结果解释与落地
- 脚本输出 JSON 中字段：
- `matrix`: 4x4 齐次矩阵（行主序）
- `rotation_matrix`: 3x3 旋转矩阵
- `translation`: 3 维平移 `[x, y, z]`（单位：米）
- `quaternion_xyzw`: 四元数 `[x, y, z, w]`
- 眼在手外结果：
- 若 `--robot-poses-frame robot_base` 或提供了 `--arm-to-robot-base`，则输出为 `robot_base^T_camera`
- 若 `--robot-poses-frame arm_base` 且未提供 arm->robot 变换，则输出为 `arm_base^T_camera`
- 发布 TF 示例：
```bash
ros2 run tf2_ros static_transform_publisher \
x y z qx qy qz qw \
base_link camera_link
```
将 `x y z qx qy qz qw` 替换为 JSON 中的 `translation` 与 `quaternion_xyzw`，帧名根据机器人而定（如 `base_link` 与 `camera_link`）。
- 眼在手上（`camera^T_end_effector`）则在末端坐标系下描述相机位姿，常用于抓取/视觉伺服求解。
---
## 六、常见问题与排查
- 报错“Not enough informative motions”或残差很大：
- 增加数据数量（>=12）
- 扩大旋转幅度，覆盖多个轴，避免纯平移或单轴小角度
- 确认 `T_b^g` 与 `T_c^t` 的定义方向正确（基座->末端、标定板->相机）
- `solvePnP` 不稳定/跳变：
- 使用更鲁棒的标定板（AprilTag/Charuco）
- 固定/稳定曝光，提升角点检测质量
- 确认相机内参/畸变准确
- 结果帧名/方向不符合期望：
- 仔细对照：脚本的 `eye_to_hand` 输出的是 `base^T_camera`，`eye_in_hand` 输出的是 `camera^T_end_effector`
- 若需要 `end_effector^T_camera`，取逆即可
---
## 七、建议采集策略
- 首先让末端在 3 个轴上分别做正/反方向旋转，各配合一定平移
- 保证每一帧采集时 `T_b^g` 与 `T_c^t` 时间匹配（尽量同步）
- 目标板尽可能占据成像区域较大比例，避免深远距离下的姿态估计不稳定
---
## 八、附录：数据制作参考
使用 `solvePnP` 生成 `T_c^t`：
```python
# objectPoints: (N,3) mm 或 m；imagePoints: (N,2) 像素
retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
T_c_t = create_homogeneous_matrix(rvec, tvec)
```
保存数据集：
```python
np.savez('dataset.npz',
robot_poses=np.stack(robot_pose_list, axis=0),
camera_poses=np.stack(camera_pose_list, axis=0))
```
若 `robot_poses` 是以机械臂基座 `arm_base` 为基准，且已知 `T_robot_base^arm_base`，可以在运行时提供：
```bash
python src/scripts/hand_eye_cali.py \
--data dataset.npz \
--robot-poses-frame arm_base \
--arm-to-robot-base T_robot_base_from_arm_base.npy \
--mode eye_to_hand \
--out handeye_result.json
```
---
## 九、数据处理脚本
提供一个模板脚本，读取机器人驱动与视觉检测的实时话题，自动同步采样并生成 dataset.npz
将手眼标定结果直接写为 ROS2 的 static_transform_publisher 配置或 URDF 片段，便于一键加载

207
src/scripts/images_to_quaternions.py Normal file
View File

@@ -0,0 +1,207 @@
#!/usr/bin/env python3
"""
Detect calibration board in images, estimate camera pose via solvePnP,
and output quaternions (qx,qy,qz,qw) for up to N images.
Supports:
- chessboard or Charuco detection
- YAML/JSON camera intrinsics (OpenCV-style keys)
- Save to CSV and print to stdout
Example:
python src/scripts/images_to_quaternions.py \
--images data/images/*.png \
--camera-calib data/camera.yaml \
--pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
--limit 12 --out /tmp/quats.csv
"""
from __future__ import annotations
import argparse
import glob
import json
import os
from typing import Tuple, List
import cv2
import numpy as np
def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
import yaml
with open(path, 'r') as f:
txt = f.read()
try:
data = yaml.safe_load(txt)
except Exception:
data = json.loads(txt)
K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
if isinstance(K, dict) and 'data' in K:
K = K['data']
D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
if isinstance(D, dict) and 'data' in D:
D = D['data']
if K is None or D is None:
raise ValueError('Calibration must contain camera_matrix/cameraMatrix/K and dist_coeffs/distCoeffs/D')
K = np.array(K, dtype=float).reshape(3, 3)
D = np.array(D, dtype=float).reshape(-1, 1)
return K, D
def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
objp = np.zeros((rows * cols, 3), np.float32)
grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
objp[:, :2] = grid * square
return objp
def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
if not ok:
return False, None
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
return True, corners.reshape(-1, 2)
def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float) -> Tuple[bool, np.ndarray, np.ndarray]:
if not hasattr(cv2, 'aruco'):
raise RuntimeError('opencv-contrib-python is required for Charuco detection')
aruco = cv2.aruco
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
params = aruco.DetectorParameters()
detector = aruco.ArucoDetector(aruco_dict, params)
corners, ids, _ = detector.detectMarkers(gray)
if ids is None or len(ids) == 0:
return False, None, None
retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
ok = retval is not None and retval >= 4
return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
def rot_to_quat(R: np.ndarray) -> np.ndarray:
tr = np.trace(R)
if tr > 0.0:
S = np.sqrt(tr + 1.0) * 2.0
qw = 0.25 * S
qx = (R[2, 1] - R[1, 2]) / S
qy = (R[0, 2] - R[2, 0]) / S
qz = (R[1, 0] - R[0, 1]) / S
else:
if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
qx = 0.25 * S
qy = (R[0, 1] + R[1, 0]) / S
qz = (R[0, 2] + R[2, 0]) / S
qw = (R[2, 1] - R[1, 2]) / S
elif R[1, 1] > R[2, 2]:
S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
qx = (R[0, 1] + R[1, 0]) / S
qy = 0.25 * S
qz = (R[1, 2] + R[2, 1]) / S
qw = (R[0, 2] - R[2, 0]) / S
else:
S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
qx = (R[0, 2] + R[2, 0]) / S
qy = (R[1, 2] + R[2, 1]) / S
qz = 0.25 * S
qw = (R[1, 0] - R[0, 1]) / S
q = np.array([qx, qy, qz, qw])
return q / (np.linalg.norm(q) + 1e-12)
def pnp_quaternion_for_image(image_path: str, K: np.ndarray, D: np.ndarray,
pattern: str, rows: int, cols: int, square: float,
charuco_marker: float | None = None) -> Tuple[np.ndarray, np.ndarray]:
gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
if gray is None:
raise FileNotFoundError(f'Cannot read image: {image_path}')
if pattern == 'chessboard':
ok, corners = detect_chessboard(gray, rows, cols)
if not ok:
raise RuntimeError(f'Chessboard detection failed: {image_path}')
objp = build_chessboard_object_points(rows, cols, square)
retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
if not retval:
raise RuntimeError(f'solvePnP failed: {image_path}')
elif pattern == 'charuco':
if charuco_marker is None:
raise ValueError('charuco requires --charuco-marker')
ok, charuco_corners, charuco_ids = detect_charuco(gray, rows, cols, square, charuco_marker)
if not ok:
raise RuntimeError(f'Charuco detection failed: {image_path}')
aruco = cv2.aruco
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
obj_pts = []
img_pts = []
for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
obj_pts.append(board.getChessboardCorners()[idx][0])
img_pts.append(corner)
obj_pts = np.array(obj_pts, dtype=np.float32)
img_pts = np.array(img_pts, dtype=np.float32)
retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
if not retval:
raise RuntimeError(f'solvePnP failed: {image_path}')
else:
raise ValueError('pattern must be chessboard or charuco')
R, _ = cv2.Rodrigues(rvec)
q = rot_to_quat(R)
return q, tvec.flatten()
def main():
parser = argparse.ArgumentParser(description='Estimate quaternions from board detections in images (solvePnP).')
parser.add_argument('--images', type=str, required=True, help='Directory or glob for images')
parser.add_argument('--camera-calib', type=str, required=True, help='Camera intrinsics (YAML/JSON)')
parser.add_argument('--pattern', type=str, default='chessboard', choices=['chessboard', 'charuco'])
parser.add_argument('--board-rows', type=int, default=6)
parser.add_argument('--board-cols', type=int, default=9)
parser.add_argument('--square-size', type=float, default=0.024, help='meters')
parser.add_argument('--charuco-marker', type=float, default=None, help='meters for Charuco marker size')
parser.add_argument('--limit', type=int, default=12, help='Max number of images to process')
parser.add_argument('--out', type=str, default=None, help='Output CSV path')
args = parser.parse_args()
# Collect images
if os.path.isdir(args.images):
img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
else:
img_paths = sorted(glob.glob(args.images))
if not img_paths:
raise FileNotFoundError(f'No images found: {args.images}')
img_paths = img_paths[: args.limit]
# Load intrinsics
K, D = load_camera_calib(args.camera_calib)
# Process
results: List[Tuple[str, np.ndarray]] = []
for p in img_paths:
try:
q, t = pnp_quaternion_for_image(
p, K, D,
args.pattern, args.board_rows, args.board_cols, args.square_size,
args.charuco_marker,
)
results.append((p, q))
print(f'{os.path.basename(p)}: qx={q[0]:.8f}, qy={q[1]:.8f}, qz={q[2]:.8f}, qw={q[3]:.8f}')
except Exception as e:
print(f'WARN: {p}: {e}')
if args.out:
import csv
with open(args.out, 'w', newline='') as f:
writer = csv.writer(f)
writer.writerow(['image', 'qx', 'qy', 'qz', 'qw'])
for p, q in results:
writer.writerow([os.path.basename(p), f'{q[0]:.8f}', f'{q[1]:.8f}', f'{q[2]:.8f}', f'{q[3]:.8f}'])
print(f'Written CSV: {args.out}')
if __name__ == '__main__':
main()

115
src/scripts/quaternion_conversion.md Normal file
View File

@@ -0,0 +1,115 @@
# 姿态到四元数转换说明
本文档说明如何将姿态表示（欧拉角与旋转向量 Rodrigues/Axis-Angle）转换为四元数 (qx, qy, qz, qw)，包含原理、公式、边界情况与一个完整的计算示例，并给出现有脚本 `src/scripts/euler_to_quaternion.py` 的使用方法。
## 基本约定
- 四元数记为 q = [x, y, z, w]，其中 w 为实部，(x, y, z) 为虚部（与 ROS、OpenCV 常见约定一致）。
- 旋转向量 r = (rx, ry, rz) 的方向为旋转轴，模长 |r| 为旋转角（弧度）。
- 欧拉角使用“内在旋转”约定，支持顺序 xyz（默认）与 zyx。
---
## 1) 旋转向量 (Rodrigues) → 四元数
给定旋转向量 r = (rx, ry, rz)：
1. 计算旋转角与单位旋转轴：
- 角度：θ = |r| = sqrt(rx^2 + ry^2 + rz^2)
- 轴：u = r / θ = (ux, uy, uz)
2. 由轴角到四元数：
- s = sin(θ/2), c = cos(θ/2)
- q = (ux·s, uy·s, uz·s, c)
3. 数值稳定性：若 θ 非常小（例如 < 1e-12），可近似认为 q ≈ (0, 0, 0, 1)。
4. 归一化：实际实现中会做一次单位化 q ← q / ||q||，以避免数值误差。
> 注：四元数 q 与 -q 表示同一旋转（双覆表示）。
### 示例（完整计算过程）
已知 r = (-1.433, 0.114, -0.430)（单位：rad）：
- θ = |r| ≈ 1.5004615956
- u = r / θ ≈ (-0.955039439, 0.075976620, -0.286578478)
- s = sin(θ/2) ≈ 0.6818076141
- c = cos(θ/2) ≈ 0.7315315286
- q = (ux·s, uy·s, uz·s, c)
≈ (-0.6511531610, 0.0518014378, -0.1953913882, 0.7315315286)
因此，对应四元数为：
- qx,qy,qz,qw ≈ -0.65115316, 0.05180144, -0.19539139, 0.73153153
---
## 2) 欧拉角 → 四元数
对于欧拉角 (rx, ry, rz)，若输入单位为弧度：
- 以 xyz 顺序为例（依次绕自身 x、y、z 轴）：
1. 构造三个轴角四元数：
- qx = (sin(rx/2), 0, 0, cos(rx/2))
- qy = (0, sin(ry/2), 0, cos(ry/2))
- qz = (0, 0, sin(rz/2), cos(rz/2))
2. 乘法顺序为 q = qx ⊗ qy ⊗ qz（内在旋转，右乘积）。
- 对于 zyx 顺序：q = qz ⊗ qy ⊗ qx。
- 最后同样进行单位化。
> 提示：不同库/软件可能有“外在 vs 内在”、“左乘 vs 右乘”、“轴顺序”等差异，需确保与使用方约定一致。
---
## 3) 边界与常见问题
- 零角或极小角：直接返回单位四元数 (0,0,0,1) 或采用泰勒展开近似。
- 符号一致性：q 与 -q 表示同一旋转，批量处理时常将 w 约束为非负以保证连续性（可选）。
- 单位：所有角度必须使用弧度；若源数据是度，请先转弧度（乘以 π/180）。
- 数值稳定：建议在输出前做单位化，避免浮点累积误差。
---
## 4) 使用脚本批量/单次转换
脚本路径：`src/scripts/euler_to_quaternion.py`
- 单次旋转向量 → 四元数：
```bash
python src/scripts/euler_to_quaternion.py --mode rotvec --single RX RY RZ
# 例如：
python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
```
- CSV 批量（默认欧拉角，若为旋转向量请加 --mode rotvec）：
```bash
python src/scripts/euler_to_quaternion.py \
--in input.csv --out output.csv \
--rx rx --ry ry --rz rz \
--mode rotvec
```
- 欧拉角（内在旋转顺序 xyz 或 zyx）：
```bash
python src/scripts/euler_to_quaternion.py \
--mode euler --order xyz --single RX RY RZ
```
> 如果输入 CSV 同时包含位姿中的位置列（x,y,z），脚本支持可选的单位转换 `--mm-to-m`（将毫米换算为米，并额外输出 x_m,y_m,z_m 列）。
---
## 5) 本次 12 组旋转向量的结果
以下 12 组 (rx, ry, rz)（单位：rad）对应的四元数 (qx, qy, qz, qw)：
1. -0.65115316, 0.05180144, -0.19539139, 0.73153153
2. -0.71991924, -0.00710155, -0.17753865, 0.67092912
3. -0.44521470, -0.25512818, -0.41269388, 0.75258039
4. -0.72304324, 0.09631938, -0.32264833, 0.60318248
5. -0.67311368, 0.06894426, -0.11793097, 0.72681287
6. -0.73524204, -0.19261515, -0.19652833, 0.61943132
7. -0.75500427, -0.08296268, -0.09788718, 0.64304265
8. -0.88627353, -0.08089799, -0.15658968, 0.42831579
9. -0.62408775, -0.13051614, -0.11718879, 0.76141106
10. -0.67818166, -0.10516535, -0.18696062, 0.70289090
11. -0.77275040, -0.19297175, -0.05741665, 0.60193194
12. -0.66493346, 0.09013744, -0.16351565, 0.72318833
---
## 6) 参考实现
脚本 `euler_to_quaternion.py` 中的核心函数：
- `rotvec_to_quaternion(rx, ry, rz)`：实现了第 1 节所述的 Rodrigues → 四元数转换，并在小角度与归一化方面做了稳健处理。
- `euler_to_quaternion(rx, ry, rz, order, degrees)`：实现了第 2 节所述的欧拉角 → 四元数转换，支持 xyz/zyx 两种顺序与度/弧度输入。
如需将结果保存为 CSV 或用于后续手眼标定、TF 发布等，可直接复用该脚本的命令行接口。
---

3
src/scripts/requirements.txt Normal file
View File

@@ -0,0 +1,3 @@
# Pinned to avoid NumPy 2.x ABI issues with some wheels
numpy<2
opencv-python==4.11.0.86