From e342eebb1c7ad6f50a8cf515c38f519c06921c79 Mon Sep 17 00:00:00 2001 From: david Date: Thu, 16 Oct 2025 19:14:29 +0800 Subject: [PATCH] seperate left/right arm control --- .gitignore | 1 + src/brain/include/brain/skill_manager.hpp | 7 +- src/brain/src/cerebellum_node.cpp | 71 ++- .../src/vision_object_recg_node.cpp | 27 +- src/scripts/euler_to_equa.py | 46 ++ src/scripts/euler_to_quaternion.py | 207 ++++++ src/scripts/hand_eye_cali.py | 588 ++++++++++++++++++ src/scripts/hand_eye_calibration.md | 224 +++++++ src/scripts/images_to_quaternions.py | 207 ++++++ src/scripts/quaternion_conversion.md | 115 ++++ src/scripts/requirements.txt | 3 + 11 files changed, 1480 insertions(+), 16 deletions(-) create mode 100644 src/scripts/euler_to_equa.py create mode 100644 src/scripts/euler_to_quaternion.py create mode 100644 src/scripts/hand_eye_cali.py create mode 100644 src/scripts/hand_eye_calibration.md create mode 100644 src/scripts/images_to_quaternions.py create mode 100644 src/scripts/quaternion_conversion.md create mode 100644 src/scripts/requirements.txt diff --git a/.gitignore b/.gitignore index 2eb2090..0f94f3c 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ install/ log/ .vscode/ +.venv/ \ No newline at end of file diff --git a/src/brain/include/brain/skill_manager.hpp b/src/brain/include/brain/skill_manager.hpp index 9bf0c43..c27dbe6 100644 --- a/src/brain/include/brain/skill_manager.hpp +++ b/src/brain/include/brain/skill_manager.hpp @@ -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; @@ -272,7 +273,7 @@ private: internal_skill, std::function{}, std::function)>{}, + const std::shared_ptr)>{}, std::function {}, std::function &)> {}); } @@ -283,7 +284,7 @@ private: const std::string & internal_skill, const std::function & make_goal_override, const std::function::SharedPtr, - const std::shared_ptr)> & on_feedback_override, + const std::shared_ptr)> & on_feedback_override, const std::function::WrappedResult &)> & on_result_override, const std::function> &)> & on_goal_response_override) { @@ -344,7 +345,7 @@ template<> struct SkillActionTrait { 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; diff --git a/src/brain/src/cerebellum_node.cpp b/src/brain/src/cerebellum_node.cpp index af5b3ac..c9e59dd 100644 --- a/src/brain/src/cerebellum_node.cpp +++ b/src/brain/src/cerebellum_node.cpp @@ -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::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::duration(GetTimeoutForSkill(skill))); + if (skill == "Arm") { + RCLCPP_INFO(this->get_logger(), "[Arm] 并发发送两个 goal: body_id=1 与 body_id=0"); + + std::atomic 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(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((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(skills.size())); bool ok = false; std::string detail; auto it = skill_manager_->skills().find(skill); if (it == skill_manager_->skills().end()) { diff --git a/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp b/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp index 43bb2e3..9e94615 100644 --- a/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp +++ b/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp @@ -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); diff --git a/src/scripts/euler_to_equa.py b/src/scripts/euler_to_equa.py new file mode 100644 index 0000000..6988255 --- /dev/null +++ b/src/scripts/euler_to_equa.py @@ -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}]") \ No newline at end of file diff --git a/src/scripts/euler_to_quaternion.py b/src/scripts/euler_to_quaternion.py new file mode 100644 index 0000000..549b979 --- /dev/null +++ b/src/scripts/euler_to_quaternion.py @@ -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() diff --git a/src/scripts/hand_eye_cali.py b/src/scripts/hand_eye_cali.py new file mode 100644 index 0000000..9942694 --- /dev/null +++ b/src/scripts/hand_eye_cali.py @@ -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") \ No newline at end of file diff --git a/src/scripts/hand_eye_calibration.md b/src/scripts/hand_eye_calibration.md new file mode 100644 index 0000000..15eb319 --- /dev/null +++ b/src/scripts/hand_eye_calibration.md @@ -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 片段，便于一键加载 \ No newline at end of file diff --git a/src/scripts/images_to_quaternions.py b/src/scripts/images_to_quaternions.py new file mode 100644 index 0000000..17f6bd2 --- /dev/null +++ b/src/scripts/images_to_quaternions.py @@ -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() diff --git a/src/scripts/quaternion_conversion.md b/src/scripts/quaternion_conversion.md new file mode 100644 index 0000000..fc4a5f5 --- /dev/null +++ b/src/scripts/quaternion_conversion.md @@ -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 发布等，可直接复用该脚本的命令行接口。 + +--- diff --git a/src/scripts/requirements.txt b/src/scripts/requirements.txt new file mode 100644 index 0000000..d05e307 --- /dev/null +++ b/src/scripts/requirements.txt @@ -0,0 +1,3 @@ +# Pinned to avoid NumPy 2.x ABI issues with some wheels +numpy<2 +opencv-python==4.11.0.86