953 lines
32 KiB
C++
953 lines
32 KiB
C++
#ifndef HANDEYECALIB_LIBRARY
|
||
#define HANDEYECALIB_LIBRARY
|
||
#endif
|
||
|
||
#include "HandEyeCalib.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include <Eigen/Dense>
|
||
#include <Eigen/SVD>
|
||
#include <cmath>
|
||
|
||
HandEyeCalib::HandEyeCalib()
|
||
{
|
||
}
|
||
|
||
HandEyeCalib::~HandEyeCalib()
|
||
{
|
||
}
|
||
|
||
int HandEyeCalib::CalculateRT(
|
||
const std::vector<HECPoint3D>& eyePoints,
|
||
const std::vector<HECPoint3D>& robotPoints,
|
||
HECCalibResult& result)
|
||
{
|
||
// 检查输入参数
|
||
if (eyePoints.size() != robotPoints.size()) {
|
||
return ERR_CODE(APP_ERR_PARAM);
|
||
}
|
||
|
||
int N = static_cast<int>(eyePoints.size());
|
||
if (N < 3) {
|
||
return ERR_CODE(APP_ERR_PARAM); // 至少需要3个点
|
||
}
|
||
|
||
// 【1】计算质心
|
||
HECPoint3D p1, p2;
|
||
for (int i = 0; i < N; i++) {
|
||
p1 += eyePoints[i];
|
||
p2 += robotPoints[i];
|
||
}
|
||
p1 = p1 / static_cast<double>(N);
|
||
p2 = p2 / static_cast<double>(N);
|
||
result.centerEye = p1;
|
||
result.centerRobot = p2;
|
||
|
||
// 【2】计算去中心坐标
|
||
std::vector<HECPoint3D> q1(N), q2(N);
|
||
for (int i = 0; i < N; i++) {
|
||
q1[i] = eyePoints[i] - p1;
|
||
q2[i] = robotPoints[i] - p2;
|
||
}
|
||
|
||
// 【3】计算协方差矩阵 W = sum(q1 * q2^T)
|
||
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Vector3d v1(q1[i].x, q1[i].y, q1[i].z);
|
||
Eigen::Vector3d v2(q2[i].x, q2[i].y, q2[i].z);
|
||
W += v1 * v2.transpose();
|
||
}
|
||
|
||
// 【4】对W进行SVD分解
|
||
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||
Eigen::Matrix3d U = svd.matrixU();
|
||
Eigen::Matrix3d V = svd.matrixV();
|
||
|
||
// 【5】计算旋转矩阵 R = V * M * U^T
|
||
// M用于处理反射情况，确保R是正交旋转矩阵
|
||
double det = (U * V.transpose()).determinant();
|
||
Eigen::Matrix3d M;
|
||
M << 1, 0, 0,
|
||
0, 1, 0,
|
||
0, 0, det;
|
||
|
||
Eigen::Matrix3d R_eigen = V * M * U.transpose();
|
||
|
||
// 【6】计算平移向量 T = p2 - R * p1
|
||
Eigen::Vector3d p1_eigen(p1.x, p1.y, p1.z);
|
||
Eigen::Vector3d p2_eigen(p2.x, p2.y, p2.z);
|
||
Eigen::Vector3d T_eigen = p2_eigen - R_eigen * p1_eigen;
|
||
|
||
// 【7】转换为输出格式
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
result.R.at(i, j) = R_eigen(i, j);
|
||
}
|
||
}
|
||
result.T = HECTranslationVector(T_eigen(0), T_eigen(1), T_eigen(2));
|
||
|
||
// 【8】计算标定误差
|
||
result.error = CalculateError(eyePoints, robotPoints, result);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int HandEyeCalib::CalculateRT(
|
||
const std::vector<HECCalibPointPair>& pointPairs,
|
||
HECCalibResult& result)
|
||
{
|
||
std::vector<HECPoint3D> eyePoints;
|
||
std::vector<HECPoint3D> robotPoints;
|
||
|
||
eyePoints.reserve(pointPairs.size());
|
||
robotPoints.reserve(pointPairs.size());
|
||
|
||
for (const auto& pair : pointPairs) {
|
||
eyePoints.push_back(pair.eyePoint);
|
||
robotPoints.push_back(pair.robotPoint);
|
||
}
|
||
|
||
return CalculateRT(eyePoints, robotPoints, result);
|
||
}
|
||
|
||
void HandEyeCalib::TransformPoint(
|
||
const HECRotationMatrix& R,
|
||
const HECTranslationVector& T,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint)
|
||
{
|
||
// 使用Eigen进行计算
|
||
Eigen::Matrix3d R_eigen;
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
R_eigen(i, j) = R.at(i, j);
|
||
}
|
||
}
|
||
Eigen::Vector3d T_eigen(T.at(0), T.at(1), T.at(2));
|
||
Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);
|
||
|
||
Eigen::Vector3d dst_eigen = R_eigen * src_eigen + T_eigen;
|
||
|
||
dstPoint.x = dst_eigen(0);
|
||
dstPoint.y = dst_eigen(1);
|
||
dstPoint.z = dst_eigen(2);
|
||
}
|
||
|
||
void HandEyeCalib::RotatePoint(
|
||
const HECRotationMatrix& R,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint)
|
||
{
|
||
// 使用Eigen进行计算
|
||
Eigen::Matrix3d R_eigen;
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
R_eigen(i, j) = R.at(i, j);
|
||
}
|
||
}
|
||
Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);
|
||
|
||
Eigen::Vector3d dst_eigen = R_eigen * src_eigen;
|
||
|
||
dstPoint.x = dst_eigen(0);
|
||
dstPoint.y = dst_eigen(1);
|
||
dstPoint.z = dst_eigen(2);
|
||
}
|
||
|
||
void HandEyeCalib::RotationMatrixToEuler(
|
||
const HECRotationMatrix& R,
|
||
HECEulerOrder order,
|
||
HECEulerAngles& angles)
|
||
{
|
||
// 将 HECRotationMatrix 转换为 Eigen::Matrix3d
|
||
Eigen::Matrix3d R_eigen;
|
||
for (int i = 0; i < 3; ++i)
|
||
for (int j = 0; j < 3; ++j)
|
||
R_eigen(i, j) = R.at(i, j);
|
||
|
||
// 外旋 ABC = 内旋 CBA，使用 Eigen 的 eulerAngles（内旋模式）
|
||
Eigen::Vector3d ea;
|
||
switch (order) {
|
||
case HECEulerOrder::XYZ:
|
||
// 外旋 XYZ = 内旋 ZYX
|
||
ea = R_eigen.eulerAngles(2, 1, 0); // 返回 [yaw, pitch, roll]
|
||
angles.yaw = ea[0]; angles.pitch = ea[1]; angles.roll = ea[2];
|
||
break;
|
||
case HECEulerOrder::XZY:
|
||
// 外旋 XZY = 内旋 YZX
|
||
ea = R_eigen.eulerAngles(1, 2, 0); // 返回 [pitch, yaw, roll]
|
||
angles.pitch = ea[0]; angles.yaw = ea[1]; angles.roll = ea[2];
|
||
break;
|
||
case HECEulerOrder::YXZ:
|
||
// 外旋 YXZ = 内旋 ZXY
|
||
ea = R_eigen.eulerAngles(2, 0, 1); // 返回 [yaw, roll, pitch]
|
||
angles.yaw = ea[0]; angles.roll = ea[1]; angles.pitch = ea[2];
|
||
break;
|
||
case HECEulerOrder::YZX:
|
||
// 外旋 YZX = 内旋 XZY
|
||
ea = R_eigen.eulerAngles(0, 2, 1); // 返回 [roll, yaw, pitch]
|
||
angles.roll = ea[0]; angles.yaw = ea[1]; angles.pitch = ea[2];
|
||
break;
|
||
case HECEulerOrder::ZXY:
|
||
// 外旋 ZXY = 内旋 YXZ
|
||
ea = R_eigen.eulerAngles(1, 0, 2); // 返回 [pitch, roll, yaw]
|
||
angles.pitch = ea[0]; angles.roll = ea[1]; angles.yaw = ea[2];
|
||
break;
|
||
case HECEulerOrder::ZYX:
|
||
// 外旋 ZYX = 内旋 XYZ
|
||
ea = R_eigen.eulerAngles(0, 1, 2); // 返回 [roll, pitch, yaw]
|
||
angles.roll = ea[0]; angles.pitch = ea[1]; angles.yaw = ea[2];
|
||
break;
|
||
}
|
||
}
|
||
|
||
void HandEyeCalib::EulerToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECEulerOrder order,
|
||
HECRotationMatrix& R)
|
||
{
|
||
double roll = angles.roll; // 绕X轴
|
||
double pitch = angles.pitch; // 绕Y轴
|
||
double yaw = angles.yaw; // 绕Z轴
|
||
|
||
// 使用 Eigen::AngleAxisd 构造各轴旋转矩阵
|
||
Eigen::Matrix3d Rx = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()).toRotationMatrix();
|
||
Eigen::Matrix3d Ry = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
|
||
Eigen::Matrix3d Rz = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
|
||
|
||
// 外旋 ABC：矩阵公式为 R = Rc · Rb · Ra（右乘，反向）
|
||
Eigen::Matrix3d R_eigen;
|
||
switch (order) {
|
||
case HECEulerOrder::XYZ: R_eigen = Rz * Ry * Rx; break; // 外旋 XYZ
|
||
case HECEulerOrder::XZY: R_eigen = Ry * Rz * Rx; break; // 外旋 XZY
|
||
case HECEulerOrder::YXZ: R_eigen = Rz * Rx * Ry; break; // 外旋 YXZ
|
||
case HECEulerOrder::YZX: R_eigen = Rx * Rz * Ry; break; // 外旋 YZX
|
||
case HECEulerOrder::ZXY: R_eigen = Ry * Rx * Rz; break; // 外旋 ZXY
|
||
case HECEulerOrder::ZYX: R_eigen = Rx * Ry * Rz; break; // 外旋 ZYX
|
||
}
|
||
|
||
// 转换回 HECRotationMatrix
|
||
for (int i = 0; i < 3; ++i)
|
||
for (int j = 0; j < 3; ++j)
|
||
R.at(i, j) = R_eigen(i, j);
|
||
}
|
||
|
||
bool HandEyeCalib::TransformPose(
|
||
const HECCalibResult& calibResult,
|
||
const HECPoint3D& eyeCenter,
|
||
const HECPoint3D& longAxisDir,
|
||
const HECPoint3D& normalDir,
|
||
int dirVectorInvert,
|
||
HECEulerOrder eulerOrder,
|
||
HECLongAxisDir longAxisMapping,
|
||
HECPoseResult& poseResult)
|
||
{
|
||
auto cross = [](const HECPoint3D& a, const HECPoint3D& b) {
|
||
return HECPoint3D(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
|
||
};
|
||
auto dot = [](const HECPoint3D& a, const HECPoint3D& b) {
|
||
return a.x*b.x + a.y*b.y + a.z*b.z;
|
||
};
|
||
|
||
HECPoint3D xAxis, yAxis, zAxis;
|
||
|
||
if (longAxisMapping == HECLongAxisDir::AxisY) {
|
||
// Long axis maps to Y: Y = longAxisDir, Z = normalDir, X = cross(Y, Z)
|
||
yAxis = longAxisDir.normalized();
|
||
zAxis = normalDir.normalized();
|
||
if (yAxis.norm() < 1e-6 || zAxis.norm() < 1e-6) return false;
|
||
|
||
zAxis = (zAxis - yAxis * dot(yAxis, zAxis)).normalized();
|
||
if (zAxis.norm() < 1e-6) return false;
|
||
|
||
xAxis = cross(yAxis, zAxis).normalized();
|
||
if (xAxis.norm() < 1e-6) return false;
|
||
|
||
zAxis = cross(xAxis, yAxis).normalized();
|
||
if (zAxis.norm() < 1e-6) return false;
|
||
} else {
|
||
// Long axis maps to X (default): X = longAxisDir, Z = normalDir, Y = cross(Z, X)
|
||
xAxis = longAxisDir.normalized();
|
||
zAxis = normalDir.normalized();
|
||
if (xAxis.norm() < 1e-6 || zAxis.norm() < 1e-6) return false;
|
||
|
||
zAxis = (zAxis - xAxis * dot(xAxis, zAxis)).normalized();
|
||
if (zAxis.norm() < 1e-6) return false;
|
||
|
||
yAxis = cross(zAxis, xAxis).normalized();
|
||
if (yAxis.norm() < 1e-6) return false;
|
||
|
||
zAxis = cross(xAxis, yAxis).normalized();
|
||
if (zAxis.norm() < 1e-6) return false;
|
||
}
|
||
|
||
// Compute eye-frame Euler angles before flip
|
||
HECRotationMatrix eyeR;
|
||
eyeR.at(0, 0) = xAxis.x; eyeR.at(0, 1) = yAxis.x; eyeR.at(0, 2) = zAxis.x;
|
||
eyeR.at(1, 0) = xAxis.y; eyeR.at(1, 1) = yAxis.y; eyeR.at(1, 2) = zAxis.y;
|
||
eyeR.at(2, 0) = xAxis.z; eyeR.at(2, 1) = yAxis.z; eyeR.at(2, 2) = zAxis.z;
|
||
HECEulerAngles eyeEuler;
|
||
RotationMatrixToEuler(eyeR, eulerOrder, eyeEuler);
|
||
double eyeRollDeg, eyePitchDeg, eyeYawDeg;
|
||
eyeEuler.toDegrees(eyeRollDeg, eyePitchDeg, eyeYawDeg);
|
||
LOG_INFO("[HandEyeCalib] Eye Frame (longAxisMapping=%d) Euler (deg): Roll=%.6f, Pitch=%.6f, Yaw=%.6f\n",
|
||
static_cast<int>(longAxisMapping), eyeRollDeg, eyePitchDeg, eyeYawDeg);
|
||
|
||
std::vector<HECPoint3D> dirVectors = { xAxis, yAxis, zAxis };
|
||
|
||
// Apply axis flip
|
||
switch (dirVectorInvert) {
|
||
case 1:
|
||
dirVectors[0] = dirVectors[0] * (-1.0);
|
||
dirVectors[1] = dirVectors[1] * (-1.0);
|
||
break;
|
||
case 2:
|
||
dirVectors[0] = dirVectors[0] * (-1.0);
|
||
dirVectors[2] = dirVectors[2] * (-1.0);
|
||
break;
|
||
case 3:
|
||
dirVectors[1] = dirVectors[1] * (-1.0);
|
||
dirVectors[2] = dirVectors[2] * (-1.0);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// Transform position
|
||
TransformPoint(calibResult.R, calibResult.T, eyeCenter, poseResult.position);
|
||
|
||
// Rotate direction vectors and build rotation matrix
|
||
std::vector<HECPoint3D> robotDirs(3);
|
||
for (int i = 0; i < 3; ++i) {
|
||
RotatePoint(calibResult.R, dirVectors[i], robotDirs[i]);
|
||
}
|
||
|
||
HECRotationMatrix R_pose;
|
||
R_pose.at(0, 0) = robotDirs[0].x; R_pose.at(0, 1) = robotDirs[1].x; R_pose.at(0, 2) = robotDirs[2].x;
|
||
R_pose.at(1, 0) = robotDirs[0].y; R_pose.at(1, 1) = robotDirs[1].y; R_pose.at(1, 2) = robotDirs[2].y;
|
||
R_pose.at(2, 0) = robotDirs[0].z; R_pose.at(2, 1) = robotDirs[1].z; R_pose.at(2, 2) = robotDirs[2].z;
|
||
|
||
// Decompose to Euler angles (radians), then convert to degrees
|
||
HECEulerAngles euler;
|
||
RotationMatrixToEuler(R_pose, eulerOrder, euler);
|
||
double rollDeg, pitchDeg, yawDeg;
|
||
euler.toDegrees(rollDeg, pitchDeg, yawDeg);
|
||
poseResult.angles = HECEulerAngles::fromDegrees(rollDeg, pitchDeg, yawDeg);
|
||
|
||
return true;
|
||
}
|
||
|
||
double HandEyeCalib::CalculateError(
|
||
const std::vector<HECPoint3D>& eyePoints,
|
||
const std::vector<HECPoint3D>& robotPoints,
|
||
const HECCalibResult& calibResult)
|
||
{
|
||
if (eyePoints.size() != robotPoints.size() || eyePoints.empty()) {
|
||
return -1.0;
|
||
}
|
||
|
||
double totalError = 0.0;
|
||
double maxErr = 0.0;
|
||
double minErr = std::numeric_limits<double>::max();
|
||
int N = static_cast<int>(eyePoints.size());
|
||
|
||
for (int i = 0; i < N; i++) {
|
||
HECPoint3D transformedPoint;
|
||
TransformPoint(calibResult.R, calibResult.T, eyePoints[i], transformedPoint);
|
||
|
||
HECPoint3D diff = transformedPoint - robotPoints[i];
|
||
double error = diff.norm();
|
||
totalError += error;
|
||
|
||
if (error > maxErr) maxErr = error;
|
||
if (error < minErr) minErr = error;
|
||
}
|
||
|
||
// 更新 calibResult 中的误差信息（需要强制转换为非 const）
|
||
HECCalibResult& result = const_cast<HECCalibResult&>(calibResult);
|
||
result.maxError = maxErr;
|
||
result.minError = minErr;
|
||
|
||
return totalError / N;
|
||
}
|
||
|
||
int HandEyeCalib::CalculateEyeInHand(
|
||
const std::vector<HECEyeInHandData>& calibData,
|
||
HECCalibResult& result)
|
||
{
|
||
int N = static_cast<int>(calibData.size());
|
||
if (N < 3) {
|
||
return ERR_CODE(APP_ERR_PARAM);
|
||
}
|
||
|
||
// ================================================================
|
||
// 预处理：提取末端位姿和相机观测点
|
||
// ================================================================
|
||
// 眼在手上：相机安装在机器人末端，标定靶固定
|
||
// 约束：T_end_i * X * P_cam_i = P_base（对所有 i，P_base 相同）
|
||
// 其中 X = [R_x | t_x] 是相机到末端的变换（待求）
|
||
|
||
std::vector<Eigen::Matrix3d> R_ends(N);
|
||
std::vector<Eigen::Vector3d> t_ends(N);
|
||
std::vector<Eigen::Vector3d> p_cams(N);
|
||
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||
for (int r = 0; r < 4; r++)
|
||
for (int c = 0; c < 4; c++)
|
||
T(r, c) = calibData[i].endPose.at(r, c);
|
||
R_ends[i] = T.block<3, 3>(0, 0);
|
||
t_ends[i] = T.block<3, 1>(0, 3);
|
||
p_cams[i] = Eigen::Vector3d(
|
||
calibData[i].targetInCamera.x,
|
||
calibData[i].targetInCamera.y,
|
||
calibData[i].targetInCamera.z);
|
||
}
|
||
|
||
int numPairs = N * (N - 1) / 2;
|
||
|
||
// ================================================================
|
||
// 第一阶段：线性初始化
|
||
// ================================================================
|
||
// 对两组 (i, j)：
|
||
// R_i*(R_x*p_i + t_x) + t_i = R_j*(R_x*p_j + t_x) + t_j
|
||
// 展开：
|
||
// (R_i-R_j)*t_x + R_i*M*p_i - R_j*M*p_j = t_j - t_i
|
||
// 其中 M = R_x，对 [t_x; vec(M)] 线性
|
||
|
||
Eigen::MatrixXd C(3 * numPairs, 12);
|
||
Eigen::VectorXd d_vec(3 * numPairs);
|
||
|
||
int row = 0;
|
||
for (int i = 0; i < N; i++) {
|
||
for (int j = i + 1; j < N; j++) {
|
||
// (R_i - R_j) * t_x
|
||
C.block<3, 3>(row, 0) = R_ends[i] - R_ends[j];
|
||
|
||
// d(R*M*p)_k / dM_{l,m} = R_{k,l} * p_m (row-major vec)
|
||
for (int k = 0; k < 3; k++) {
|
||
for (int l = 0; l < 3; l++) {
|
||
for (int m = 0; m < 3; m++) {
|
||
C(row + k, 3 + l * 3 + m) =
|
||
R_ends[i](k, l) * p_cams[i](m)
|
||
- R_ends[j](k, l) * p_cams[j](m);
|
||
}
|
||
}
|
||
}
|
||
|
||
d_vec.segment<3>(row) = t_ends[j] - t_ends[i];
|
||
row += 3;
|
||
}
|
||
}
|
||
|
||
Eigen::VectorXd x_lin = C.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d_vec);
|
||
|
||
// 提取平移
|
||
Eigen::Vector3d t_x = x_lin.head<3>();
|
||
|
||
// 提取 M 并投影到最近旋转矩阵
|
||
Eigen::Matrix3d M_mat;
|
||
for (int l = 0; l < 3; l++)
|
||
for (int m = 0; m < 3; m++)
|
||
M_mat(l, m) = x_lin(3 + l * 3 + m);
|
||
|
||
Eigen::JacobiSVD<Eigen::Matrix3d> svd_M(M_mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||
Eigen::Matrix3d R_x = svd_M.matrixU() * svd_M.matrixV().transpose();
|
||
if (R_x.determinant() < 0) {
|
||
Eigen::Matrix3d V = svd_M.matrixV();
|
||
V.col(2) *= -1;
|
||
R_x = svd_M.matrixU() * V.transpose();
|
||
}
|
||
|
||
// 已知旋转后重新线性求解平移
|
||
Eigen::MatrixXd C_t(3 * numPairs, 3);
|
||
Eigen::VectorXd d_t(3 * numPairs);
|
||
row = 0;
|
||
for (int i = 0; i < N; i++) {
|
||
for (int j = i + 1; j < N; j++) {
|
||
C_t.block<3, 3>(row, 0) = R_ends[i] - R_ends[j];
|
||
d_t.segment<3>(row) = t_ends[j] - t_ends[i]
|
||
- R_ends[i] * R_x * p_cams[i]
|
||
+ R_ends[j] * R_x * p_cams[j];
|
||
row += 3;
|
||
}
|
||
}
|
||
t_x = C_t.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d_t);
|
||
|
||
// ================================================================
|
||
// 第二阶段：Levenberg-Marquardt 非线性优化
|
||
// ================================================================
|
||
// 参数化：角轴 w(3) + 平移 t(3) = 6 参数
|
||
// 残差：r_ij = T_end_i * X * p_i - T_end_j * X * p_j
|
||
// 最小化 sum ||r_ij||^2
|
||
|
||
auto skew = [](const Eigen::Vector3d& v) -> Eigen::Matrix3d {
|
||
Eigen::Matrix3d S;
|
||
S << 0, -v(2), v(1),
|
||
v(2), 0, -v(0),
|
||
-v(1), v(0), 0;
|
||
return S;
|
||
};
|
||
|
||
auto paramsToRot = [](const Eigen::Vector3d& w) -> Eigen::Matrix3d {
|
||
double theta = w.norm();
|
||
if (theta < 1e-10) return Eigen::Matrix3d::Identity();
|
||
return Eigen::AngleAxisd(theta, w / theta).toRotationMatrix();
|
||
};
|
||
|
||
auto computeCost = [&](const Eigen::Matrix3d& R, const Eigen::Vector3d& t) -> double {
|
||
double cost = 0;
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Vector3d base_i = R_ends[i] * (R * p_cams[i] + t) + t_ends[i];
|
||
for (int j = i + 1; j < N; j++) {
|
||
Eigen::Vector3d base_j = R_ends[j] * (R * p_cams[j] + t) + t_ends[j];
|
||
cost += (base_i - base_j).squaredNorm();
|
||
}
|
||
}
|
||
return 0.5 * cost;
|
||
};
|
||
|
||
// 初始化参数
|
||
Eigen::AngleAxisd aa_init(R_x);
|
||
Eigen::VectorXd params(6);
|
||
if (aa_init.angle() > 1e-10) {
|
||
params.head<3>() = aa_init.angle() * aa_init.axis();
|
||
} else {
|
||
params.head<3>().setZero();
|
||
}
|
||
params.tail<3>() = t_x;
|
||
|
||
double lambda = 1e-3;
|
||
|
||
for (int iter = 0; iter < 100; iter++) {
|
||
Eigen::Matrix3d R_curr = paramsToRot(params.head<3>());
|
||
Eigen::Vector3d t_curr = params.tail<3>();
|
||
|
||
// 残差和 Jacobian
|
||
Eigen::VectorXd residual(3 * numPairs);
|
||
Eigen::MatrixXd J(3 * numPairs, 6);
|
||
|
||
row = 0;
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Vector3d Rp_i = R_curr * p_cams[i];
|
||
Eigen::Vector3d base_i = R_ends[i] * (Rp_i + t_curr) + t_ends[i];
|
||
|
||
for (int j = i + 1; j < N; j++) {
|
||
Eigen::Vector3d Rp_j = R_curr * p_cams[j];
|
||
Eigen::Vector3d base_j = R_ends[j] * (Rp_j + t_curr) + t_ends[j];
|
||
|
||
residual.segment<3>(row) = base_i - base_j;
|
||
|
||
// dr/dw = -R_i * skew(Rp_i) + R_j * skew(Rp_j)
|
||
J.block<3, 3>(row, 0) = -R_ends[i] * skew(Rp_i) + R_ends[j] * skew(Rp_j);
|
||
// dr/dt = R_i - R_j
|
||
J.block<3, 3>(row, 3) = R_ends[i] - R_ends[j];
|
||
|
||
row += 3;
|
||
}
|
||
}
|
||
|
||
double cost = 0.5 * residual.squaredNorm();
|
||
Eigen::MatrixXd JtJ = J.transpose() * J;
|
||
Eigen::VectorXd Jtr = J.transpose() * residual;
|
||
Eigen::VectorXd prev_params = params;
|
||
|
||
// LM 自适应阻尼更新
|
||
bool accepted = false;
|
||
for (int lm = 0; lm < 8; lm++) {
|
||
Eigen::MatrixXd damped = JtJ;
|
||
damped.diagonal() += lambda * JtJ.diagonal();
|
||
Eigen::VectorXd delta = damped.ldlt().solve(Jtr);
|
||
Eigen::VectorXd trial = prev_params - delta;
|
||
|
||
Eigen::Matrix3d R_trial = paramsToRot(trial.head<3>());
|
||
double newCost = computeCost(R_trial, trial.tail<3>());
|
||
|
||
if (newCost < cost) {
|
||
params = trial;
|
||
lambda = std::max(lambda * 0.1, 1e-12);
|
||
accepted = true;
|
||
break;
|
||
} else {
|
||
lambda *= 10;
|
||
}
|
||
}
|
||
|
||
if (!accepted) break;
|
||
if ((params - prev_params).norm() < 1e-10) break;
|
||
}
|
||
|
||
// 提取最终结果
|
||
Eigen::Matrix3d R_final = paramsToRot(params.head<3>());
|
||
Eigen::Vector3d t_final = params.tail<3>();
|
||
|
||
for (int i = 0; i < 3; i++)
|
||
for (int j = 0; j < 3; j++)
|
||
result.R.at(i, j) = R_final(i, j);
|
||
result.T = HECTranslationVector(t_final(0), t_final(1), t_final(2));
|
||
|
||
// ================================================================
|
||
// 误差计算：各观测映射到基座坐标系的离散程度
|
||
// ================================================================
|
||
Eigen::Matrix4d T_result = Eigen::Matrix4d::Identity();
|
||
T_result.block<3, 3>(0, 0) = R_final;
|
||
T_result.block<3, 1>(0, 3) = t_final;
|
||
|
||
std::vector<Eigen::Vector3d> basePoints(N);
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Vector4d ph(p_cams[i](0), p_cams[i](1), p_cams[i](2), 1.0);
|
||
Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
|
||
for (int r = 0; r < 4; r++)
|
||
for (int c = 0; c < 4; c++)
|
||
T_end(r, c) = calibData[i].endPose.at(r, c);
|
||
basePoints[i] = (T_end * T_result * ph).head<3>();
|
||
}
|
||
|
||
Eigen::Vector3d meanBase = Eigen::Vector3d::Zero();
|
||
for (const auto& p : basePoints) meanBase += p;
|
||
meanBase /= N;
|
||
|
||
double totalError = 0, maxErr = 0, minErr = std::numeric_limits<double>::max();
|
||
for (const auto& p : basePoints) {
|
||
double err = (p - meanBase).norm();
|
||
totalError += err;
|
||
if (err > maxErr) maxErr = err;
|
||
if (err < minErr) minErr = err;
|
||
}
|
||
result.error = totalError / N;
|
||
result.maxError = maxErr;
|
||
result.minError = minErr;
|
||
result.centerEye = HECPoint3D(0, 0, 0);
|
||
result.centerRobot = HECPoint3D(meanBase(0), meanBase(1), meanBase(2));
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
Eigen::Matrix3d HandEyeCalib::eulerToRotationMatrix(double rx, double ry, double rz, HECEulerOrder order)
|
||
{
|
||
// 将角度转为弧度
|
||
const double deg2rad = M_PI / 180.0;
|
||
double roll = rx * deg2rad;
|
||
double pitch = ry * deg2rad;
|
||
double yaw = rz * deg2rad;
|
||
|
||
// 复用现有的 EulerToRotationMatrix 逻辑
|
||
HECEulerAngles angles(roll, pitch, yaw);
|
||
HECRotationMatrix R;
|
||
EulerToRotationMatrix(angles, order, R);
|
||
|
||
// 转换为 Eigen 矩阵
|
||
Eigen::Matrix3d result;
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
result(i, j) = R.at(i, j);
|
||
}
|
||
}
|
||
return result;
|
||
}
|
||
|
||
HECTCPCalibResult HandEyeCalib::CalculateTCP(const HECTCPCalibData& data)
|
||
{
|
||
HECTCPCalibResult result;
|
||
|
||
// 参数校验
|
||
int N = static_cast<int>(data.poses.size());
|
||
if (N < 3) {
|
||
result.success = false;
|
||
result.errorMessage = "至少需要3组法兰位姿数据";
|
||
return result;
|
||
}
|
||
|
||
// 构建每组位姿的旋转矩阵和位置向量
|
||
std::vector<Eigen::Matrix3d> rotations(N);
|
||
std::vector<Eigen::Vector3d> positions(N);
|
||
|
||
for (int i = 0; i < N; ++i) {
|
||
const auto& pose = data.poses[i];
|
||
rotations[i] = eulerToRotationMatrix(pose.rx, pose.ry, pose.rz, pose.eulerOrder);
|
||
positions[i] = Eigen::Vector3d(pose.x, pose.y, pose.z);
|
||
}
|
||
|
||
// === 3-DOF 位置求解 ===
|
||
// 约束：R_i * t_tcp + p_i = P_tcp（常数）
|
||
// 对相邻位姿对：(R_i - R_j) * t_tcp = p_j - p_i
|
||
int numPairs = N - 1;
|
||
Eigen::MatrixXd A(3 * numPairs, 3);
|
||
Eigen::VectorXd b(3 * numPairs);
|
||
|
||
for (int i = 0; i < numPairs; ++i) {
|
||
Eigen::Matrix3d dR = rotations[i] - rotations[i + 1];
|
||
Eigen::Vector3d dp = positions[i + 1] - positions[i];
|
||
|
||
A.block<3, 3>(i * 3, 0) = dR;
|
||
b.segment<3>(i * 3) = dp;
|
||
}
|
||
|
||
// SVD 最小二乘求解
|
||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
|
||
|
||
// 检查奇异值，判断退化情况
|
||
Eigen::VectorXd singularValues = svd.singularValues();
|
||
if (singularValues.size() < 3 || singularValues(2) < 1e-6) {
|
||
result.success = false;
|
||
result.errorMessage = "位姿数据退化，无法求解（可能所有位姿过于相似）";
|
||
return result;
|
||
}
|
||
|
||
Eigen::Vector3d t_tcp = svd.solve(b);
|
||
|
||
result.tx = t_tcp(0);
|
||
result.ty = t_tcp(1);
|
||
result.tz = t_tcp(2);
|
||
|
||
// 计算残差：所有位姿计算 R_i * t_tcp + p_i，应趋近同一点
|
||
Eigen::Vector3d meanTCPPoint = Eigen::Vector3d::Zero();
|
||
for (int i = 0; i < N; ++i) {
|
||
meanTCPPoint += rotations[i] * t_tcp + positions[i];
|
||
}
|
||
meanTCPPoint /= N;
|
||
|
||
double totalResidual = 0.0;
|
||
for (int i = 0; i < N; ++i) {
|
||
Eigen::Vector3d tcpPoint = rotations[i] * t_tcp + positions[i];
|
||
totalResidual += (tcpPoint - meanTCPPoint).squaredNorm();
|
||
}
|
||
result.residualError = std::sqrt(totalResidual / N);
|
||
|
||
// === 6-DOF 姿态求解 ===
|
||
if (data.mode == HECTCPCalibMode::Full6DOF) {
|
||
int refIdx = data.referencePoseIndex;
|
||
if (refIdx < 0 || refIdx >= N) {
|
||
result.success = false;
|
||
result.errorMessage = "参考位姿索引越界";
|
||
return result;
|
||
}
|
||
|
||
// 参考位姿的法兰旋转矩阵
|
||
Eigen::Matrix3d R_ref = rotations[refIdx];
|
||
|
||
// 期望的世界坐标系朝向旋转矩阵
|
||
Eigen::Matrix3d R_world = eulerToRotationMatrix(
|
||
data.worldRx, data.worldRy, data.worldRz, data.worldEulerOrder);
|
||
|
||
// TCP 相对于法兰的旋转：R_tcp = R_ref^T * R_world
|
||
Eigen::Matrix3d R_tcp = R_ref.transpose() * R_world;
|
||
|
||
// 将 R_tcp 转换为欧拉角（度）输出
|
||
HECRotationMatrix R_out;
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
R_out.at(i, j) = R_tcp(i, j);
|
||
}
|
||
}
|
||
|
||
HECEulerAngles tcpAngles;
|
||
RotationMatrixToEuler(R_out, data.worldEulerOrder, tcpAngles);
|
||
|
||
double rxDeg, ryDeg, rzDeg;
|
||
tcpAngles.toDegrees(rxDeg, ryDeg, rzDeg);
|
||
result.rx = rxDeg;
|
||
result.ry = ryDeg;
|
||
result.rz = rzDeg;
|
||
}
|
||
|
||
result.success = true;
|
||
return result;
|
||
}
|
||
|
||
int HandEyeCalib::CalculateEyeToHandWithPose(
|
||
const std::vector<HECEyeToHandData>& calibData,
|
||
HECCalibResult& result)
|
||
{
|
||
int N = static_cast<int>(calibData.size());
|
||
if (N < 3) {
|
||
return ERR_CODE(APP_ERR_PARAM);
|
||
}
|
||
|
||
// Eye-To-Hand 标定（Park 方法）
|
||
// 相机固定，标定板在末端移动
|
||
//
|
||
// 对于每个位姿 i：
|
||
// T_board_in_cam_i = T_cam_to_base^{-1} * T_base_to_end_i * T_end_to_board
|
||
//
|
||
// 其中 T_end_to_board 是标定板相对末端的固定变换（未知）
|
||
//
|
||
// 对于两个位姿 i 和 j：
|
||
// T_board_i = T_cam_to_base^{-1} * T_end_i * T_end_to_board
|
||
// T_board_j = T_cam_to_base^{-1} * T_end_j * T_end_to_board
|
||
//
|
||
// 消去 T_end_to_board：
|
||
// T_board_i * T_board_j^{-1} = T_cam_to_base^{-1} * T_end_i * T_end_j^{-1} * T_cam_to_base
|
||
//
|
||
// 即 XA = BX 形式（注意顺序！）：
|
||
// A = T_end_i * T_end_j^{-1} (末端位姿的相对变换)
|
||
// B = T_board_i * T_board_j^{-1} (标定板在相机坐标系的相对变换)
|
||
// X = T_cam_to_base^{-1} (基座到相机的变换)
|
||
//
|
||
// 转换为 AX = XB 形式：X^{-1}A = BX^{-1}
|
||
// 令 Y = X^{-1} = T_cam_to_base，则：YA = BY
|
||
// 即 A^{-1}Y = YB^{-1}，或 AY = YB (取逆)
|
||
|
||
// 构建相对变换（使用所有唯一位姿对，提供更多约束）
|
||
std::vector<Eigen::Matrix3d> A_rot, B_rot;
|
||
std::vector<Eigen::Vector3d> A_trans, B_trans;
|
||
|
||
for (int i = 0; i < N; i++) {
|
||
for (int j = i + 1; j < N; j++) {
|
||
// 获取末端位姿 T_end_i 和 T_end_j
|
||
Eigen::Matrix4d T_end_i = Eigen::Matrix4d::Identity();
|
||
Eigen::Matrix4d T_end_j = Eigen::Matrix4d::Identity();
|
||
|
||
for (int r = 0; r < 4; r++) {
|
||
for (int c = 0; c < 4; c++) {
|
||
T_end_i(r, c) = calibData[i].endPose.at(r, c);
|
||
T_end_j(r, c) = calibData[j].endPose.at(r, c);
|
||
}
|
||
}
|
||
|
||
// A = T_end_i * T_end_j^{-1}
|
||
Eigen::Matrix4d A = T_end_i * T_end_j.inverse();
|
||
|
||
// 获取标定板在相机坐标系的位姿
|
||
Eigen::Matrix4d T_board_i = Eigen::Matrix4d::Identity();
|
||
Eigen::Matrix4d T_board_j = Eigen::Matrix4d::Identity();
|
||
|
||
for (int r = 0; r < 4; r++) {
|
||
for (int c = 0; c < 4; c++) {
|
||
T_board_i(r, c) = calibData[i].boardInCamera.at(r, c);
|
||
T_board_j(r, c) = calibData[j].boardInCamera.at(r, c);
|
||
}
|
||
}
|
||
|
||
// B = T_board_i * T_board_j^{-1}
|
||
Eigen::Matrix4d B = T_board_i * T_board_j.inverse();
|
||
|
||
A_rot.push_back(A.block<3, 3>(0, 0));
|
||
A_trans.push_back(A.block<3, 1>(0, 3));
|
||
B_rot.push_back(B.block<3, 3>(0, 0));
|
||
B_trans.push_back(B.block<3, 1>(0, 3));
|
||
}
|
||
}
|
||
|
||
// 使用 Tsai-Lenz 方法求解旋转矩阵
|
||
// AY = YB 的旋转部分：R_A * R_Y = R_Y * R_B
|
||
// 展开为 Kronecker 积：(I ⊗ R_A - R_B^T ⊗ I) * vec(R_Y) = 0
|
||
int numPairs = static_cast<int>(A_rot.size());
|
||
Eigen::MatrixXd M(9 * numPairs, 9);
|
||
M.setZero();
|
||
|
||
for (int i = 0; i < numPairs; i++) {
|
||
Eigen::Matrix3d Ai = A_rot[i];
|
||
Eigen::Matrix3d Bi = B_rot[i];
|
||
|
||
for (int r = 0; r < 3; r++) {
|
||
for (int c = 0; c < 3; c++) {
|
||
int row = i * 9 + r * 3 + c;
|
||
for (int k = 0; k < 3; k++) {
|
||
M(row, k * 3 + c) += Ai(r, k); // R_A * R_Y 项
|
||
M(row, r * 3 + k) -= Bi(k, c); // R_Y * R_B 项
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// SVD 求解
|
||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullV);
|
||
Eigen::VectorXd x = svd.matrixV().col(8);
|
||
|
||
// 重构旋转矩阵
|
||
Eigen::Matrix3d R_raw;
|
||
R_raw << x(0), x(1), x(2),
|
||
x(3), x(4), x(5),
|
||
x(6), x(7), x(8);
|
||
|
||
// 正交化旋转矩阵
|
||
Eigen::JacobiSVD<Eigen::Matrix3d> svd_R(R_raw, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||
Eigen::Matrix3d R_cam = svd_R.matrixU() * svd_R.matrixV().transpose();
|
||
|
||
// 确保行列式为1
|
||
if (R_cam.determinant() < 0) {
|
||
R_cam = -R_cam;
|
||
}
|
||
|
||
// 求解平移向量
|
||
// (R_A - I) * t_Y = R_Y * t_B - t_A
|
||
Eigen::MatrixXd C(3 * numPairs, 3);
|
||
Eigen::VectorXd d(3 * numPairs);
|
||
|
||
for (int i = 0; i < numPairs; i++) {
|
||
C.block<3, 3>(i * 3, 0) = A_rot[i] - Eigen::Matrix3d::Identity();
|
||
d.segment<3>(i * 3) = R_cam * B_trans[i] - A_trans[i];
|
||
}
|
||
|
||
// 最小二乘求解
|
||
Eigen::Vector3d t_cam = C.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d);
|
||
|
||
// 输出结果
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
result.R.at(i, j) = R_cam(i, j);
|
||
}
|
||
}
|
||
result.T = HECTranslationVector(t_cam(0), t_cam(1), t_cam(2));
|
||
|
||
// 计算误差
|
||
double totalError = 0.0;
|
||
double maxErr = 0.0;
|
||
double minErr = std::numeric_limits<double>::max();
|
||
Eigen::Matrix4d T_result = Eigen::Matrix4d::Identity();
|
||
T_result.block<3, 3>(0, 0) = R_cam;
|
||
T_result.block<3, 1>(0, 3) = t_cam;
|
||
|
||
// 计算标定板相对末端的位置（应该一致）
|
||
std::vector<Eigen::Vector3d> boardInEndPoints;
|
||
for (int i = 0; i < N; i++) {
|
||
Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
|
||
Eigen::Matrix4d T_board = Eigen::Matrix4d::Identity();
|
||
|
||
for (int r = 0; r < 4; r++) {
|
||
for (int c = 0; c < 4; c++) {
|
||
T_end(r, c) = calibData[i].endPose.at(r, c);
|
||
T_board(r, c) = calibData[i].boardInCamera.at(r, c);
|
||
}
|
||
}
|
||
|
||
// T_board_in_end = T_end^{-1} * T_cam_to_base * T_board_in_cam
|
||
Eigen::Matrix4d T_board_in_end = T_end.inverse() * T_result * T_board;
|
||
boardInEndPoints.push_back(T_board_in_end.block<3, 1>(0, 3));
|
||
}
|
||
|
||
// 计算标定板在末端坐标系下位置的离散程度作为误差
|
||
Eigen::Vector3d meanBoardInEnd = Eigen::Vector3d::Zero();
|
||
for (const auto& p : boardInEndPoints) {
|
||
meanBoardInEnd += p;
|
||
}
|
||
meanBoardInEnd /= N;
|
||
|
||
for (const auto& p : boardInEndPoints) {
|
||
double error = (p - meanBoardInEnd).norm();
|
||
totalError += error;
|
||
if (error > maxErr) maxErr = error;
|
||
if (error < minErr) minErr = error;
|
||
}
|
||
result.error = totalError / N;
|
||
result.maxError = maxErr;
|
||
result.minError = minErr;
|
||
|
||
result.centerEye = HECPoint3D(0, 0, 0);
|
||
result.centerRobot = HECPoint3D(meanBoardInEnd(0), meanBoardInEnd(1), meanBoardInEnd(2));
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 导出函数实现
|
||
IHandEyeCalib* CreateHandEyeCalibInstance()
|
||
{
|
||
return new HandEyeCalib();
|
||
}
|
||
|
||
void DestroyHandEyeCalibInstance(IHandEyeCalib* instance)
|
||
{
|
||
delete instance;
|
||
}
|