197 lines
6.6 KiB
C++
197 lines
6.6 KiB
C++
#ifndef IHANDEYECALIB_H
|
||
#define IHANDEYECALIB_H
|
||
|
||
#include <vector>
|
||
#include "HandEyeCalib_global.h"
|
||
#include "HandEyeCalibTypes.h"
|
||
|
||
/**
|
||
* @brief 手眼标定接口类
|
||
* 提供基于SVD分解的手眼标定算法，使用Eigen库实现
|
||
* 不依赖OpenCV
|
||
*/
|
||
class IHandEyeCalib
|
||
{
|
||
public:
|
||
virtual ~IHandEyeCalib() = default;
|
||
|
||
/**
|
||
* @brief 计算旋转平移矩阵 (从眼坐标系到机器人坐标系)
|
||
* 使用SVD分解方法计算两组对应点之间的刚体变换
|
||
* @param eyePoints 眼坐标系（相机坐标系）下的点集
|
||
* @param robotPoints 机器人坐标系下的点集
|
||
* @param result 输出的标定结果 (R, T, 质心等)
|
||
* @return 成功返回0，失败返回错误码
|
||
*/
|
||
virtual int CalculateRT(
|
||
const std::vector<HECPoint3D>& eyePoints,
|
||
const std::vector<HECPoint3D>& robotPoints,
|
||
HECCalibResult& result) = 0;
|
||
|
||
/**
|
||
* @brief 计算旋转平移矩阵 (使用点对数组)
|
||
* @param pointPairs 对应点对数组
|
||
* @param result 输出的标定结果
|
||
* @return 成功返回0，失败返回错误码
|
||
*/
|
||
virtual int CalculateRT(
|
||
const std::vector<HECCalibPointPair>& pointPairs,
|
||
HECCalibResult& result) = 0;
|
||
|
||
/**
|
||
* @brief 使用RT矩阵变换点 (完整变换: R*p + T)
|
||
* @param R 旋转矩阵
|
||
* @param T 平移向量
|
||
* @param srcPoint 源点
|
||
* @param dstPoint 输出的变换后的点
|
||
*/
|
||
virtual void TransformPoint(
|
||
const HECRotationMatrix& R,
|
||
const HECTranslationVector& T,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint) = 0;
|
||
|
||
/**
|
||
* @brief 使用RT矩阵变换点 (基于质心的变换)
|
||
* 变换公式: dstPoint = R * (srcPoint - srcCenter) + dstCenter
|
||
* @param R 旋转矩阵
|
||
* @param T 平移向量
|
||
* @param srcCenter 源坐标系质心
|
||
* @param dstCenter 目标坐标系质心
|
||
* @param srcPoint 源点
|
||
* @param dstPoint 输出的变换后的点
|
||
*/
|
||
virtual void TransformPointWithCenter(
|
||
const HECRotationMatrix& R,
|
||
const HECTranslationVector& T,
|
||
const HECPoint3D& srcCenter,
|
||
const HECPoint3D& dstCenter,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint) = 0;
|
||
|
||
/**
|
||
* @brief 仅使用旋转矩阵变换点 (不含平移)
|
||
* @param R 旋转矩阵
|
||
* @param srcPoint 源点
|
||
* @param dstPoint 输出的变换后的点
|
||
*/
|
||
virtual void RotatePoint(
|
||
const HECRotationMatrix& R,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint) = 0;
|
||
|
||
/**
|
||
* @brief 从旋转矩阵计算欧拉角 (ZYX顺序，保持向后兼容)
|
||
* @param R 旋转矩阵
|
||
* @param angles 输出的欧拉角 (弧度制)
|
||
*/
|
||
virtual void RotationMatrixToEulerZYX(
|
||
const HECRotationMatrix& R,
|
||
HECEulerAngles& angles) = 0;
|
||
|
||
/**
|
||
* @brief 从旋转矩阵计算欧拉角 (支持多种旋转顺序)
|
||
* @param R 旋转矩阵
|
||
* @param order 欧拉角旋转顺序
|
||
* @param angles 输出的欧拉角 (弧度制)
|
||
*/
|
||
virtual void RotationMatrixToEuler(
|
||
const HECRotationMatrix& R,
|
||
HECEulerOrder order,
|
||
HECEulerAngles& angles) = 0;
|
||
|
||
/**
|
||
* @brief 从欧拉角计算旋转矩阵 (ZYX顺序，保持向后兼容)
|
||
* @param angles 欧拉角 (弧度制)
|
||
* @param R 输出的旋转矩阵
|
||
*/
|
||
virtual void EulerZYXToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECRotationMatrix& R) = 0;
|
||
|
||
/**
|
||
* @brief 从欧拉角计算旋转矩阵 (支持多种旋转顺序)
|
||
* @param angles 欧拉角 (弧度制)
|
||
* @param order 欧拉角旋转顺序
|
||
* @param R 输出的旋转矩阵
|
||
*/
|
||
virtual void EulerToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECEulerOrder order,
|
||
HECRotationMatrix& R) = 0;
|
||
|
||
/**
|
||
* @brief 计算完整的姿态转换 (位置 + 姿态)
|
||
* 将眼坐标系下的位姿转换到机器人坐标系
|
||
* @param calibResult 标定结果
|
||
* @param eyePoint 眼坐标系下的点
|
||
* @param eyeDirVectors 眼坐标系下的方向向量 (X, Y, Z轴方向)
|
||
* @param invertYZ 是否对Y轴和Z轴方向取反 (坐标系方向调整)
|
||
* @param poseResult 输出的机器人坐标系下的位姿
|
||
*/
|
||
virtual void TransformPose(
|
||
const HECCalibResult& calibResult,
|
||
const HECPoint3D& eyePoint,
|
||
const std::vector<HECPoint3D>& eyeDirVectors,
|
||
bool invertYZ,
|
||
HECPoseResult& poseResult) = 0;
|
||
|
||
/**
|
||
* @brief 计算标定误差
|
||
* @param eyePoints 眼坐标系下的点集
|
||
* @param robotPoints 机器人坐标系下的点集
|
||
* @param calibResult 标定结果
|
||
* @return 平均误差 (mm)
|
||
*/
|
||
virtual double CalculateError(
|
||
const std::vector<HECPoint3D>& eyePoints,
|
||
const std::vector<HECPoint3D>& robotPoints,
|
||
const HECCalibResult& calibResult) = 0;
|
||
|
||
/**
|
||
* @brief 眼在手上标定 (Eye-In-Hand)
|
||
* 相机安装在机器人末端，求相机坐标系到末端坐标系的变换
|
||
*
|
||
* 原理：对于固定的标定点P_base（在基座坐标系下），有：
|
||
* P_base = T_end * T_cam * P_cam
|
||
* 其中 T_end 是末端位姿，T_cam 是待求的相机到末端的变换
|
||
*
|
||
* 通过多组数据，将问题转化为 AX=XB 形式求解
|
||
*
|
||
* @param calibData 标定数据数组，每组包含末端位姿和相机观测点
|
||
* @param result 输出的标定结果 (相机到末端的变换)
|
||
* @return 成功返回0，失败返回错误码
|
||
*/
|
||
virtual int CalculateEyeInHand(
|
||
const std::vector<HECEyeInHandData>& calibData,
|
||
HECCalibResult& result) = 0;
|
||
|
||
/**
|
||
* @brief 眼在手上标定 - 使用已知的固定标定点
|
||
* 当标定点在基座坐标系下的位置已知时使用此方法
|
||
*
|
||
* @param calibData 标定数据数组
|
||
* @param targetInBase 标定点在基座坐标系下的已知坐标
|
||
* @param result 输出的标定结果
|
||
* @return 成功返回0，失败返回错误码
|
||
*/
|
||
virtual int CalculateEyeInHandWithTarget(
|
||
const std::vector<HECEyeInHandData>& calibData,
|
||
const HECPoint3D& targetInBase,
|
||
HECCalibResult& result) = 0;
|
||
};
|
||
|
||
/**
|
||
* @brief 创建手眼标定实例
|
||
* @return 手眼标定实例指针
|
||
*/
|
||
IHandEyeCalib* CreateHandEyeCalibInstance();
|
||
|
||
/**
|
||
* @brief 销毁手眼标定实例
|
||
* @param instance 实例指针
|
||
*/
|
||
void DestroyHandEyeCalibInstance(IHandEyeCalib* instance);
|
||
|
||
#endif // IHANDEYECALIB_H
|