498 lines
16 KiB
C++
498 lines
16 KiB
C++
#include "CoordinateTransform.h"
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/Geometry>
|
|
#include <cmath>
|
|
|
|
namespace CCoordinateTransform
|
|
{
|
|
|
|
// ==================== 内部辅助函数 ====================
|
|
|
|
// 将自定义 CTRotationMatrix 转为 Eigen Matrix3d
|
|
static Eigen::Matrix3d toEigenMatrix(const CTRotationMatrix& R)
|
|
{
|
|
Eigen::Matrix3d mat;
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
mat(i, j) = R.at(i, j);
|
|
}
|
|
}
|
|
return mat;
|
|
}
|
|
|
|
// 将 Eigen Matrix3d 转为自定义 CTRotationMatrix
|
|
static CTRotationMatrix fromEigenMatrix(const Eigen::Matrix3d& mat)
|
|
{
|
|
CTRotationMatrix R;
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
R.at(i, j) = mat(i, j);
|
|
}
|
|
}
|
|
return R;
|
|
}
|
|
|
|
// 将自定义 CTQuaternionD 转为 Eigen Quaterniond
|
|
static Eigen::Quaterniond toEigenQuaternion(const CTQuaternionD& q)
|
|
{
|
|
return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
|
|
}
|
|
|
|
// 将 Eigen Quaterniond 转为自定义 CTQuaternionD
|
|
static CTQuaternionD fromEigenQuaternion(const Eigen::Quaterniond& eq)
|
|
{
|
|
return CTQuaternionD(eq.w(), eq.x(), eq.y(), eq.z());
|
|
}
|
|
|
|
// 将自定义 CTVec3D 转为 Eigen Vector3d
|
|
static Eigen::Vector3d toEigenVector(const CTVec3D& v)
|
|
{
|
|
return Eigen::Vector3d(v.x, v.y, v.z);
|
|
}
|
|
|
|
// 将 Eigen Vector3d 转为自定义 CTVec3D
|
|
static CTVec3D fromEigenVector(const Eigen::Vector3d& ev)
|
|
{
|
|
return CTVec3D(ev.x(), ev.y(), ev.z());
|
|
}
|
|
|
|
// ==================== 欧拉角与四元数互转 ====================
|
|
|
|
CTQuaternionD eulerToQuaternion(const CTEulerAngles& euler)
|
|
{
|
|
// 使用 Eigen 的 AngleAxis 构建旋转
|
|
Eigen::AngleAxisd rollAngle(euler.roll, Eigen::Vector3d::UnitX());
|
|
Eigen::AngleAxisd pitchAngle(euler.pitch, Eigen::Vector3d::UnitY());
|
|
Eigen::AngleAxisd yawAngle(euler.yaw, Eigen::Vector3d::UnitZ());
|
|
|
|
Eigen::Quaterniond q;
|
|
switch (euler.order)
|
|
{
|
|
// ==================== 内旋顺序 ====================
|
|
case CTEulerOrder::XYZ:
|
|
q = rollAngle * pitchAngle * yawAngle;
|
|
break;
|
|
case CTEulerOrder::ZYX:
|
|
q = yawAngle * pitchAngle * rollAngle;
|
|
break;
|
|
case CTEulerOrder::ZXY:
|
|
q = yawAngle * rollAngle * pitchAngle;
|
|
break;
|
|
case CTEulerOrder::YXZ:
|
|
q = pitchAngle * rollAngle * yawAngle;
|
|
break;
|
|
case CTEulerOrder::YZX:
|
|
q = pitchAngle * yawAngle * rollAngle;
|
|
break;
|
|
case CTEulerOrder::XZY:
|
|
q = rollAngle * yawAngle * pitchAngle;
|
|
break;
|
|
|
|
// ==================== 外旋顺序 ====================
|
|
case CTEulerOrder::sZYX:
|
|
q = rollAngle * pitchAngle * yawAngle;
|
|
break;
|
|
case CTEulerOrder::sXYZ:
|
|
q = yawAngle * pitchAngle * rollAngle;
|
|
break;
|
|
case CTEulerOrder::sZXY:
|
|
q = pitchAngle * rollAngle * yawAngle;
|
|
break;
|
|
case CTEulerOrder::sYXZ:
|
|
q = yawAngle * rollAngle * pitchAngle;
|
|
break;
|
|
case CTEulerOrder::sYZX:
|
|
q = rollAngle * yawAngle * pitchAngle;
|
|
break;
|
|
case CTEulerOrder::sXZY:
|
|
q = pitchAngle * yawAngle * rollAngle;
|
|
break;
|
|
|
|
default:
|
|
q = yawAngle * pitchAngle * rollAngle;
|
|
break;
|
|
}
|
|
|
|
return fromEigenQuaternion(q.normalized());
|
|
}
|
|
|
|
CTEulerAngles quaternionToEuler(const CTQuaternionD& q, CTEulerOrder order)
|
|
{
|
|
Eigen::Quaterniond eq = toEigenQuaternion(q).normalized();
|
|
Eigen::Matrix3d R = eq.toRotationMatrix();
|
|
|
|
CTEulerAngles euler;
|
|
euler.order = order;
|
|
|
|
Eigen::Vector3d angles;
|
|
|
|
switch (order)
|
|
{
|
|
// ==================== 内旋顺序 ====================
|
|
case CTEulerOrder::ZYX:
|
|
angles = R.eulerAngles(2, 1, 0);
|
|
euler.yaw = angles(0);
|
|
euler.pitch = angles(1);
|
|
euler.roll = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::XYZ:
|
|
angles = R.eulerAngles(0, 1, 2);
|
|
euler.roll = angles(0);
|
|
euler.pitch = angles(1);
|
|
euler.yaw = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::ZXY:
|
|
angles = R.eulerAngles(2, 0, 1);
|
|
euler.yaw = angles(0);
|
|
euler.roll = angles(1);
|
|
euler.pitch = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::YXZ:
|
|
angles = R.eulerAngles(1, 0, 2);
|
|
euler.pitch = angles(0);
|
|
euler.roll = angles(1);
|
|
euler.yaw = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::YZX:
|
|
angles = R.eulerAngles(1, 2, 0);
|
|
euler.pitch = angles(0);
|
|
euler.yaw = angles(1);
|
|
euler.roll = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::XZY:
|
|
angles = R.eulerAngles(0, 2, 1);
|
|
euler.roll = angles(0);
|
|
euler.yaw = angles(1);
|
|
euler.pitch = angles(2);
|
|
break;
|
|
|
|
// ==================== 外旋顺序 ====================
|
|
case CTEulerOrder::sZYX:
|
|
angles = R.eulerAngles(0, 1, 2);
|
|
euler.roll = angles(0);
|
|
euler.pitch = angles(1);
|
|
euler.yaw = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::sXYZ:
|
|
angles = R.eulerAngles(2, 1, 0);
|
|
euler.yaw = angles(0);
|
|
euler.pitch = angles(1);
|
|
euler.roll = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::sZXY:
|
|
angles = R.eulerAngles(1, 0, 2);
|
|
euler.pitch = angles(0);
|
|
euler.roll = angles(1);
|
|
euler.yaw = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::sYXZ:
|
|
angles = R.eulerAngles(2, 0, 1);
|
|
euler.yaw = angles(0);
|
|
euler.roll = angles(1);
|
|
euler.pitch = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::sYZX:
|
|
angles = R.eulerAngles(0, 2, 1);
|
|
euler.roll = angles(0);
|
|
euler.yaw = angles(1);
|
|
euler.pitch = angles(2);
|
|
break;
|
|
|
|
case CTEulerOrder::sXZY:
|
|
angles = R.eulerAngles(1, 2, 0);
|
|
euler.pitch = angles(0);
|
|
euler.yaw = angles(1);
|
|
euler.roll = angles(2);
|
|
break;
|
|
|
|
default:
|
|
angles = R.eulerAngles(2, 1, 0);
|
|
euler.yaw = angles(0);
|
|
euler.pitch = angles(1);
|
|
euler.roll = angles(2);
|
|
break;
|
|
}
|
|
|
|
return euler;
|
|
}
|
|
|
|
// ==================== 旋转矩阵与欧拉角互转 ====================
|
|
|
|
CTRotationMatrix eulerToRotationMatrix(const CTEulerAngles& euler)
|
|
{
|
|
CTQuaternionD q = eulerToQuaternion(euler);
|
|
return quaternionToRotationMatrix(q);
|
|
}
|
|
|
|
CTEulerAngles rotationMatrixToEuler(const CTRotationMatrix& R, CTEulerOrder order)
|
|
{
|
|
CTQuaternionD q = rotationMatrixToQuaternion(R);
|
|
return quaternionToEuler(q, order);
|
|
}
|
|
|
|
// ==================== 旋转矩阵与四元数互转 ====================
|
|
|
|
CTRotationMatrix quaternionToRotationMatrix(const CTQuaternionD& q)
|
|
{
|
|
Eigen::Quaterniond eq = toEigenQuaternion(q).normalized();
|
|
return fromEigenMatrix(eq.toRotationMatrix());
|
|
}
|
|
|
|
CTQuaternionD rotationMatrixToQuaternion(const CTRotationMatrix& R)
|
|
{
|
|
Eigen::Matrix3d mat = toEigenMatrix(R);
|
|
Eigen::Quaterniond eq(mat);
|
|
return fromEigenQuaternion(eq.normalized());
|
|
}
|
|
|
|
} // namespace CCoordinateTransform
|
|
|
|
// ==================== 结构体成员函数实现 ====================
|
|
|
|
CTHomogeneousMatrix CTThreeAxisCalibResult::toHomogeneousMatrix() const
|
|
{
|
|
CTEulerAngles euler(0.0, 0.0, rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
|
|
CTVec3D t(dx, dy, dz);
|
|
return CTHomogeneousMatrix(R, t);
|
|
}
|
|
|
|
CTHomogeneousMatrix CTSixAxisCalibResult::toHomogeneousMatrix() const
|
|
{
|
|
CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
|
|
CTVec3D t(dx, dy, dz);
|
|
return CTHomogeneousMatrix(R, t);
|
|
}
|
|
|
|
CTHomogeneousMatrix CTRobotPose::toHomogeneousMatrix() const
|
|
{
|
|
CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
|
|
CTVec3D t(x, y, z);
|
|
return CTHomogeneousMatrix(R, t);
|
|
}
|
|
|
|
CTHomogeneousMatrix CTCameraPose::toHomogeneousMatrix() const
|
|
{
|
|
CTEulerAngles euler(rx, ry, rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
|
|
CTVec3D t(x, y, z);
|
|
return CTHomogeneousMatrix(R, t);
|
|
}
|
|
|
|
CTHomogeneousMatrix CTCameraPose::toHomogeneousMatrix(CTEulerOrder order) const
|
|
{
|
|
CTEulerAngles euler(rx, ry, rz, order);
|
|
CTRotationMatrix R = CCoordinateTransform::eulerToRotationMatrix(euler);
|
|
CTVec3D t(x, y, z);
|
|
return CTHomogeneousMatrix(R, t);
|
|
}
|
|
|
|
// ==================== 继续命名空间内的函数 ====================
|
|
|
|
namespace CCoordinateTransform
|
|
{
|
|
|
|
// ==================== 三轴手眼转换 ====================
|
|
|
|
CTVec3D threeAxisCameraToRobot(const CTVec3D& cameraPt,
|
|
const CTRobotPose& robotPose,
|
|
const CTThreeAxisCalibResult& handEye)
|
|
{
|
|
CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
|
|
CTRobotPose poseRz(robotPose.x, robotPose.y, robotPose.z, 0.0, 0.0, robotPose.rz);
|
|
CTHomogeneousMatrix T_robot = poseRz.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
|
|
return T_total.transformPoint(cameraPt);
|
|
}
|
|
|
|
CTVec3D threeAxisRobotToCamera(const CTVec3D& robotPt,
|
|
const CTRobotPose& robotPose,
|
|
const CTThreeAxisCalibResult& handEye)
|
|
{
|
|
CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
|
|
CTRobotPose poseRz(robotPose.x, robotPose.y, robotPose.z, 0.0, 0.0, robotPose.rz);
|
|
CTHomogeneousMatrix T_robot = poseRz.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
|
|
CTHomogeneousMatrix T_inv = T_total.inverse();
|
|
return T_inv.transformPoint(robotPt);
|
|
}
|
|
|
|
void threeAxisCalcGraspPose(const CTVec3D& cameraPt,
|
|
double cameraAngle,
|
|
const CTRobotPose& robotPose,
|
|
const CTThreeAxisCalibResult& handEye,
|
|
CTRobotPose& targetPose)
|
|
{
|
|
CTVec3D robotPt = threeAxisCameraToRobot(cameraPt, robotPose, handEye);
|
|
double targetAngle = normalizeAngle(robotPose.rz + handEye.rz + cameraAngle);
|
|
|
|
targetPose.x = robotPt.x;
|
|
targetPose.y = robotPt.y;
|
|
targetPose.z = robotPt.z;
|
|
targetPose.rx = 0.0;
|
|
targetPose.ry = 0.0;
|
|
targetPose.rz = targetAngle;
|
|
}
|
|
|
|
// ==================== 六轴手眼转换 ====================
|
|
|
|
CTVec3D sixAxisEyeInHandCameraToRobot(const CTVec3D& cameraPt,
|
|
const CTRobotPose& robotPose,
|
|
const CTSixAxisCalibResult& handEye)
|
|
{
|
|
CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
|
|
return T_total.transformPoint(cameraPt);
|
|
}
|
|
|
|
CTVec3D sixAxisEyeInHandRobotToCamera(const CTVec3D& robotPt,
|
|
const CTRobotPose& robotPose,
|
|
const CTSixAxisCalibResult& handEye)
|
|
{
|
|
CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
|
|
CTHomogeneousMatrix T_inv = T_total.inverse();
|
|
return T_inv.transformPoint(robotPt);
|
|
}
|
|
|
|
CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
|
|
const CTSixAxisCalibResult& cameraToBase)
|
|
{
|
|
CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();
|
|
return T_cam_to_base.transformPoint(cameraPt);
|
|
}
|
|
|
|
CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
|
|
const CTHomogeneousMatrix& cameraToBaseMatrix)
|
|
{
|
|
return cameraToBaseMatrix.transformPoint(cameraPt);
|
|
}
|
|
|
|
CTVec3D sixAxisEyeToHandRobotToCamera(const CTVec3D& robotPt,
|
|
const CTSixAxisCalibResult& cameraToBase)
|
|
{
|
|
CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_inv = T_cam_to_base.inverse();
|
|
return T_inv.transformPoint(robotPt);
|
|
}
|
|
|
|
void sixAxisEyeInHandCalcGraspPose(const CTCameraPose& cameraPose,
|
|
const CTRobotPose& robotPose,
|
|
const CTSixAxisCalibResult& handEye,
|
|
CTRobotPose& targetPose)
|
|
{
|
|
CTHomogeneousMatrix T_hand_eye = handEye.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_robot = robotPose.toHomogeneousMatrix();
|
|
CTHomogeneousMatrix T_total = T_robot * T_hand_eye;
|
|
|
|
CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
|
|
CTVec3D robotPt = T_total.transformPoint(cameraPt);
|
|
targetPose.x = robotPt.x;
|
|
targetPose.y = robotPt.y;
|
|
targetPose.z = robotPt.z;
|
|
|
|
CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
|
|
CTRotationMatrix R_total = T_total.getRotation();
|
|
CTRotationMatrix R_robot = R_total * R_cam;
|
|
|
|
CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, CTEulerOrder::ZYX);
|
|
targetPose.rx = robotEuler.roll;
|
|
targetPose.ry = robotEuler.pitch;
|
|
targetPose.rz = robotEuler.yaw;
|
|
}
|
|
|
|
void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
|
|
const CTSixAxisCalibResult& cameraToBase,
|
|
CTRobotPose& targetPose)
|
|
{
|
|
CTHomogeneousMatrix T_cam_to_base = cameraToBase.toHomogeneousMatrix();
|
|
|
|
CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
|
|
CTVec3D robotPt = T_cam_to_base.transformPoint(cameraPt);
|
|
targetPose.x = robotPt.x;
|
|
targetPose.y = robotPt.y;
|
|
targetPose.z = robotPt.z;
|
|
|
|
CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, CTEulerOrder::ZYX);
|
|
CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
|
|
CTRotationMatrix R_cam_to_base = T_cam_to_base.getRotation();
|
|
CTRotationMatrix R_robot = R_cam_to_base * R_cam;
|
|
|
|
CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, CTEulerOrder::ZYX);
|
|
targetPose.rx = robotEuler.roll;
|
|
targetPose.ry = robotEuler.pitch;
|
|
targetPose.rz = robotEuler.yaw;
|
|
}
|
|
|
|
void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
|
|
const CTHomogeneousMatrix& cameraToBaseMatrix,
|
|
CTEulerOrder eulerOrder,
|
|
CTRobotPose& targetPose)
|
|
{
|
|
CTVec3D cameraPt(cameraPose.x, cameraPose.y, cameraPose.z);
|
|
CTVec3D robotPt = cameraToBaseMatrix.transformPoint(cameraPt);
|
|
targetPose.x = robotPt.x;
|
|
targetPose.y = robotPt.y;
|
|
targetPose.z = robotPt.z;
|
|
|
|
CTEulerAngles camEuler(cameraPose.rx, cameraPose.ry, cameraPose.rz, eulerOrder);
|
|
CTRotationMatrix R_cam = eulerToRotationMatrix(camEuler);
|
|
CTRotationMatrix R_cam_to_base = cameraToBaseMatrix.getRotation();
|
|
CTRotationMatrix R_robot = R_cam_to_base * R_cam;
|
|
|
|
CTEulerAngles robotEuler = rotationMatrixToEuler(R_robot, eulerOrder);
|
|
targetPose.rx = robotEuler.roll;
|
|
targetPose.ry = robotEuler.pitch;
|
|
targetPose.rz = robotEuler.yaw;
|
|
}
|
|
|
|
// ==================== 辅助函数 ====================
|
|
|
|
double normalizeAngle(double angle)
|
|
{
|
|
while (angle > M_PI)
|
|
angle -= 2.0 * M_PI;
|
|
while (angle < -M_PI)
|
|
angle += 2.0 * M_PI;
|
|
return angle;
|
|
}
|
|
|
|
double quaternionAngleDiff(const CTQuaternionD& q1, const CTQuaternionD& q2)
|
|
{
|
|
Eigen::Quaterniond eq1 = toEigenQuaternion(q1).normalized();
|
|
Eigen::Quaterniond eq2 = toEigenQuaternion(q2).normalized();
|
|
|
|
double dot = eq1.dot(eq2);
|
|
if (dot < 0.0) {
|
|
dot = -dot;
|
|
}
|
|
if (dot > 1.0) dot = 1.0;
|
|
|
|
return 2.0 * std::acos(dot);
|
|
}
|
|
|
|
CTQuaternionD quaternionSlerp(const CTQuaternionD& q1, const CTQuaternionD& q2, double t)
|
|
{
|
|
Eigen::Quaterniond eq1 = toEigenQuaternion(q1).normalized();
|
|
Eigen::Quaterniond eq2 = toEigenQuaternion(q2).normalized();
|
|
|
|
Eigen::Quaterniond result = eq1.slerp(t, eq2);
|
|
return fromEigenQuaternion(result);
|
|
}
|
|
|
|
} // namespace CCoordinateTransform
|