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