# HandEyeCalib

`HandEyeCalib` 提供手眼标定、TCP 标定，以及基于标定结果的点位/姿态转换能力。

当前模块由两部分组成：

- `IHandEyeCalib`
  面向应用和工具的标定算法接口。
- `Utils/CoordinateTransform`
  面向业务侧的坐标变换工具函数。

## 当前保留的公开能力

### IHandEyeCalib

```cpp
virtual int CalculateRT(
    const std::vector<HECPoint3D>& eyePoints,
    const std::vector<HECPoint3D>& robotPoints,
    HECCalibResult& result) = 0;

virtual int CalculateRT(
    const std::vector<HECCalibPointPair>& pointPairs,
    HECCalibResult& result) = 0;

virtual void TransformPoint(
    const HECRotationMatrix& R,
    const HECTranslationVector& T,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint) = 0;

virtual void RotatePoint(
    const HECRotationMatrix& R,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint) = 0;

virtual void RotationMatrixToEuler(
    const HECRotationMatrix& R,
    HECEulerOrder order,
    HECEulerAngles& angles) = 0;

virtual void EulerToRotationMatrix(
    const HECEulerAngles& angles,
    HECEulerOrder order,
    HECRotationMatrix& R) = 0;

virtual bool TransformPose(
    const HECCalibResult& calibResult,
    const HECPoint3D& eyeCenter,
    const HECPoint3D& longAxisDir,
    const HECPoint3D& normalDir,
    int dirVectorInvert,
    HECEulerOrder eulerOrder,
    HECLongAxisDir longAxisMapping,
    HECPoseResult& poseResult) = 0;

virtual double CalculateError(
    const std::vector<HECPoint3D>& eyePoints,
    const std::vector<HECPoint3D>& robotPoints,
    const HECCalibResult& calibResult) = 0;

virtual int CalculateEyeInHand(
    const std::vector<HECEyeInHandData>& calibData,
    HECCalibResult& result) = 0;

virtual HECTCPCalibResult CalculateTCP(
    const HECTCPCalibData& data) = 0;

virtual int CalculateEyeToHandWithPose(
    const std::vector<HECEyeToHandData>& calibData,
    HECCalibResult& result) = 0;
```

### CoordinateTransform

主要保留两类场景：

- `threeAxis*`
  三轴设备坐标转换
- `sixAxis*`
  六轴设备坐标转换

其中六轴眼在手上转换的核心接口为：

```cpp
CTHomogeneousMatrix sixAxisEyeInHandBuildTransform(
    const CTRobotPose& robotPose,
    CTEulerOrder eulerOrder,
    const CTHomogeneousMatrix& handEyeMatrix);
```

**约定**：`robotPose.rx / ry / rz` 必须已经是真实绕 X/Y/Z 轴的角（弧度）。
若机器人报文的字段排列不是 `RX, RY, RZ`（例如配天 `A=RZ, B=RY, C=RX`），调用方需要先把字段按真实轴含义装进 `CTRobotPose`，再调用本函数。

## 已移除的旧接口

以下接口已经从公开 API 中移除，不建议继续使用：

- `TransformPointWithCenter`
- `RotationMatrixToEulerZYX`
- `EulerZYXToRotationMatrix`
- 旧版 `TransformPose(calibResult, eyePoint, eyeDirVectors, invertYZ, ...)`
- `CalculateEyeInHandWithTarget`

原因：

- 仓库内已无实际调用
- 与现有接口功能重复
- 增加维护成本，容易让业务层误选旧接口

## 使用示例

### 1. 创建与销毁实例

```cpp
#include "IHandEyeCalib.h"

IHandEyeCalib* calib = CreateHandEyeCalibInstance();
// ...
DestroyHandEyeCalibInstance(calib);
```

### 2. 对应点刚体变换标定

```cpp
std::vector<HECPoint3D> eyePoints;
std::vector<HECPoint3D> robotPoints;

HECCalibResult result;
int ret = calib->CalculateRT(eyePoints, robotPoints, result);
```

### 3. Eye-In-Hand 标定

```cpp
std::vector<HECEyeInHandData> calibData;

HECCalibResult result;
int ret = calib->CalculateEyeInHand(calibData, result);
```

### 4. 欧拉角与旋转矩阵互转

```cpp
HECEulerAngles srcAngles;
srcAngles.roll = 0.1;
srcAngles.pitch = 0.2;
srcAngles.yaw = 0.3;

HECRotationMatrix rotation;
calib->EulerToRotationMatrix(srcAngles, HECEulerOrder::ZYX, rotation);

HECEulerAngles dstAngles;
calib->RotationMatrixToEuler(rotation, HECEulerOrder::ZYX, dstAngles);
```

### 5. 由中心点和方向种子生成机器人姿态

```cpp
HECPoseResult poseResult;
bool ok = calib->TransformPose(
    calibResult,
    HECPoint3D(100.0, 50.0, 200.0),   // eyeCenter
    HECPoint3D(1.0, 0.0, 0.0),        // longAxisDir
    HECPoint3D(0.0, 0.0, 1.0),        // normalDir
    0,                                // dirVectorInvert
    HECEulerOrder::ZYX,
    HECLongAxisDir::AxisX,
    poseResult);
```

### 6. 业务侧直接做六轴眼在手上转换

```cpp
#include "CoordinateTransform.h"

// 若机器人协议字段是 (A, B, C) 对应 (RZ, RY, RX)，
// 调用前需按真实轴含义把 C 放 rx、B 放 ry、A 放 rz。
CTRobotPose robotPose = CTRobotPose::fromDegrees(
    x, y, z,
    c, b, a);

CTHomogeneousMatrix total = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
    robotPose,
    CTEulerOrder::ZYX,
    handEyeMatrix);

CTVec3D ptRobot = total.transformPoint(CTVec3D(camX, camY, camZ));
```

## 整理原则

- 算法能力放在 `IHandEyeCalib`
- 通用坐标变换放在 `CoordinateTransform`
- 业务层只负责选择配置和调用
- 同一能力只保留一套主入口，避免重复接口长期并存