# 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,
    CTRobotPoseConvention poseConvention = CTRobotPoseConvention::LegacyRxRyRz);
```

`poseConvention` 用于描述机器人报文中三个角字段的排列约定。

例如：

- `LegacyRxRyRz`
  兼容旧逻辑，字段按 `RX, RY, RZ` 解释
- `OrderedRzRyRx`
  字段按 `RZ, RY, RX` 解释

## 已移除的旧接口

以下接口已经从公开 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"

CTRobotPose robotPose = CTRobotPose::fromDegrees(
    x, y, z,
    a1, a2, a3);

CTHomogeneousMatrix total = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
    robotPose,
    CTEulerOrder::sZYX,
    handEyeMatrix,
    CTRobotPoseConvention::OrderedRzRyRx);

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

## 整理原则

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