170 lines
8.0 KiB
C++
170 lines
8.0 KiB
C++
#ifndef COORDINATE_TRANSFORM_H
|
||
#define COORDINATE_TRANSFORM_H
|
||
|
||
#include "CoordinateTypes.h"
|
||
|
||
namespace CCoordinateTransform
|
||
{
|
||
// ==================== 欧拉角与四元数互转 ====================
|
||
|
||
/// 欧拉角转四元数
|
||
/// @param euler 欧拉角 (弧度制)
|
||
/// @return 归一化的四元数
|
||
CTQuaternionD eulerToQuaternion(const CTEulerAngles& euler);
|
||
|
||
/// 四元数转欧拉角
|
||
/// @param q 四元数 (会自动归一化)
|
||
/// @param order 欧拉角顺序
|
||
/// @return 欧拉角 (弧度制)
|
||
CTEulerAngles quaternionToEuler(const CTQuaternionD& q, CTEulerOrder order = CTEulerOrder::ZYX);
|
||
|
||
// ==================== 旋转矩阵与欧拉角互转 ====================
|
||
|
||
/// 欧拉角转旋转矩阵
|
||
/// @param euler 欧拉角 (弧度制)
|
||
/// @return 3x3旋转矩阵
|
||
CTRotationMatrix eulerToRotationMatrix(const CTEulerAngles& euler);
|
||
|
||
/// 旋转矩阵转欧拉角
|
||
/// @param R 3x3旋转矩阵
|
||
/// @param order 欧拉角顺序
|
||
/// @return 欧拉角 (弧度制)
|
||
CTEulerAngles rotationMatrixToEuler(const CTRotationMatrix& R, CTEulerOrder order = CTEulerOrder::ZYX);
|
||
|
||
// ==================== 旋转矩阵与四元数互转 ====================
|
||
|
||
/// 四元数转旋转矩阵
|
||
/// @param q 四元数
|
||
/// @return 3x3旋转矩阵
|
||
CTRotationMatrix quaternionToRotationMatrix(const CTQuaternionD& q);
|
||
|
||
/// 旋转矩阵转四元数
|
||
/// @param R 3x3旋转矩阵
|
||
/// @return 归一化的四元数
|
||
CTQuaternionD rotationMatrixToQuaternion(const CTRotationMatrix& R);
|
||
|
||
// ==================== 三轴手眼转换 ====================
|
||
|
||
/// 三轴手眼转换：相机坐标 -> 机器人坐标
|
||
/// 适用于三轴龙门架等只有XYZ平移和Z轴旋转的设备
|
||
/// @param cameraPt 相机坐标系下的点
|
||
/// @param robotPose 机器人当前位姿 (XYZ + Rz)
|
||
/// @param handEye 三轴手眼标定矩阵
|
||
/// @return 机器人基坐标系下的点
|
||
CTVec3D threeAxisCameraToRobot(const CTVec3D& cameraPt,
|
||
const CTRobotPose& robotPose,
|
||
const CTThreeAxisCalibResult& handEye);
|
||
|
||
/// 三轴手眼转换：机器人坐标 -> 相机坐标
|
||
/// @param robotPt 机器人基坐标系下的点
|
||
/// @param robotPose 机器人当前位姿 (XYZ + Rz)
|
||
/// @param handEye 三轴手眼标定矩阵
|
||
/// @return 相机坐标系下的点
|
||
CTVec3D threeAxisRobotToCamera(const CTVec3D& robotPt,
|
||
const CTRobotPose& robotPose,
|
||
const CTThreeAxisCalibResult& handEye);
|
||
|
||
/// 三轴手眼转换：计算抓取位姿
|
||
/// @param cameraPt 相机坐标系下的目标点
|
||
/// @param cameraAngle 相机坐标系下的目标角度 (弧度)
|
||
/// @param robotPose 机器人当前位姿
|
||
/// @param handEye 三轴手眼标定矩阵
|
||
/// @param[out] targetPose 输出的目标抓取位姿
|
||
void threeAxisCalcGraspPose(const CTVec3D& cameraPt,
|
||
double cameraAngle,
|
||
const CTRobotPose& robotPose,
|
||
const CTThreeAxisCalibResult& handEye,
|
||
CTRobotPose& targetPose);
|
||
|
||
// ==================== 六轴手眼转换 ====================
|
||
|
||
/// 六轴手眼转换 (Eye-in-Hand)：相机坐标 -> 机器人坐标
|
||
/// 相机安装在机器人末端执行器上
|
||
/// @param cameraPt 相机坐标系下的点
|
||
/// @param robotPose 机器人当前位姿 (完整6轴)
|
||
/// @param handEye 六轴手眼标定矩阵 (相机到末端执行器)
|
||
/// @return 机器人基坐标系下的点
|
||
CTVec3D sixAxisEyeInHandCameraToRobot(const CTVec3D& cameraPt,
|
||
const CTRobotPose& robotPose,
|
||
const CTSixAxisCalibResult& handEye);
|
||
|
||
/// 六轴手眼转换 (Eye-in-Hand)：机器人坐标 -> 相机坐标
|
||
/// @param robotPt 机器人基坐标系下的点
|
||
/// @param robotPose 机器人当前位姿
|
||
/// @param handEye 六轴手眼标定矩阵
|
||
/// @return 相机坐标系下的点
|
||
CTVec3D sixAxisEyeInHandRobotToCamera(const CTVec3D& robotPt,
|
||
const CTRobotPose& robotPose,
|
||
const CTSixAxisCalibResult& handEye);
|
||
|
||
/// 六轴手眼转换 (Eye-to-Hand)：相机坐标 -> 机器人坐标
|
||
/// 相机固定在外部，观察机器人工作空间
|
||
/// @param cameraPt 相机坐标系下的点
|
||
/// @param cameraToBase 相机到机器人基座的标定矩阵
|
||
/// @return 机器人基坐标系下的点
|
||
CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
|
||
const CTSixAxisCalibResult& cameraToBase);
|
||
|
||
/// 六轴手眼转换 (Eye-to-Hand)：相机坐标 -> 机器人坐标 (直接使用齐次矩阵)
|
||
/// @param cameraPt 相机坐标系下的点
|
||
/// @param cameraToBaseMatrix 相机到机器人基座的4x4齐次变换矩阵
|
||
/// @return 机器人基坐标系下的点
|
||
CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
|
||
const CTHomogeneousMatrix& cameraToBaseMatrix);
|
||
|
||
/// 六轴手眼转换 (Eye-to-Hand)：机器人坐标 -> 相机坐标
|
||
/// @param robotPt 机器人基坐标系下的点
|
||
/// @param cameraToBase 相机到机器人基座的标定矩阵
|
||
/// @return 相机坐标系下的点
|
||
CTVec3D sixAxisEyeToHandRobotToCamera(const CTVec3D& robotPt,
|
||
const CTSixAxisCalibResult& cameraToBase);
|
||
|
||
/// 六轴手眼转换：计算抓取位姿 (Eye-in-Hand)
|
||
/// @param cameraPose 相机坐标系下的目标位姿
|
||
/// @param robotPose 机器人当前位姿
|
||
/// @param handEye 六轴手眼标定矩阵
|
||
/// @param[out] targetPose 输出的目标抓取位姿
|
||
void sixAxisEyeInHandCalcGraspPose(const CTCameraPose& cameraPose,
|
||
const CTRobotPose& robotPose,
|
||
const CTSixAxisCalibResult& handEye,
|
||
CTRobotPose& targetPose);
|
||
|
||
/// 六轴手眼转换：计算抓取位姿 (Eye-to-Hand)
|
||
/// @param cameraPose 相机坐标系下的目标位姿
|
||
/// @param cameraToBase 相机到机器人基座的标定矩阵
|
||
/// @param[out] targetPose 输出的目标抓取位姿
|
||
void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
|
||
const CTSixAxisCalibResult& cameraToBase,
|
||
CTRobotPose& targetPose);
|
||
|
||
/// 六轴手眼转换：计算抓取位姿 (Eye-to-Hand) - 直接使用齐次矩阵
|
||
/// @param cameraPose 相机坐标系下的目标位姿
|
||
/// @param cameraToBaseMatrix 相机到机器人基座的4x4齐次变换矩阵
|
||
/// @param eulerOrder 输出欧拉角的顺序 (默认XYZ，对应外旋ZYX即RZ-RY-RX)
|
||
/// @param[out] targetPose 输出的目标抓取位姿
|
||
void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
|
||
const CTHomogeneousMatrix& cameraToBaseMatrix,
|
||
CTEulerOrder eulerOrder,
|
||
CTRobotPose& targetPose);
|
||
|
||
// ==================== 辅助函数 ====================
|
||
|
||
/// 角度转弧度
|
||
inline double degToRad(double deg) { return deg * M_PI / 180.0; }
|
||
|
||
/// 弧度转角度
|
||
inline double radToDeg(double rad) { return rad * 180.0 / M_PI; }
|
||
|
||
/// 归一化角度到 [-π, π]
|
||
double normalizeAngle(double angle);
|
||
|
||
/// 计算两个四元数之间的角度差 (弧度)
|
||
double quaternionAngleDiff(const CTQuaternionD& q1, const CTQuaternionD& q2);
|
||
|
||
/// 四元数球面线性插值 (SLERP)
|
||
CTQuaternionD quaternionSlerp(const CTQuaternionD& q1, const CTQuaternionD& q2, double t);
|
||
|
||
} // namespace CCoordinateTransform
|
||
|
||
#endif // COORDINATE_TRANSFORM_H
|