197 lines
5.0 KiB
Markdown
197 lines
5.0 KiB
Markdown
# 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`
|
||
- 业务层只负责选择配置和调用
|
||
- 同一能力只保留一套主入口，避免重复接口长期并存
|