# HandEyeCalib 手眼标定模块

## 概述

HandEyeCalib 是一个基于 Eigen 库实现的手眼标定模块，支持 Eye-To-Hand（眼在手外）和 Eye-In-Hand（眼在手上）两种标定模式。

## 依赖

- Eigen 3.3.9+（仅头文件）
- C++11 或更高版本

## 两种标定模式

### Eye-To-Hand（眼在手外）

相机固定安装在外部（如支架上），不随机器人运动。

```
┌─────────┐
│  相机   │ ← 固定位置
└────┬────┘
     │ 观测
     ▼
┌─────────┐      ┌─────────┐
│ 标定点  │ ←──→ │ 机器人  │
└─────────┘      └─────────┘
```

**求解目标**：相机坐标系到机器人基座坐标系的变换矩阵

**数学模型**：
```
P_robot = R * P_camera + T
```

### Eye-In-Hand（眼在手上）

相机安装在机器人末端，随机器人运动。

```
┌─────────┐
│ 机器人  │
└────┬────┘
     │
┌────┴────┐
│  末端   │
├─────────┤
│  相机   │ ← 随末端运动
└────┬────┘
     │ 观测
     ▼
┌─────────┐
│ 标定点  │ ← 固定位置
└─────────┘
```

**求解目标**：相机坐标系到末端坐标系的变换矩阵

**数学模型**（AX=XB 问题）：
```
T_end * T_cam * P_camera = P_base
```

## 算法原理

### SVD 分解法（Eye-To-Hand）

1. 计算两组点的质心
2. 去中心化处理
3. 构建协方差矩阵 W = Σ(q1 * q2^T)
4. SVD 分解：W = U * S * V^T
5. 计算旋转矩阵：R = V * M * U^T
6. 计算平移向量：T = p2 - R * p1

### Tsai-Lenz 方法（Eye-In-Hand）

1. 构建相邻位姿间的相对变换 A = T_end_i^(-1) * T_end_j
2. 构建超定方程组
3. SVD 求解旋转矩阵
4. 最小二乘求解平移向量

## 欧拉角旋转顺序

支持6种常用旋转顺序：

| 枚举值 | 旋转顺序 | 说明 |
|--------|----------|------|
| `HECEulerOrder::XYZ` | X→Y→Z | Roll-Pitch-Yaw |
| `HECEulerOrder::XZY` | X→Z→Y | |
| `HECEulerOrder::YXZ` | Y→X→Z | |
| `HECEulerOrder::YZX` | Y→Z→X | |
| `HECEulerOrder::ZXY` | Z→X→Y | |
| `HECEulerOrder::ZYX` | Z→Y→X | Yaw-Pitch-Roll，机器人常用 |

## API 参考

### 创建/销毁实例

```cpp
// 创建实例
IHandEyeCalib* calib = CreateHandEyeCalibInstance();

// 使用完毕后销毁
DestroyHandEyeCalibInstance(calib);
```

### Eye-To-Hand 标定

```cpp
int CalculateRT(
    const std::vector<HECPoint3D>& eyePoints,    // 相机坐标系下的点
    const std::vector<HECPoint3D>& robotPoints,  // 机器人坐标系下的点
    HECCalibResult& result);                     // 输出结果
```

### Eye-In-Hand 标定

```cpp
// 标定点位置未知
int CalculateEyeInHand(
    const std::vector<HECEyeInHandData>& calibData,  // 标定数据
    HECCalibResult& result);                         // 输出结果

// 标定点位置已知
int CalculateEyeInHandWithTarget(
    const std::vector<HECEyeInHandData>& calibData,  // 标定数据
    const HECPoint3D& targetInBase,                  // 标定点在基座下的坐标
    HECCalibResult& result);                         // 输出结果
```

### 坐标变换

```cpp
// 完整变换: dst = R * src + T
void TransformPoint(
    const HECRotationMatrix& R,
    const HECTranslationVector& T,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint);

// 仅旋转: dst = R * src
void RotatePoint(
    const HECRotationMatrix& R,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint);
```

### 欧拉角转换

```cpp
// 旋转矩阵 → 欧拉角
void RotationMatrixToEuler(
    const HECRotationMatrix& R,
    HECEulerOrder order,
    HECEulerAngles& angles);

// 欧拉角 → 旋转矩阵
void EulerToRotationMatrix(
    const HECEulerAngles& angles,
    HECEulerOrder order,
    HECRotationMatrix& R);
```

## 使用示例

### 示例1：Eye-To-Hand 标定

```cpp
#include "IHandEyeCalib.h"

void EyeToHandCalibration()
{
    // 创建标定实例
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();

    // 准备标定数据（至少3组对应点）
    std::vector<HECPoint3D> cameraPoints;  // 相机坐标系下的点
    std::vector<HECPoint3D> robotPoints;   // 机器人坐标系下的点

    // 添加标定点（示例数据）
    cameraPoints.push_back(HECPoint3D(100.0, 50.0, 200.0));
    robotPoints.push_back(HECPoint3D(500.0, 300.0, 100.0));

    cameraPoints.push_back(HECPoint3D(150.0, 80.0, 210.0));
    robotPoints.push_back(HECPoint3D(550.0, 330.0, 110.0));

    cameraPoints.push_back(HECPoint3D(120.0, 60.0, 195.0));
    robotPoints.push_back(HECPoint3D(520.0, 310.0, 95.0));

    // 更多点...（建议10组以上）

    // 执行标定
    HECCalibResult result;
    int ret = calib->CalculateRT(cameraPoints, robotPoints, result);

    if (ret == 0) {
        // 标定成功，输出结果
        printf("标定误差: %.3f mm\n", result.error);

        // 使用标定结果进行坐标转换
        HECPoint3D cameraPoint(130.0, 70.0, 205.0);
        HECPoint3D robotPoint;
        calib->TransformPoint(result.R, result.T, cameraPoint, robotPoint);

        printf("相机点 (%.2f, %.2f, %.2f) -> 机器人点 (%.2f, %.2f, %.2f)\n",
               cameraPoint.x, cameraPoint.y, cameraPoint.z,
               robotPoint.x, robotPoint.y, robotPoint.z);
    }

    // 销毁实例
    DestroyHandEyeCalibInstance(calib);
}
```

### 示例2：Eye-In-Hand 标定（标定点位置未知）

```cpp
#include "IHandEyeCalib.h"

void EyeInHandCalibration()
{
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();

    // 准备标定数据
    std::vector<HECEyeInHandData> calibData;

    // 在不同位姿下采集数据
    for (int i = 0; i < 10; i++) {
        HECEyeInHandData data;

        // 获取当前末端位姿（从机器人控制器读取）
        // data.endPose = getRobotEndPose();

        // 示例：构建末端位姿矩阵
        HECRotationMatrix R_end;
        HECTranslationVector T_end(500.0 + i * 10, 200.0, 300.0 - i * 5);
        data.endPose = HECHomogeneousMatrix(R_end, T_end);

        // 获取相机观测到的标定点坐标
        // data.targetInCamera = detectTargetInCamera();
        data.targetInCamera = HECPoint3D(100.0 - i * 2, 50.0 + i, 200.0);

        calibData.push_back(data);
    }

    // 执行标定
    HECCalibResult result;
    int ret = calib->CalculateEyeInHand(calibData, result);

    if (ret == 0) {
        printf("Eye-In-Hand 标定成功\n");
        printf("标定误差: %.3f mm\n", result.error);

        // 输出相机到末端的变换矩阵
        printf("旋转矩阵 R:\n");
        for (int i = 0; i < 3; i++) {
            printf("  [%.6f, %.6f, %.6f]\n",
                   result.R.at(i, 0), result.R.at(i, 1), result.R.at(i, 2));
        }
        printf("平移向量 T: [%.3f, %.3f, %.3f]\n",
               result.T.at(0), result.T.at(1), result.T.at(2));
    }

    DestroyHandEyeCalibInstance(calib);
}
```

### 示例3：Eye-In-Hand 标定（标定点位置已知）

```cpp
#include "IHandEyeCalib.h"

void EyeInHandCalibrationWithTarget()
{
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();

    std::vector<HECEyeInHandData> calibData;

    // 采集多组数据（同示例2）
    // ...

    // 标定点在基座坐标系下的已知位置
    HECPoint3D targetInBase(600.0, 400.0, 50.0);

    // 执行标定
    HECCalibResult result;
    int ret = calib->CalculateEyeInHandWithTarget(calibData, targetInBase, result);

    if (ret == 0) {
        printf("标定成功，误差: %.3f mm\n", result.error);
    }

    DestroyHandEyeCalibInstance(calib);
}
```

### 示例4：欧拉角转换

```cpp
#include "IHandEyeCalib.h"

void EulerAngleConversion()
{
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();

    // 从欧拉角创建旋转矩阵
    HECEulerAngles angles;
    angles.roll = 0.1;   // 弧度
    angles.pitch = 0.2;
    angles.yaw = 0.3;

    HECRotationMatrix R;

    // 使用 ZYX 顺序（机器人常用）
    calib->EulerToRotationMatrix(angles, HECEulerOrder::ZYX, R);

    // 使用 XYZ 顺序
    calib->EulerToRotationMatrix(angles, HECEulerOrder::XYZ, R);

    // 从旋转矩阵提取欧拉角
    HECEulerAngles extractedAngles;
    calib->RotationMatrixToEuler(R, HECEulerOrder::ZYX, extractedAngles);

    // 角度制转换
    double roll_deg, pitch_deg, yaw_deg;
    extractedAngles.toDegrees(roll_deg, pitch_deg, yaw_deg);
    printf("欧拉角 (度): Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
           roll_deg, pitch_deg, yaw_deg);

    DestroyHandEyeCalibInstance(calib);
}
```

### 示例5：完整的位姿转换

```cpp
#include "IHandEyeCalib.h"

void PoseTransformation()
{
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();

    // 假设已完成标定
    HECCalibResult calibResult;
    // ... 标定过程 ...

    // 相机检测到的目标位置
    HECPoint3D eyePoint(100.0, 50.0, 200.0);

    // 相机检测到的目标姿态（X/Y/Z轴方向向量）
    std::vector<HECPoint3D> eyeDirVectors;
    eyeDirVectors.push_back(HECPoint3D(1.0, 0.0, 0.0));  // X轴
    eyeDirVectors.push_back(HECPoint3D(0.0, 1.0, 0.0));  // Y轴
    eyeDirVectors.push_back(HECPoint3D(0.0, 0.0, 1.0));  // Z轴

    // 转换到机器人坐标系
    HECPoseResult poseResult;
    calib->TransformPose(calibResult, eyePoint, eyeDirVectors, false, poseResult);

    // 输出机器人位姿
    printf("机器人位置: X=%.2f, Y=%.2f, Z=%.2f\n",
           poseResult.position.x, poseResult.position.y, poseResult.position.z);

    double roll_deg, pitch_deg, yaw_deg;
    poseResult.angles.toDegrees(roll_deg, pitch_deg, yaw_deg);
    printf("机器人姿态: Roll=%.2f°, Pitch=%.2f°, Yaw=%.2f°\n",
           roll_deg, pitch_deg, yaw_deg);

    DestroyHandEyeCalibInstance(calib);
}
```

## 标定建议

1. **采集点数**：建议采集 10-20 组标定点
2. **点分布**：标定点应均匀分布在工作空间内
3. **避免共面**：标定点不应全部位于同一平面
4. **精度要求**：机器人示教精度和相机检测精度直接影响标定结果
5. **验证**：标定完成后，使用新的测试点验证标定精度

## 错误码

| 错误码 | 说明 |
|--------|------|
| 0 (SUCCESS) | 成功 |
| APP_ERR_PARAM | 参数错误（点数不足或不匹配） |

## 文件结构

```
Module/HandEyeCalib/
├── Inc/
│   ├── HandEyeCalib_global.h    # 导出宏定义
│   ├── HandEyeCalibTypes.h      # 数据类型定义
│   └── IHandEyeCalib.h          # 接口定义
├── _Inc/
│   └── HandEyeCalib.h           # 实现类声明
├── Src/
│   └── HandEyeCalib.cpp         # 实现
├── Doc/
│   └── HandEyeCalib.md          # 本文档
└── HandEyeCalib.pro             # Qt 工程文件
```